def createProcesses(self, highPriorityProcesses, medPriorityProcesses): # Zgun elevation zgunUpDownButtons = ValueIntegrator(self.sensors.upDownButton(13, 14), min = -1.0, max = 1.0, scaling = 0.002) zgunElevationServo = self.controls.servo(0) zgunElevation = SimpleControlMediator( zgunUpDownButtons, zgunElevationServo ) highPriorityProcesses.append(zgunElevation) # Fire button zgunTrigger = self.sensors.button(6) zgunFireMotor = self.controls.motor(0) zgunFire = SimpleControlMediator( zgunTrigger, zgunFireMotor ) medPriorityProcesses.append(zgunFire) # Motor/laser arm enable/disable motorArmButton = ToggleButtonValue(self.sensors.button(5)) zgunArmMotor = self.controls.motorMode(0) zgunArm = SimpleControlMediator( motorArmButton, zgunArmMotor ) highPriorityProcesses.append(zgunArm)
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 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)
class ChallengeHeadingRemoteControl(ChallengeInterface):
def __init__(self):
self.controls = ControlAccessFactory.getSingleton()
self.sensors = SensorAccessFactory.getSingleton()
self.vision = VisionAccessFactory.getSingleton()
# Common controls
self.grabberControl = GrabberControl()
self.cameraLevellingControl = CameraLevellingControl()
self.zGunControl = ZGunControl()
# Get config
config = Config()
self.pidP = config.get("heading.pid.p", 0.06)
self.pidI = config.get("heading.pid.i", 0.001)
self.pidD = config.get("heading.pid.d", 0.004)
self.proportionalOnMeasure = config.get("heading.pid.pom", False)
self.maxForward = config.get("heading.forward.max", 1.0)
self.maxManualTurn = config.get("heading.manualturn.max", -15.0)
self.maxHeadingTurn = config.get("heading.headingturn.max", 0.5)
self.constantSpeed = config.get("lava.speed", 0.7)
self.cameraTilt = config.get("lava.camera.tilt", 0.5)
config.save()
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)
def move(self):
if self.constantEnable.getValue() > 0:
if not self.pidHeading.auto_mode:
self.pidHeading.auto_mode = True
# Vision turns
self.headingError.setTarget(self.visionTargetHeading.getValue())
print(self.pidHeading.components)
elif self.motorEnable.getValue() > 0:
if not self.pidHeading.auto_mode:
self.pidHeading.auto_mode = True
# Manual turns
if self.joystickLeftRight.getValue() != 0.0:
self.headingError.setTarget(self.sensors.yaw().getValue() +
self.joystickLeftRight.getValue() *
self.maxManualTurn)
print(self.pidHeading.components)
else:
# No change in position
self.pidHeading.auto_mode = False
self.headingError.setTarget(self.sensors.yaw().getValue())
def stop(self):
''' Stop the challenge
'''
self.motorEnable.setValue(0, status=0)