def createProcesses(self, highPriorityProcesses, medPriorityProcesses):
# Set up the state machine
self.stateMachine = StateMachine("MotorsOff")
self.stateMachine.addState("MotorsOff", self.ControlOff,
self.MotorsOffState, self.ControlOn)
self.stateMachine.addState("SimpleManual", None,
self.simpleManualControl, None)
self.stateMachine.addState("MPUAssistedManual", None,
self.mpuAssistedControl, None)
self.stateMachine.addState("90Left", self.turn90DegLeft,
self.fastTurnState, None)
self.stateMachine.addState("90Right", self.turn90DegRight,
self.fastTurnState, None)
# Yaw control
yaw = self.sensors.yaw()
self.pidHeading = PID(
self.pidP,
self.pidI,
self.pidD,
sample_time=0.008,
proportional_on_measurement=self.proportionalOnMeasure,
output_limits=(-1.0, 1.0))
self.headingError = HeadingPIDErrorValue(yaw, self.pidHeading,
yaw.getValue())
# Motors
motorsStop = FixedValue(0.0)
self.motorEnable = self.sensors.button(4)
self.autoModeForwardSpeed = FixedValue(0.0)
self.autoModeEnable = ToggleButtonValue(self.sensors.button(5))
self.joystickForward = self.sensors.joystickAxis(1)
self.joystickLeftRight = self.sensors.joystickAxis(3)
self.left90 = OneShotButtonValue(self.sensors.button(3))
self.right90 = OneShotButtonValue(self.sensors.button(1))
self.motorLSimpleManualSpeed = [
SpeedDirectionCombiner(
Scaler(self.joystickForward, scaling=self.maxForward),
Scaler(self.joystickLeftRight, scaling=self.maxSimpleTurn))
]
self.motorRSimpleManualSpeed = [
SpeedDirectionCombiner(
Scaler(self.joystickForward, scaling=self.maxForward),
Scaler(self.joystickLeftRight, scaling=-self.maxSimpleTurn))
]
self.motorLMPUAssistedSpeed = [
SpeedDirectionCombiner(
Scaler(self.joystickForward, scaling=self.maxForward),
Scaler(self.headingError, scaling=-self.maxHeadingTurn))
]
self.motorRMPUAssistedSpeed = [
SpeedDirectionCombiner(
Scaler(self.joystickForward, scaling=self.maxForward),
Scaler(self.headingError, scaling=self.maxHeadingTurn))
]
self.motorsSpeedMode = ValueAdder(
[self.motorEnable, self.autoModeEnable],
max=2) # 0=off, 1=manual/auto manual forward, 2=full auto
# Switch motor speed calculation depending upone what buttons are pressed
motorL = SwitchingControlMediator([
motorsStop, self.motorLSimpleManualSpeed,
self.motorLMPUAssistedSpeed
], self.controls.motor(2), self.motorsSpeedMode)
highPriorityProcesses.append(motorL)
motorR = SwitchingControlMediator([
motorsStop, self.motorRSimpleManualSpeed,
self.motorRMPUAssistedSpeed
], self.controls.motor(1), self.motorsSpeedMode)
highPriorityProcesses.append(motorR)
#self.speedSensorL = self.sensors.counter(0)
#self.speedSensorR = self.sensors.counter(1)
# LED display state
self.ledIndicator = self.controls.led(0)
medPriorityProcesses.append(
SimpleControlMediator(
Scaler(self.motorEnable, scaling=2, offset=2, max=4),
self.ledIndicator))
# LED eyes
self.ledEyeLeft = self.controls.led(20)
self.ledEyeRight = self.controls.led(21)
medPriorityProcesses.append(
SimpleControlMediator(Scaler(self.motorEnable, scaling=255),
self.ledEyeLeft))
medPriorityProcesses.append(
SimpleControlMediator(Scaler(self.autoModeEnable, scaling=255),
self.ledEyeRight))
# Common controls
self.grabberControl.createProcesses(highPriorityProcesses,
medPriorityProcesses)
self.cameraLevellingControl.createProcesses(highPriorityProcesses,
medPriorityProcesses,
self.cameraTilt)
self.zGunControl.createProcesses(highPriorityProcesses,
medPriorityProcesses)
def run(self):
# Get initial state
self.sensors.process()
# Set initial state of servos and motors
self.controls.stopAllMotors()
# Yaw control
yaw = self.sensors.yaw()
pidHeading = PID(0.3, 0.003, 0.05, sample_time=0.05)
target_heading = HeadingPIDErrorValue(yaw,
pidHeading,
yaw.getValue(),
min=-0.2,
max=0.2,
scaling=0.04)
# Initialise the PID
target_heading.getValue()
# Common controls
self.highPriorityProcesses.append(self.controls)
# Camera angle servo
cameraTiltServo = self.controls.servo(6)
currentPitch = Scaler(self.sensors.pitch(), scaling=-0.015)
cameraUpDownButtons = ValueIntegrator(self.sensors.upDownButton(2, 0),
scaling=-0.01,
min=-0.85,
max=0.85,
offset=0.5)
cameraLeveller = SimpleControlMediator( Scaler([currentPitch, cameraUpDownButtons], min=-0.9, max=0.85 ), \
cameraTiltServo )
self.highPriorityProcesses.append(cameraLeveller)
# Grabber hand servo
grabberServo = self.controls.servo(5)
grabReleaseButtons = ValueIntegrator(self.sensors.upDownButton(1, 3),
min=-0.8,
max=0.6,
scaling=0.2)
grabber = SimpleControlMediator( grabReleaseButtons, \
grabberServo )
self.medPriorityProcesses.append(grabber)
# Motors
motorsStop = FixedValue(0.0)
motorEnable = self.sensors.button(4)
joystickForward = self.sensors.joystickAxis(1)
joystickLeftRight = self.sensors.joystickAxis(3)
# Speed PID controller
## LEFT
speedSensorL = self.sensors.rateCounter(0) # Raw speed
speedSensorScaledL = Scaler(
speedSensorL, scaling=0.0005) # Roughly +/-1000 => +/1.0 max speed
speedRequestL = ValueAdder([
Scaler(joystickForward, scaling=0.5),
Scaler(joystickLeftRight, scaling=-0.2),
Scaler(target_heading, scaling=-1.0)
])
#pidL = PID(2.0,0.0,0.05, sample_time=0.05, output_limits=(-1.0, 1.0), proportional_on_measurement = False)
pidL = PID(0.3,
0.3,
0.05,
sample_time=0.05,
output_limits=(-1.0, 1.0),
proportional_on_measurement=True)
torqueErrorL = SimplePIDErrorValue(
pidL, Scaler(speedSensorScaledL, scaling=-1.0))
#torqueL = ValueAdder([speedRequestL, torqueErrorL])
torqueL = speedRequestL
motorL = SwitchingControlMediator( [ motorsStop, # Choice 0 = Stopped \
# Choice 1 = Controlled
[torqueL] # Speed control via PID \
],
self.controls.motor(2), \
motorEnable )
self.highPriorityProcesses.append(motorL)
## RIGHT
speedSensorR = self.sensors.rateCounter(1) # Raw speed
speedSensorScaledR = Scaler(
speedSensorR,
scaling=-0.0005) # Roughly +/-1000 => +/1.0 max speed
speedRequestR = ValueAdder([
Scaler(joystickForward, scaling=0.5),
Scaler(joystickLeftRight, scaling=0.2),
Scaler(target_heading, scaling=1.0)
])
#pidL = PID(2.0,0.0,0.05, sample_time=0.05, output_limits=(-1.0, 1.0), proportional_on_measurement = False)
pidR = PID(0.3,
0.3,
0.05,
sample_time=0.05,
output_limits=(-1.0, 1.0),
proportional_on_measurement=True)
torqueErrorR = SimplePIDErrorValue(
pidR, Scaler(speedSensorScaledR, scaling=-1.0))
#torqueR = ValueAdder([speedRequestR, torqueErrorR], scaling=-1.0)
torqueR = ValueAdder([speedRequestR], scaling=-1.0)
motorR = SwitchingControlMediator( [ motorsStop, # Choice 0 = Stopped \
# Choice 1 = Controlled
[torqueR] # Speed control via PID \
],
self.controls.motor(1), \
motorEnable )
self.highPriorityProcesses.append(motorR)
# Led Status
ledIndicator = self.controls.led(0)
# Set initial state of servos and motors
self.processAll(self.highPriorityProcesses)
self.processAll(self.medPriorityProcesses)
# Loop until the user clicks the close button.
done = False
running = False
while not done:
self.counter += 1
# Get current sensor state
self.sensors.process()
#
# EVENT PROCESSING STEP
#
for event in pygame.event.get(): # User did something.
if event.type == pygame.QUIT: # If user clicked close.
done = True # Flag that we are done so we exit this loop.
running = (motorEnable.getValue() > 0)
if running:
ledIndicator.setValue(0x04)
if not pidHeading.auto_mode:
pidHeading.auto_mode = True
targetL = speedRequestL.getValue()
print(f"targetL: {targetL}")
torqueErrorL.setTarget(targetL)
if not pidL.auto_mode:
pidL.auto_mode = True
targetR = speedRequestR.getValue()
#print(f"targetR: {targetR}")
torqueErrorR.setTarget(targetR)
if not pidR.auto_mode:
pidR.auto_mode = True
else:
ledIndicator.setValue(0x02)
torqueErrorL.setTarget(0.0)
pidL.auto_mode = False
torqueErrorR.setTarget(0.0)
pidR.auto_mode = False
target_heading.setTarget(yaw.getValue())
pidHeading.auto_mode = False
# Update everything
self.processAll(self.highPriorityProcesses)
if self.counter % 10 == 0:
self.processAll(self.medPriorityProcesses)
#speedSensorValL = speedSensorL.getCounter()
rateOfChangeL = speedSensorL.getValue()
speedL = speedSensorScaledL.getValue()
print(f"scaled speedL: {speedL}, speed: {rateOfChangeL}")
pidLcomponents = pidL.components
print(f"pidL: {pidLcomponents}")
#speedSensorValR = speedSensorR.getCounter()
#rateOfChangeR = speedSensorR.getValue()
#print(f"speedSensorValR: {speedSensorValR}, speed: {rateOfChangeR}")
#if running:
# currentTorqueL = torqueL.getValue()
# torqueLprev.setValue(currentTorqueL)
# currentTorqueR = torqueR.getValue()
# torqueRprev.setValue(currentTorqueR)
#else:
# torqueLprev.setValue(0.0)
# torqueRprev.setValue(0.0)
pygame.time.wait(10) # mS
# Keep motors alive if sensors also alive
if self.sensors.checkWatchdog() > 0:
self.controls.resetWatchdog()
else:
motorEnable.setValue(0, status=0)
def createProcesses(self, highPriorityProcesses, medPriorityProcesses):
# Set up the state machine
self.stateMachine = StateMachine("MotorsOff")
self.stateMachine.addState("MotorsOff", self.ControlOff,
self.MotorsOffState, self.ControlOn)
self.stateMachine.addState("Manual", None, self.ManualControl, None)
self.stateMachine.addState("FindLight", self.StopForward,
self.FindLight, self.FindLightAchieved)
self.stateMachine.addState("MoveToLight", self.StartMoveToLight,
self.MoveToLight, self.StopForward)
self.stateMachine.addState("Standstill", self.StopToStandStill,
self.StandStill, None)
# Yaw control
yaw = self.sensors.yaw()
self.pidHeading = PID(
self.pidP,
self.pidI,
self.pidD,
sample_time=0.008,
proportional_on_measurement=self.proportionalOnMeasure,
output_limits=(-1.0, 1.0))
self.headingError = HeadingPIDErrorValue(yaw, self.pidHeading,
yaw.getValue())
# Vision
self.visionTargetHeading = self.vision.getImageResult()
# Motors
motorsStop = FixedValue(0.0)
self.motorEnable = self.sensors.button(4)
self.autoModeForwardSpeed = FixedValue(0.0)
self.autoModeEnable = ToggleButtonValue(self.sensors.button(5))
self.joystickForward = self.sensors.joystickAxis(1)
self.joystickLeftRight = self.sensors.joystickAxis(3)
self.motorLManualSpeed = [
SpeedDirectionCombiner(
Scaler(self.joystickForward, scaling=self.maxForward),
Scaler(self.headingError, scaling=-self.maxHeadingTurn))
]
self.motorRManualSpeed = [
SpeedDirectionCombiner(
Scaler(self.joystickForward, scaling=self.maxForward),
Scaler(self.headingError, scaling=self.maxHeadingTurn))
]
self.motorLAutoSpeed = [
SpeedDirectionCombiner(
self.autoModeForwardSpeed,
Scaler(self.headingError, scaling=-self.maxHeadingTurn))
]
self.motorRAutoSpeed = [
SpeedDirectionCombiner(
self.autoModeForwardSpeed,
Scaler(self.headingError, scaling=self.maxHeadingTurn))
]
self.motorsSpeedMode = ValueAdder(
[self.motorEnable, self.autoModeEnable],
max=2) # 0=off, 1=manual/auto manual forward, 2=full auto
# Switch motor speed calculation depending upone what buttons are pressed
motorL = SwitchingControlMediator(
[motorsStop, self.motorLManualSpeed, self.motorLAutoSpeed],
self.controls.motor(2), self.motorsSpeedMode)
highPriorityProcesses.append(motorL)
motorR = SwitchingControlMediator(
[motorsStop, self.motorRManualSpeed, self.motorRAutoSpeed],
self.controls.motor(1), self.motorsSpeedMode)
highPriorityProcesses.append(motorR)
#self.speedSensorL = self.sensors.counter(0)
#self.speedSensorR = self.sensors.counter(1)
# LED display state
self.ledIndicator = self.controls.led(0)
medPriorityProcesses.append(
SimpleControlMediator(
Scaler(self.motorEnable, scaling=2, offset=2, max=4),
self.ledIndicator))
# LED eyes
self.ledEyeLeft = self.controls.led(20)
self.ledEyeRight = self.controls.led(21)
#medPriorityProcesses.append(SimpleControlMediator( self.motorEnable, self.ledEyeLeft))
medPriorityProcesses.append(
SimpleControlMediator(
PeriodicWaveGenerator(self.motorEnable, 0.2, 0.1),
self.ledEyeLeft))
medPriorityProcesses.append(
SimpleControlMediator(self.autoModeEnable, self.ledEyeRight))
# Common controls
self.grabberControl.createProcesses(highPriorityProcesses,
medPriorityProcesses)
self.cameraLevellingControl.createProcesses(highPriorityProcesses,
medPriorityProcesses,
self.cameraTilt)
self.zGunControl.createProcesses(highPriorityProcesses,
medPriorityProcesses)
def createProcesses(self, highPriorityProcesses, medPriorityProcesses):
# Yaw control
yaw = self.sensors.yaw()
self.pidHeading = PID(
self.pidP,
self.pidI,
self.pidD,
sample_time=0.008,
proportional_on_measurement=self.proportionalOnMeasure)
self.headingError = HeadingPIDErrorValue(yaw,
self.pidHeading,
yaw.getValue(),
min=-1.0,
max=1.0,
scaling=1.0)
# Initialise the PID
self.headingError.getValue()
# Vision
self.visionTargetHeading = self.vision.getLineHeading()
# Motors
motorsStop = FixedValue(0.0)
motorConstant = FixedValue(self.constantSpeed)
self.motorEnable = self.sensors.button(4)
self.constantEnable = ToggleButtonValue(self.sensors.button(5))
#self.constantEnable = TimedTriggerValue(self.sensors.button(5), 1.0, retriggerable = True)
self.joystickForward = self.sensors.joystickAxis(1)
self.joystickLeftRight = self.sensors.joystickAxis(3)
motorL = SwitchingControlMediator( [ motorsStop, # Choice 0 = Stopped \
# Choice 1 = Controlled
#[ValueLambda(Scaler(self.joystickForward, scaling = 0.9)), ValueLambda(Scaler(self.joystickLeftRight, scaling = -0.5))] # Joystick \
#[ValueLambda([Scaler(self.joystickForward, scaling = self.maxForward), Scaler(self.joystickLeftRight, scaling = -self.maxManualTurn), Scaler(self.headingError, scaling = -self.maxHeadingTurn)])] # Joystick \
#[ValueLambda([Scaler(self.joystickForward, scaling = self.maxForward), Scaler(self.headingError, scaling = -self.maxHeadingTurn)])] # Joystick \
#[ValueLambda([Scaler(self.joystickForward, scaling = self.maxForward)]), Scaler(self.headingError, scaling = -self.maxHeadingTurn)] # Joystick \
[SpeedDirectionCombiner(Scaler(self.joystickForward, scaling = self.maxForward), Scaler(self.headingError, scaling = -self.maxHeadingTurn))], \
[SpeedDirectionCombiner(motorConstant, Scaler(self.headingError, scaling = -self.maxHeadingTurn))] \
],
self.controls.motor(2), \
ValueAdder([self.motorEnable,self.constantEnable], max=2) )
self.speedSensorL = self.sensors.counter(0)
highPriorityProcesses.append(motorL)
motorR = SwitchingControlMediator( [ motorsStop, # Choice 0 = Stopped \
# Choice 1 = Controlled
#[ValueLambda(Scaler(self.joystickForward, scaling = -0.9)), ValueLambda(Scaler(self.joystickLeftRight, scaling = -0.5))] # Joystick \
#[ValueLambda([Scaler(self.joystickForward, scaling = -self.maxForward), Scaler(self.joystickLeftRight, scaling = -self.maxManualTurn), Scaler(self.headingError, scaling = -self.maxHeadingTurn)])] # Joystick \
#[ValueLambda([Scaler(self.joystickForward, scaling = -self.maxForward), Scaler(self.headingError, scaling = -self.maxHeadingTurn)])] # Joystick \
#[ValueLambda([Scaler(self.joystickForward, scaling = -self.maxForward)]), Scaler(self.headingError, scaling = -self.maxHeadingTurn)] # Joystick \
[Scaler(SpeedDirectionCombiner(Scaler(self.joystickForward, scaling = self.maxForward), Scaler(self.headingError, scaling = self.maxHeadingTurn)), scaling = 1.0)], \
[SpeedDirectionCombiner(motorConstant, Scaler(self.headingError, scaling = self.maxHeadingTurn), scaling = 1.0)] \
],
self.controls.motor(1), \
ValueAdder([self.motorEnable,self.constantEnable], max=2) )
highPriorityProcesses.append(motorR)
# LED display state
self.ledIndicator = self.controls.led(0)
medPriorityProcesses.append(
SimpleControlMediator(
Scaler(self.motorEnable, scaling=2, offset=2, max=4),
self.ledIndicator))
# LED eyes
self.ledEyeLeft = self.controls.led(20)
self.ledEyeRight = self.controls.led(21)
medPriorityProcesses.append(
SimpleControlMediator(Scaler(self.motorEnable, scaling=255),
self.ledEyeLeft))
medPriorityProcesses.append(
SimpleControlMediator(Scaler(self.constantEnable, scaling=255),
self.ledEyeRight))
# Common controls
self.grabberControl.createProcesses(highPriorityProcesses,
medPriorityProcesses)
self.cameraLevellingControl.createProcesses(highPriorityProcesses,
medPriorityProcesses,
self.cameraTilt)
self.zGunControl.createProcesses(highPriorityProcesses,
medPriorityProcesses)