|
|
@@ -95,20 +95,20 @@ class OdometryBase(CBD):
|
|
|
# Create the Connections
|
|
|
self.addConnection("phiLdot", "B-OUZVyqj9DiQNmVnVL9-16", input_port_name='IN1')
|
|
|
self.addConnection("phiRdot", "B-OUZVyqj9DiQNmVnVL9-16", input_port_name='IN2')
|
|
|
- self.addConnection("half", "div2", output_port_name='OUT1', input_port_name='IN1')
|
|
|
- self.addConnection("B-OUZVyqj9DiQNmVnVL9-16", "div2", output_port_name='OUT1', input_port_name='IN2')
|
|
|
+ self.addConnection("half", "div2", input_port_name='IN1', output_port_name='OUT1')
|
|
|
+ self.addConnection("B-OUZVyqj9DiQNmVnVL9-16", "div2", input_port_name='IN2', output_port_name='OUT1')
|
|
|
self.addConnection("mulX", "ydot", output_port_name='OUT1')
|
|
|
- self.addConnection("div2", "mulX", output_port_name='OUT1', input_port_name='IN1')
|
|
|
- self.addConnection("div2", "mulY", output_port_name='OUT1', input_port_name='IN1')
|
|
|
+ self.addConnection("div2", "mulX", input_port_name='IN1', output_port_name='OUT1')
|
|
|
+ self.addConnection("div2", "mulY", input_port_name='IN1', output_port_name='OUT1')
|
|
|
self.addConnection("mulY", "xdot", output_port_name='OUT1')
|
|
|
self.addConnection("heading", "sin", input_port_name='IN1')
|
|
|
self.addConnection("heading", "cos", input_port_name='IN1')
|
|
|
- self.addConnection("mulSin", "mulX", output_port_name='OUT1', input_port_name='IN2')
|
|
|
- self.addConnection("sin", "mulSin", output_port_name='OUT1', input_port_name='IN1')
|
|
|
- self.addConnection("r", "mulSin", output_port_name='OUT1', input_port_name='IN2')
|
|
|
- self.addConnection("r", "mulCos", output_port_name='OUT1', input_port_name='IN1')
|
|
|
- self.addConnection("mulCos", "mulY", output_port_name='OUT1', input_port_name='IN2')
|
|
|
- self.addConnection("cos", "mulCos", output_port_name='OUT1', input_port_name='IN2')
|
|
|
+ self.addConnection("mulSin", "mulX", input_port_name='IN2', output_port_name='OUT1')
|
|
|
+ self.addConnection("sin", "mulSin", input_port_name='IN1', output_port_name='OUT1')
|
|
|
+ self.addConnection("r", "mulSin", input_port_name='IN2', output_port_name='OUT1')
|
|
|
+ self.addConnection("r", "mulCos", input_port_name='IN1', output_port_name='OUT1')
|
|
|
+ self.addConnection("mulCos", "mulY", input_port_name='IN2', output_port_name='OUT1')
|
|
|
+ self.addConnection("cos", "mulCos", input_port_name='IN2', output_port_name='OUT1')
|
|
|
|
|
|
|
|
|
class Odometry(CBD):
|
|
|
@@ -127,23 +127,23 @@ class Odometry(CBD):
|
|
|
self.addBlock(ConstantBlock("init_w", value=(initial[2])))
|
|
|
|
|
|
# Create the Connections
|
|
|
- self.addConnection("delta", "int_w", output_port_name='OUT1', input_port_name='delta_t')
|
|
|
- self.addConnection("delta", "int_y", output_port_name='OUT1', input_port_name='delta_t')
|
|
|
- self.addConnection("delta", "int_x", output_port_name='OUT1', input_port_name='delta_t')
|
|
|
+ self.addConnection("delta", "int_w", input_port_name='delta_t', output_port_name='OUT1')
|
|
|
+ self.addConnection("delta", "int_y", input_port_name='delta_t', output_port_name='OUT1')
|
|
|
+ self.addConnection("delta", "int_x", input_port_name='delta_t', output_port_name='OUT1')
|
|
|
self.addConnection("int_w", "heading", output_port_name='OUT1')
|
|
|
- self.addConnection("int_w", "base", output_port_name='OUT1', input_port_name='heading')
|
|
|
+ self.addConnection("int_w", "base", input_port_name='heading', output_port_name='OUT1')
|
|
|
self.addConnection("phiLdot", "base", input_port_name='phiLdot')
|
|
|
self.addConnection("phiLdot", "OCegTr4naEkMIVMNTQIZ-73", input_port_name='phiLdot')
|
|
|
self.addConnection("phiRdot", "base", input_port_name='phiRdot')
|
|
|
self.addConnection("phiRdot", "OCegTr4naEkMIVMNTQIZ-73", input_port_name='phiRdot')
|
|
|
- self.addConnection("base", "int_x", output_port_name='xdot', input_port_name='IN1')
|
|
|
- self.addConnection("base", "int_y", output_port_name='ydot', input_port_name='IN1')
|
|
|
+ self.addConnection("base", "int_x", input_port_name='IN1', output_port_name='xdot')
|
|
|
+ self.addConnection("base", "int_y", input_port_name='IN1', output_port_name='ydot')
|
|
|
self.addConnection("int_x", "x", output_port_name='OUT1')
|
|
|
self.addConnection("int_y", "y", output_port_name='OUT1')
|
|
|
- self.addConnection("OCegTr4naEkMIVMNTQIZ-73", "int_w", output_port_name='wdot', input_port_name='IN1')
|
|
|
- self.addConnection("init_w", "int_w", output_port_name='OUT1', input_port_name='IC')
|
|
|
- self.addConnection("init_y", "int_y", output_port_name='OUT1', input_port_name='IC')
|
|
|
- self.addConnection("init_x", "int_x", output_port_name='OUT1', input_port_name='IC')
|
|
|
+ self.addConnection("OCegTr4naEkMIVMNTQIZ-73", "int_w", input_port_name='IN1', output_port_name='wdot')
|
|
|
+ self.addConnection("init_w", "int_w", input_port_name='IC', output_port_name='OUT1')
|
|
|
+ self.addConnection("init_y", "int_y", input_port_name='IC', output_port_name='OUT1')
|
|
|
+ self.addConnection("init_x", "int_x", input_port_name='IC', output_port_name='OUT1')
|
|
|
|
|
|
|
|
|
class OdometryW(CBD):
|
|
|
@@ -161,11 +161,12 @@ class OdometryW(CBD):
|
|
|
# Create the Connections
|
|
|
self.addConnection("phiRdot", "OCegTr4naEkMIVMNTQIZ-39", input_port_name='IN1')
|
|
|
self.addConnection("phiLdot", "OCegTr4naEkMIVMNTQIZ-43", input_port_name='IN1')
|
|
|
- self.addConnection("OCegTr4naEkMIVMNTQIZ-39", "OCegTr4naEkMIVMNTQIZ-43", output_port_name='OUT1', input_port_name='IN2')
|
|
|
- self.addConnection("OCegTr4naEkMIVMNTQIZ-43", "multr", output_port_name='OUT1', input_port_name='IN1')
|
|
|
- self.addConnection("rneg", "multr", output_port_name='OUT1', input_port_name='IN2')
|
|
|
- self.addConnection("invd", "multd", output_port_name='OUT1', input_port_name='IN1')
|
|
|
- self.addConnection("multr", "multd", output_port_name='OUT1', input_port_name='IN2')
|
|
|
+ self.addConnection("OCegTr4naEkMIVMNTQIZ-39", "OCegTr4naEkMIVMNTQIZ-43", input_port_name='IN2',
|
|
|
+ output_port_name='OUT1')
|
|
|
+ self.addConnection("OCegTr4naEkMIVMNTQIZ-43", "multr", input_port_name='IN1', output_port_name='OUT1')
|
|
|
+ self.addConnection("rneg", "multr", input_port_name='IN2', output_port_name='OUT1')
|
|
|
+ self.addConnection("invd", "multd", input_port_name='IN1', output_port_name='OUT1')
|
|
|
+ self.addConnection("multr", "multd", input_port_name='IN2', output_port_name='OUT1')
|
|
|
self.addConnection("multd", "wdot", output_port_name='OUT1')
|
|
|
|
|
|
|
|
|
@@ -183,8 +184,8 @@ class ControllerPID(CBD):
|
|
|
|
|
|
# Create the Connections
|
|
|
self.addConnection("color", "sum", input_port_name='IN1')
|
|
|
- self.addConnection("threshold", "sum", output_port_name='OUT1', input_port_name='IN2')
|
|
|
- self.addConnection("sum", "PID", output_port_name='OUT1', input_port_name='IN1')
|
|
|
+ self.addConnection("threshold", "sum", input_port_name='IN2', output_port_name='OUT1')
|
|
|
+ self.addConnection("sum", "PID", input_port_name='IN1', output_port_name='OUT1')
|
|
|
self.addConnection("PID", "mult", input_port_name='IN1', output_port_name='OUT1')
|
|
|
self.addConnection("friction", "mult", input_port_name='IN2', output_port_name='OUT1')
|
|
|
self.addConnection("mult", "heading", output_port_name='OUT1')
|
|
|
@@ -205,9 +206,9 @@ class Controller(CBD):
|
|
|
# Create the Connections
|
|
|
self.addConnection("v", "velocity", output_port_name='OUT1')
|
|
|
self.addConnection("color", "sum", input_port_name='IN1')
|
|
|
- self.addConnection("threshold", "sum", output_port_name='OUT1', input_port_name='IN2')
|
|
|
- self.addConnection("sum", "w1qmmAkClArfh_rfX7cO-38", output_port_name='OUT1', input_port_name='IN2')
|
|
|
- self.addConnection("gamma", "w1qmmAkClArfh_rfX7cO-38", output_port_name='OUT1', input_port_name='IN1')
|
|
|
+ self.addConnection("threshold", "sum", input_port_name='IN2', output_port_name='OUT1')
|
|
|
+ self.addConnection("sum", "w1qmmAkClArfh_rfX7cO-38", input_port_name='IN2', output_port_name='OUT1')
|
|
|
+ self.addConnection("gamma", "w1qmmAkClArfh_rfX7cO-38", input_port_name='IN1', output_port_name='OUT1')
|
|
|
self.addConnection("w1qmmAkClArfh_rfX7cO-38", "heading", output_port_name='OUT1')
|
|
|
|
|
|
|
|
|
@@ -230,22 +231,22 @@ class PID(CBD):
|
|
|
self.addBlock(AdderBlock("sum2"))
|
|
|
|
|
|
# Create the Connections
|
|
|
- self.addConnection("Kp", "prod", output_port_name='OUT1', input_port_name='IN1')
|
|
|
+ self.addConnection("Kp", "prod", input_port_name='IN1', output_port_name='OUT1')
|
|
|
self.addConnection("IN1", "prod", input_port_name='IN2')
|
|
|
self.addConnection("IN1", "int", input_port_name='IN1')
|
|
|
self.addConnection("IN1", "deriv", input_port_name='IN1')
|
|
|
- self.addConnection("delta_t", "deriv", output_port_name='OUT1', input_port_name='delta_t')
|
|
|
- self.addConnection("delta_t", "int", output_port_name='OUT1', input_port_name='delta_t')
|
|
|
- self.addConnection("int", "imult", output_port_name='OUT1', input_port_name='IN2')
|
|
|
- self.addConnection("Ki", "imult", output_port_name='OUT1', input_port_name='IN1')
|
|
|
- self.addConnection("deriv", "dmult", output_port_name='OUT1', input_port_name='IN2')
|
|
|
- self.addConnection("Kd", "dmult", output_port_name='OUT1', input_port_name='IN1')
|
|
|
- self.addConnection("zero", "int", output_port_name='OUT1', input_port_name='IC')
|
|
|
- self.addConnection("zero", "deriv", output_port_name='OUT1', input_port_name='IC')
|
|
|
- self.addConnection("prod", "sum1", output_port_name='OUT1', input_port_name='IN1')
|
|
|
- self.addConnection("imult", "sum1", output_port_name='OUT1', input_port_name='IN2')
|
|
|
- self.addConnection("sum1", "sum2", output_port_name='OUT1', input_port_name='IN1')
|
|
|
- self.addConnection("dmult", "sum2", output_port_name='OUT1', input_port_name='IN2')
|
|
|
+ self.addConnection("delta_t", "deriv", input_port_name='delta_t', output_port_name='OUT1')
|
|
|
+ self.addConnection("delta_t", "int", input_port_name='delta_t', output_port_name='OUT1')
|
|
|
+ self.addConnection("int", "imult", input_port_name='IN2', output_port_name='OUT1')
|
|
|
+ self.addConnection("Ki", "imult", input_port_name='IN1', output_port_name='OUT1')
|
|
|
+ self.addConnection("deriv", "dmult", input_port_name='IN2', output_port_name='OUT1')
|
|
|
+ self.addConnection("Kd", "dmult", input_port_name='IN1', output_port_name='OUT1')
|
|
|
+ self.addConnection("zero", "int", input_port_name='IC', output_port_name='OUT1')
|
|
|
+ self.addConnection("zero", "deriv", input_port_name='IC', output_port_name='OUT1')
|
|
|
+ self.addConnection("prod", "sum1", input_port_name='IN1', output_port_name='OUT1')
|
|
|
+ self.addConnection("imult", "sum1", input_port_name='IN2', output_port_name='OUT1')
|
|
|
+ self.addConnection("sum1", "sum2", input_port_name='IN1', output_port_name='OUT1')
|
|
|
+ self.addConnection("dmult", "sum2", input_port_name='IN2', output_port_name='OUT1')
|
|
|
self.addConnection("sum2", "OUT1", output_port_name='OUT1')
|
|
|
|
|
|
|
|
|
@@ -269,20 +270,20 @@ class DifferentialDrive(CBD):
|
|
|
|
|
|
# Create the Connections
|
|
|
self.addConnection("steering", "steeringMult", input_port_name='IN1')
|
|
|
- self.addConnection("WheelAxis", "steeringMult", output_port_name='OUT1', input_port_name='IN2')
|
|
|
- self.addConnection("steeringMult", "halfSteeringMult", output_port_name='OUT1', input_port_name='IN1')
|
|
|
- self.addConnection("half", "halfSteeringMult", output_port_name='OUT1', input_port_name='IN2')
|
|
|
+ self.addConnection("WheelAxis", "steeringMult", input_port_name='IN2', output_port_name='OUT1')
|
|
|
+ self.addConnection("steeringMult", "halfSteeringMult", input_port_name='IN1', output_port_name='OUT1')
|
|
|
+ self.addConnection("half", "halfSteeringMult", input_port_name='IN2', output_port_name='OUT1')
|
|
|
self.addConnection("velocity", "leftSum", input_port_name='IN1')
|
|
|
self.addConnection("velocity", "rightSum", input_port_name='IN2')
|
|
|
- self.addConnection("halfSteeringMult", "rightSum", output_port_name='OUT1', input_port_name='IN1')
|
|
|
- self.addConnection("halfSteeringMult", "neg", output_port_name='OUT1', input_port_name='IN1')
|
|
|
- self.addConnection("neg", "leftSum", output_port_name='OUT1', input_port_name='IN2')
|
|
|
- self.addConnection("rightSum", "rightMult", output_port_name='OUT1', input_port_name='IN1')
|
|
|
- self.addConnection("leftSum", "leftMult", output_port_name='OUT1', input_port_name='IN2')
|
|
|
- self.addConnection("WheelRadius", "leftMult", output_port_name='OUT1', input_port_name='IN1')
|
|
|
- self.addConnection("WheelRadius", "rightMult", output_port_name='OUT1', input_port_name='IN2')
|
|
|
- self.addConnection("leftMult", "leftClamp", output_port_name='OUT1', input_port_name='IN1')
|
|
|
- self.addConnection("rightMult", "rightClamp", output_port_name='OUT1', input_port_name='IN1')
|
|
|
+ self.addConnection("halfSteeringMult", "rightSum", input_port_name='IN1', output_port_name='OUT1')
|
|
|
+ self.addConnection("halfSteeringMult", "neg", input_port_name='IN1', output_port_name='OUT1')
|
|
|
+ self.addConnection("neg", "leftSum", input_port_name='IN2', output_port_name='OUT1')
|
|
|
+ self.addConnection("rightSum", "rightMult", input_port_name='IN1', output_port_name='OUT1')
|
|
|
+ self.addConnection("leftSum", "leftMult", input_port_name='IN2', output_port_name='OUT1')
|
|
|
+ self.addConnection("WheelRadius", "leftMult", input_port_name='IN1', output_port_name='OUT1')
|
|
|
+ self.addConnection("WheelRadius", "rightMult", input_port_name='IN2', output_port_name='OUT1')
|
|
|
+ self.addConnection("leftMult", "leftClamp", input_port_name='IN1', output_port_name='OUT1')
|
|
|
+ self.addConnection("rightMult", "rightClamp", input_port_name='IN1', output_port_name='OUT1')
|
|
|
self.addConnection("rightClamp", "phiRdot", output_port_name='OUT1')
|
|
|
self.addConnection("leftClamp", "phiLdot", output_port_name='OUT1')
|
|
|
|
|
|
@@ -300,12 +301,12 @@ class AGV(CBD):
|
|
|
self.addBlock(TCPClientSender("TCP", '192.168.1.10', 8080))
|
|
|
|
|
|
# Create the Connections
|
|
|
- self.addConnection("Controller", "StaticPlant", output_port_name='heading', input_port_name='steering')
|
|
|
- self.addConnection("Controller", "StaticPlant", output_port_name='velocity', input_port_name='velocity')
|
|
|
- self.addConnection("colorSensor", "Controller", output_port_name='OUT1', input_port_name='color')
|
|
|
- self.addConnection("colorSensor", "TCP", output_port_name='OUT1', input_port_name='IN1')
|
|
|
- self.addConnection("StaticPlant", "RightMotor", output_port_name='phiRdot', input_port_name='IN1')
|
|
|
- self.addConnection("StaticPlant", "LeftMotor", output_port_name='phiLdot', input_port_name='IN1')
|
|
|
+ self.addConnection("Controller", "StaticPlant", input_port_name='steering', output_port_name='heading')
|
|
|
+ self.addConnection("Controller", "StaticPlant", input_port_name='velocity', output_port_name='velocity')
|
|
|
+ self.addConnection("colorSensor", "Controller", input_port_name='color', output_port_name='OUT1')
|
|
|
+ self.addConnection("colorSensor", "TCP", input_port_name='IN1', output_port_name='OUT1')
|
|
|
+ self.addConnection("StaticPlant", "RightMotor", input_port_name='IN1', output_port_name='phiRdot')
|
|
|
+ self.addConnection("StaticPlant", "LeftMotor", input_port_name='IN1', output_port_name='phiLdot')
|
|
|
|
|
|
|
|
|
from CBD.lib.io import WriteCSV, ReadCSV, Interpolation
|
|
|
@@ -337,25 +338,25 @@ class AGVVirtual(CBD):
|
|
|
# self.addBlock(SignalCollectorBlock("tcp_dur"))
|
|
|
|
|
|
# Create the Connections
|
|
|
- self.addConnection("Controller", "StaticPlant", output_port_name='heading', input_port_name='steering')
|
|
|
- self.addConnection("Controller", "StaticPlant", output_port_name='velocity', input_port_name='velocity')
|
|
|
- self.addConnection("environment", "Controller", output_port_name='color', input_port_name='color')
|
|
|
- self.addConnection("StaticPlant", "odo", output_port_name='phiLdot', input_port_name='phiLdot')
|
|
|
- self.addConnection("StaticPlant", "odo", output_port_name='phiRdot', input_port_name='phiRdot')
|
|
|
- self.addConnection("odo", "plot", output_port_name='x', input_port_name='X')
|
|
|
- self.addConnection("odo", "environment", output_port_name='x', input_port_name='x')
|
|
|
- self.addConnection("odo", "plot", output_port_name='y', input_port_name='Y')
|
|
|
- self.addConnection("odo", "environment", output_port_name='y', input_port_name='y')
|
|
|
- self.addConnection("environment", "int", output_port_name='offset', input_port_name='IN1')
|
|
|
- self.addConnection("zero", "int", output_port_name='OUT1', input_port_name='IC')
|
|
|
- self.addConnection("zero", "intH", output_port_name='OUT1', input_port_name='IC')
|
|
|
- self.addConnection("delta", "int", output_port_name='OUT1', input_port_name='delta_t')
|
|
|
- self.addConnection("delta", "intH", output_port_name='OUT1', input_port_name='delta_t')
|
|
|
- self.addConnection("odo", "intH", output_port_name='heading', input_port_name='IN1')
|
|
|
- self.addConnection("odo", "headingPlot", output_port_name='heading', input_port_name='IN1')
|
|
|
- self.addConnection("intH", "mult", output_port_name='OUT1', input_port_name='IN1')
|
|
|
- self.addConnection("int", "mult", output_port_name='OUT1', input_port_name='IN2')
|
|
|
- self.addConnection("mult", "errorPlot", output_port_name='OUT1', input_port_name='IN1')
|
|
|
+ self.addConnection("Controller", "StaticPlant", input_port_name='steering', output_port_name='heading')
|
|
|
+ self.addConnection("Controller", "StaticPlant", input_port_name='velocity', output_port_name='velocity')
|
|
|
+ self.addConnection("environment", "Controller", input_port_name='color', output_port_name='color')
|
|
|
+ self.addConnection("StaticPlant", "odo", input_port_name='phiLdot', output_port_name='phiLdot')
|
|
|
+ self.addConnection("StaticPlant", "odo", input_port_name='phiRdot', output_port_name='phiRdot')
|
|
|
+ self.addConnection("odo", "plot", input_port_name='X', output_port_name='x')
|
|
|
+ self.addConnection("odo", "environment", input_port_name='x', output_port_name='x')
|
|
|
+ self.addConnection("odo", "plot", input_port_name='Y', output_port_name='y')
|
|
|
+ self.addConnection("odo", "environment", input_port_name='y', output_port_name='y')
|
|
|
+ self.addConnection("environment", "int", input_port_name='IN1', output_port_name='offset')
|
|
|
+ self.addConnection("zero", "int", input_port_name='IC', output_port_name='OUT1')
|
|
|
+ self.addConnection("zero", "intH", input_port_name='IC', output_port_name='OUT1')
|
|
|
+ self.addConnection("delta", "int", input_port_name='delta_t', output_port_name='OUT1')
|
|
|
+ self.addConnection("delta", "intH", input_port_name='delta_t', output_port_name='OUT1')
|
|
|
+ self.addConnection("odo", "intH", input_port_name='IN1', output_port_name='heading')
|
|
|
+ self.addConnection("odo", "headingPlot", input_port_name='IN1', output_port_name='heading')
|
|
|
+ self.addConnection("intH", "mult", input_port_name='IN1', output_port_name='OUT1')
|
|
|
+ self.addConnection("int", "mult", input_port_name='IN2', output_port_name='OUT1')
|
|
|
+ self.addConnection("mult", "errorPlot", input_port_name='IN1', output_port_name='OUT1')
|
|
|
|
|
|
# self.addConnection("odo", "writer", output_port_name="heading", input_port_name="heading")
|
|
|
# self.addConnection("odo", "writer", output_port_name="x", input_port_name="x")
|