class Cargo:
lift_piston = wpilib.Solenoid
motor = ctre.TalonSRX
motor_speed = will_reset_to(0)
is_in_position = tunable(False)
motor_input_speed = tunable(-0.6)
motor_output_speed = tunable(1)
def lift(self):
self.is_in_position = False
def lower(self):
self.is_in_position = True
def intake(self):
self.motor_speed = self.motor_input_speed
def outtake(self):
self.motor_speed = self.motor_output_speed
def execute(self):
self.motor.set(ctre.ControlMode.PercentOutput, self.motor_speed)
class Drive:
"""
Handle robot drivetrain.
All drive interaction must go through this class.
"""
train: wpilib.drive.DifferentialDrive
y = will_reset_to(0)
rot = will_reset_to(0)
speed_constant = tunable(1.05)
rotational_constant = tunable(0.5)
squared_inputs = tunable(False)
def setup(self):
"""
Run setup code on the injected variables (train)
"""
def move(self, y: float, rot: float):
"""
Move robot.
:param y: Speed of motion in the y direction. [-1..1]
:param rot: Speed of rotation. [-1..1]
"""
self.y = y
self.rot = rot
def execute(self):
"""
Handle driving.
"""
self.train.arcadeDrive(self.speed_constant * self.y,
self.rotational_constant * self.rot,
squareInputs=self.squared_inputs)
class Cargo:
current = tunable(0)
smooth = tunable(0)
alpha = tunable(0.92)
inThreshold = tunable(11)
cargo_intake_motor: ctre.WPI_TalonSRX
def setup(self):
self.speed = 0
self.kMin = 1
self.kMax = 4 #
def shoot(self):
self.speed = 1
def intake(self):
self.speed = -1
def off(self):
self.speed = 0
def isBallIn(self):
return self.smooth > self.inThreshold
def execute(self):
self.cargo_intake_motor.set(self.speed)
self.current = self.cargo_intake_motor.getOutputCurrent()
self.smooth = (self.alpha * self.smooth) + (1 - self.alpha) * self.current
class FeederMap:
"""Simple map that holds the logic for running elements of the feeder."""
compatString = ["doof"]
shooterMotors: ShooterMotorCreation
xboxMap: XboxMap
logger: logging
loaderMotorSpeed = tunable(.4)
intakeMotorSpeed = tunable(.7)
def on_enable(self):
pass
# self.logger.setLevel(logging.DEBUG)
def run(self, loaderFunc):
"""Called when execution of a feeder element is desired."""
if loaderFunc == Type.kIntake:
if self.xboxMap.getMechRightTrig(
) > 0 and self.xboxMap.getMechLeftTrig() == 0:
self.shooterMotors.runIntake(self.intakeMotorSpeed,
Direction.kForwards)
self.logger.debug("right trig intake",
self.xboxMap.getMechRightTrig())
elif self.xboxMap.getMechLeftTrig(
) > 0 and self.xboxMap.getMechRightTrig() == 0:
self.shooterMotors.runIntake(self.intakeMotorSpeed,
Direction.kBackwards)
self.logger.debug("left trig intake",
self.xboxMap.getMechLeftTrig())
else:
self.shooterMotors.stopIntake()
if loaderFunc == Type.kLoader:
if self.xboxMap.getMechRightTrig(
) > 0 and self.xboxMap.getMechLeftTrig() == 0:
self.shooterMotors.runLoader(self.loaderMotorSpeed,
Direction.kForwards)
self.logger.debug("right trig manual",
self.xboxMap.getMechRightTrig())
elif self.xboxMap.getMechLeftTrig(
) > 0 and self.xboxMap.getMechRightTrig() == 0:
self.shooterMotors.runLoader(self.loaderMotorSpeed,
Direction.kBackwards)
self.logger.debug("left trig manual",
self.xboxMap.getMechLeftTrig())
else:
self.shooterMotors.stopLoader()
def execute(self):
pass
class CounterFSM(StateMachine):
"""
This state machine will continuously watch the current of the
intake motor, watching to see if any power cells are being
loaded, based off the spikes in current draw. If multiple cells
are grabbed, it will likely only count it as one.
If the intake is not running, the current will not be watched.
"""
intake: Intake
ball_count: int
balanced_zone = tunable(1)
spike_zone = tunable(2)
@state(first=True)
def watch_intake_state(self):
"""
Watch the intake to wait for it to become enabled.
Once enabled, wait for the motor controller to read
a current deadzone that can be detected as the motor
being at the correct speed.
"""
if abs(self.intake.intake_speed) > 0.2:
self.next_state("intake_running_ignore_current")
@state()
def intake_running_ignore_current(self):
"""
Wait for the motor controller to read a current deadzone
that can be detected as the motor being at the correct speed.
If the intake is stopped partway through this, move back to
the main state.
"""
if abs(self.intake.intake_speed) < 0.2:
self.next_state("watch_intake_state")
if self.intake.motor.getOutputCurrent() < self.balanced_zone:
self.next_state("intake_running")
@state()
def intake_running(self):
"""
Increment the ball count if a spike in current is detected.
It is very likely that when the motor spikes in current,
a ball has touched the motors, causing the motor to slightly
torque itself, drawing more current.
If the intake is stopped partway through this, move back to
the main state.
"""
if abs(self.intake.intake_speed) < 0.2:
self.next_state("watch_intake_state")
if self.intake.motor.getOutputCurrent() > self.spike_zone:
self.ball_count += 1
self.next_state("intake_running_ingore_current")
class Arm:
"""
Single Jointed arm for that holds the hatch and cargo mechs
"""
arm_motor = WPI_TalonSRX
arm_limit = wpilib.DigitalInput
reset_angle = will_reset_to(180)
kp = tunable(1)
ki = tunable(0)
kd = tunable(0)
kf = tunable(0)
target_angle = will_reset_to(180)
zeroed = False
# Constants
armRatio = 24 / 84
def setup(self):
self.arm_controller = ArmPID()
self.blahhhh = ElevatorPID(self.kp, self.ki, self.kd, self.kf)
self.arm_motor.clearStickyFaults()
self.zeroed = False
def set(self, angle):
self.target_angle = angle
self.arm_controller.resetIntegrator()
def angle(self, ticks):
return ticks * (360 * self.armRatio) / 4096
def ticks(self, angle):
return int(angle * (4096 / (360 * self.armRatio)))
def execute(self):
if not self.zeroed:
self.arm_motor.set(0.2)
if self.arm_limit.get():
self.zeroed = True
self.arm_motor.setSelectedSensorPosition(
self.ticks(self.reset_angle)
)
return # exits out of the function if we aren't zeroed
self.arm_motor.set(
self.arm_controller.update(
self.kp,
self.ki,
self.kd,
self.kf,
self.target_angle,
self.angle(self.arm_motor.getSelectedSensorPosition()),
)
)
class Recorder:
"""
Record control input for playback as an autonomous mode.
"""
directory = tunable('/tmp')
title = tunable('')
frames = []
def start(self, voltage):
"""
Start a recording.
:param voltage: Battery output voltage. Necessary for scaling speeds later on.
"""
self.voltage = voltage
def capture(self, joysticks):
"""
Make snapshot of joystick inputs during this cycle.
:param joysticks: Tuple of joysticks to read.
"""
self.frames.append({
'joysticks': [{
'axes': [joystick.getRawAxis(axs) for axs in range(joystick.getAxisCount())],
# TODO: Buttons are one-indexed. Trying to interact with the 0th button will throw.
'buttons': [joystick.getRawButton(btn) for btn in range(joystick.getButtonCount())],
'pov': [joystick.getPOV(pov) for pov in range(joystick.getPOVCount())],
} for joystick in joysticks]
})
def stop(self):
"""
End recording and save recorded data to file.
"""
with open('{directory}/{filename}.json'.format(
directory=self.directory,
filename=self.title if self.title else int(time.time())), 'w+') as f:
json.dump({
'voltage': self.voltage,
'frames': self.frames,
}, f)
self.title = ''
self.voltage = None
self.frames = []
def execute(self):
"""
Run periodically when injected through MagicBot.
"""
pass
class Drive:
"""
Handle robot drivetrain.
All drive interaction must go through this class.
"""
train: wpilib.drive.DifferentialDrive
y = will_reset_to(0)
rot = will_reset_to(0)
speed_constant = tunable(1.05)
rotational_constant = tunable(0.5)
squared_inputs = tunable(False)
fine_movement = will_reset_to(False)
fine_speed_multiplier = tunable(0.5)
fine_rotation_multiplier = tunable(0.5)
def __init__(self):
self.enabled = False
def setup(self):
"""
Set input threshold.
"""
self.train.setDeadband(0.1)
def move(self, y: float, rot: float, fine_movement: bool = False):
"""
Move robot.
:param y: Speed of motion in the y direction. [-1..1]
:param rot: Speed of rotation. [-1..1]
:param fine_movement: Decrease speeds for precise motion.
"""
self.y = y
self.rot = rot
self.fine_movement = fine_movement
def execute(self):
"""
Handle driving.
"""
self.train.arcadeDrive(
self.speed_constant * self.y *
(self.fine_speed_multiplier if self.fine_movement else 1),
self.rotational_constant * self.rot *
(self.fine_rotation_multiplier if self.fine_movement else 1),
squaredInputs=self.squared_inputs)
class Arm:
solenoid: wpilib.Solenoid
arm_up = tunable(False)
def execute(self):
self.solenoid.set(self.arm_up)
def toggle(self):
'''
Toggles whether the arm is up or down,
depending on its current position.
'''
self.arm_up = not self.arm_up
def toTop(self):
'''
Makes the arm go up, no matter what.
'''
self.arm_up = True
def toBottom(self):
'''
Makes the arm go down, no matter what.
'''
self.arm_up = False
class Fork:
fork_switch: wpilib.DigitalInput
arm_motor: ctre.WPI_TalonSRX
position = will_reset_to(0)
last_position = 0
fork_switch_on = tunable(False)
def execute(self):
# TODO: the limit switch seems to be broken on the robot :(
self.fork_switch_on = self.fork_switch.get()
# if the limit switch is on and you last opened it,
# and you are trying to open it
#if self.fork_switch_on and self.position and self.last_position == self.position:
# self.position = 0
#elif self.position != 0: # if nothing is being pressed - do this
# self.last_position = self.position
self.arm_motor.set(self.position)
#self.arm_motor.set(0)
# @timed_state(state_tm)
def open(self):
self.position = -1
def close(self):
self.position = 1
class Forklift:
winch_motor: ctre.WPI_TalonSRX
position = 0
mode = 'pct'
pct = 0
encoder = tunable(0)
pid_p = tunable(0.9)
set_p = None
top_position = tunable(400)
mid_position = tunable(45)
low_position = tunable(0)
def execute(self):
if self.set_p is None or abs(self.set_p - self.pid_p) < 0.0001:
self.winch_motor.config_kP(0, self.pid_p, 0)
self.set_p = self.pid_p
if self.mode == 'pct':
self.winch_motor.set(self.pct)
self.pct = 0
else:
# Don't uncomment this until we test it!
#self.winch_motor.set(WPI_TalonSRX.ControlMode.Position, self.position)
pass
self.encoder = self.winch_motor.getSensorCollection().getQuadraturePosition()
def normal(self, v):
self.mode = 'pct'
self.pct = v
def top(self):
self.mode = 'pos'
self.position = self.top_position
def mid(self):
self.mode = 'pos'
self.position = self.mid_position
def bot(self):
self.mode = 'pos'
self.position = self.low_position
class Ledstrip:
blinkin: wpilib.Spark
powerOut = tunable(0)
def setMode(self, powerOut):
self.powerOut = powerOut
def execute(self):
self.blinkin.set(self.powerOut)
class Elevator:
motors_loader: dict
downSpeed = tunable(-.4)
upSpeed = tunable(.4)
def on_enable(self):
self.speed = 0
self.elevatorMotor = self.motors_loader['elevatorMotor']
print("Elevator Enabled")
def setRaise(self):
self.speed = self.upSpeed
def setLower(self):
self.speed = self.downSpeed
def stop(self):
self.speed = 0
def execute(self):
self.elevatorMotor.set(self.speed)
class Ramp:
is_practice_bot = DigitalInput
solenoid = DoubleSolenoid
motor = WPI_TalonSRX
hold_position_controller = HoldPositionController
speed = tunable(1.0)
def setup(self):
self.state = RampState.LOCKED
self.pending_winch = WinchState.DISABLED
self.motor.setInverted(True)
def release(self):
self.state = RampState.RELEASED
def is_released(self):
return self.state == RampState.RELEASED
def lock(self):
self.state = RampState.LOCKED
def raise_ramp(self):
self.pending_winch = WinchState.RAISING
def lower_ramp(self):
self.pending_winch = WinchState.LOWERING
def execute(self):
if self.is_practice_bot.get():
# No ramps on practice bot
return
# Winch
if self.pending_winch == WinchState.RAISING:
self.motor.set(self.speed)
self.pending_winch = WinchState.DISABLED
elif self.pending_winch == WinchState.LOWERING:
self.motor.set(-self.speed)
self.pending_winch = WinchState.DISABLED
else:
self.motor.set(0)
# Solenoid lock
if self.state == RampState.LOCKED:
self.solenoid.set(DoubleSolenoid.Value.kReverse)
elif self.state == RampState.RELEASED:
self.solenoid.set(DoubleSolenoid.Value.kForward)
def on_disabled(self):
self.motor.set(0)
class YPosController(BasePIDComponent):
drive = swervedrive.SwerveDrive
tracker = position_tracker.PositionTracker
kP = tunable(0.09)
kI = tunable(0.0)
kD = tunable(0.0)
kF = tunable(0.0)
kToleranceFeet = tunable(0.25)
kIzone = tunable(0.25)
def __init__(self):
super().__init__(self.get_position, 'y_ctrl')
self.set_abs_output_range(0.16, 0.8)
def get_position(self):
return self.tracker.get_y() / 1.0
def move_to(self, position):
self.setpoint = position
def is_at_location(self):
return self.enabled and abs(self.get_position() - self.setpoint) < self.kToleranceFeet
def execute(self):
super().execute()
if self.rate is not None:
if self.is_at_location():
self.drive.set_raw_fwd(0)
else:
self.drive.set_raw_fwd(self.rate)
class AngleController:
drivetrain: Drivetrain
kP = tunable(default=0.2)
kI = tunable(default=0)
kD = tunable(default=0)
def __init__(self):
self.pid_output = 0
def setup(self):
self.pid = wpilib.PIDController(self.kP,
self.kI,
self.kD,
source=self.drivetrain.gyro,
output=self)
self.pid.setOutputRange(-0.4, 0.4)
self.pid.setInputRange(-180, 180)
self.pid.setContinuous()
self.pid.setAbsoluteTolerance(2)
def align_to(self, angle):
self.pid.setSetpoint(angle)
self.pid.enable()
def is_enabled(self):
return self.pid.enabled
def reset_angle(self):
self.drivetrain.gyro.reset()
def pidWrite(self, output):
self.pid_output = output
def execute(self):
if self.pid.enabled:
if self.pid.onTarget():
self.pid.disable()
else:
self.drivetrain.move_x(self.pid_output)
class IRSensor:
serial = SerialPort
i2c = I2C
threshold = tunable(300)
def setup(self):
self.activations = None
self.angle = None
self.displacement = None
self.oriented = False
self.saved_threshold = 300
def set_threshold(self):
self.serial.writeString("t"+str(self.threshold))
return self.serial.readString(3)==str(self.threshold)
def get_array_one(self):
a = str(bin(0))[2:]
b = str(bin(0))[2:]
return a+'0'*(10-len(a))+b+'0'*(10-len(b))
def get_array_two(self):
a = str(bin(0))[2:]
b = str(bin(0))[2:]
return a+'0'*(10-len(a))+b+'0'*(10-len(b))
def compute_orientation(self):
c1 = [i for i,j in enumerate(self.activations[:16]) if int(j)]
c1 = sum(c1)/(len(c1)+0.01)
c2 = [i for i,j in enumerate(self.activations[16:]) if int(j)]
c2 = sum(c2)/(len(c2)+0.01)
self.displacement = 62.5 - (8*c1+8*c2)/2
self.angle = degrees(atan2(c1-62.5,IR_SEPARATION/2))
def isOriented(self):
return self.oriented
def execute(self):
if self.saved_threshold != self.threshold:
success = self.set_threshold()
if success:
self.saved_threshold = self.threshold
else:
self.threshold = self.saved_threshold
self.activations = self.get_array_one()+self.get_array_two()
self.compute_orientation()
table.putValue('activations', self.activations)
table.putValue('angle', self.activations)
table.putValue('displacement', self.activations)
class Drivetrain:
shifter: wpilib.Solenoid
train: wpilib.drive.DifferentialDrive
forward_speed = will_reset_to(0)
turn_speed = will_reset_to(0)
kP = tunable(1)
kI = tunable(0)
kD = tunable(0)
drive_multiplier = tunable(.7)
turn_multiplier = tunable(.5)
high_gear = tunable(False)
currentSpeed = tunable(0)
integral = tunable(0)
previous_error = tunable(0)
def execute(self):
self.shifter.set(self.high_gear)
self.train.arcadeDrive(self.forward_speed, self.turn_speed)
def PID(self, targetSpeed):
'''
Uses the current speed alongside the target speed to create a smooth ramping
'''
error = self.currentSpeed - targetSpeed
self.integral = self.integral + (error * .02)
derivative = (error - self.previous_error) / .02
return self.kP * error + self.kI * self.integral + self.kD * derivative
def shift_toggle(self):
self.high_gear = not self.high_gear
def move(self, speed, turn):
self.forward_speed = -speed * self.drive_multiplier
self.turn_speed = turn * self.turn_multiplier
def movePID(self, speed, turn):
self.forward_speed = self.PID(speed)
self.turn_speed = turn
class Hatch:
lift_piston = wpilib.Solenoid
hold_piston = wpilib.Solenoid
is_holding = tunable(False)
is_in_position = tunable(False)
def lift(self):
self.is_in_position = False
def lower(self):
self.is_in_position = True
def hold(self):
self.is_holding = True
def release(self):
self.is_holding = False
def execute(self):
self.lift_piston.set(self.is_in_position)
self.hold_piston.set(self.is_holding)
class AngleController(PIDOutput):
drive: Drive
navx: navx.AHRS
kP = tunable(0.002)
kI = tunable(0.0)
kD = tunable(0.0)
rate = 0.0
def setup(self):
self.pid_controller = wpilib.PIDController(
0.01, 0.0001, 0.0001, self.navx, self
)
self.pid_controller.setInputRange(-180.0, 180.0)
self.pid_controller.setOutputRange(-.75, .75)
self.pid_controller.setContinuous()
self.pid_controller.setAbsoluteTolerance(3)
self.pid_controller.enable()
def enable(self):
self.pid_controller.enable()
def disable(self):
self.pid_controller.disable()
def align_to(self, value: float):
self.pid_controller.setSetpoint(value)
def on_target(self):
return self.pid_controller.onTarget()
# PIDOutput
def pidWrite(self, output: float):
self.rate = output
def execute(self):
self.drive.drive(0, self.rate)
class Robot(magicbot.MagicRobot):
drive: Drive
time = tunable(0)
voltage = tunable(0)
rotation = tunable(0)
def createObjects(self):
# Joysticks
self.joystick_left = wpilib.Joystick(0)
self.joystick_right = wpilib.Joystick(1)
self.joystick_alt = wpilib.Joystick(2)
# Set up Speed Controller Groups
self.left_motors = wpilib.SpeedControllerGroup(
CANSparkMax(1, MotorType.kBrushless),
CANSparkMax(2, MotorType.kBrushless),
CANSparkMax(3, MotorType.kBrushless))
self.right_motors = wpilib.SpeedControllerGroup(
CANSparkMax(4, MotorType.kBrushless),
CANSparkMax(5, MotorType.kBrushless),
CANSparkMax(6, MotorType.kBrushless))
# Drivetrain
self.train = wpilib.drive.DifferentialDrive(self.left_motors,
self.right_motors)
# NavX (purple board on top of the RoboRIO)
self.navx = navx.AHRS.create_spi()
self.navx.reset()
def teleopInit(self):
self.drive.squared_inputs = True
self.drive.rotational_constant = 0.5
self.train.setDeadband(0.1)
def teleopPeriodic(self):
self.drive.move(-self.joystick_left.getY(), self.joystick_right.getX())
class Elevator:
USE_MOTIONMAGIC = True
shooter_motor = WPI_TalonFX
shooter_motor_slave = WPI_TalonFX
kFreeSpeed = tunable(0.1)
kZeroingSpeed = tunable(0.1)
kP = tunable(0.3)
kI = tunable(0.0)
kD = tunable(0.0)
kF = tunable(0.0)
kCruiseVelocity = 30000
kAcceleration = 12000
setpoint = tunable(0)
value = tunable(0)
error = tunable(0)
class Lift:
lift_master: CANTalon
lift_encoder: common.encoder.BaseEncoder
setpoint = tunable(0)
def setup(self):
self.manual_override = False
self.manual_override_value = 0
self.pid_controller = wpilib.PIDController(0.022, 0.0, 0.0,
self.lift_encoder,
self.lift_master)
self.pid_controller.setAbsoluteTolerance(0.5)
self.pid_controller.setContinuous(False)
self.pid_controller.setOutputRange(-.165, .6)
self.pid_controller.enable()
def set_setpoint(self, new_pos):
self.setpoint = new_pos
self.pid_controller.setSetpoint(new_pos)
def get_setpoint(self) -> float:
return self.setpoint
def set_manual_override_value(self, new_value):
self.manual_override_value = new_value
def set_manual_override(self, override: bool):
# Disable pid if manual override is being enabled
if not self.manual_override and override:
self.pid_controller.disable()
# Renable pid and zero the encoders if manual override is being disabled
if self.manual_override and not override:
self.lift_encoder.zero()
self.pid_controller.enable()
self.manual_override = override
def execute(self):
if self.manual_override:
self.lift_master.set(CANTalon.ControlMode.PercentOutput,
self.manual_override_value)
wpilib.SmartDashboard.putData("Lift Encoder",
self.lift_encoder.encoder)
wpilib.SmartDashboard.putBoolean("Lift Target",
self.pid_controller.onTarget())
class TwoSteps(AutonomousStateMachine):
MODE_NAME = 'Two Steps'
DEFAULT = True
drive = Drive
drive_speed = tunable(-1)
# @timed_state(duration=2, next_state='start_driving', first=True)
# def dont_do_something(self):
@timed_state(duration=5, first=True)
def drive_forward(self):
# '''This happens second'''
self.drive.start_driving(1, 0, 0)
class Intake:
left_motor: ctre.WPI_TalonSRX
right_motor: ctre.WPI_TalonSRX
intake_speed = will_reset_to(0)
kIntakeSpeed = tunable(.7)
def execute(self):
self.left_motor.set(self.intake_speed)
self.right_motor.set(-self.intake_speed)
def suck(self):
self.intake_speed = -self.kIntakeSpeed
def push(self):
self.intake_speed = self.kIntakeSpeed
class Intake:
intake_motor: ctre.WPI_TalonSRX
speed = magicbot.will_reset_to(0)
magnitude = magicbot.tunable(0.5)
#TO DO: Find the correct directions for the intake and outake.
def ballIn(self):
self.speed = self.magnitude
def ballOut(self):
self.speed = -self.magnitude
def execute(self):
self.intake_motor.set(self.speed)
class ScorpionLoader:
compatString = ["scorpion"]
motors_shooter: dict
motors_loader: dict
xboxMap: XboxMap
shooterSpeed = tunable(-0.4)
def setup(self):
self.intakeSpeed = 0
self.loaderSpeed = 0
self.shooterSpeed = 0
self.loaderMotor = self.motors_loader["loaderMotor"]
self.intakeMotor = self.motors_loader["intakeMotor"]
self.logger.info("Shooter Motor Component Created")
def runLoader(self, lSpeed):
self.loaderSpeed = lSpeed
self.loader = True
def runIntake(self, iSpeed):
self.intakeSpeed = iSpeed
self.intake = True
def stopIntake(self):
self.intake = False
def stopLoader(self):
self.loader = False
def isLoaderActive(self):
return self.loader
def checkController(self):
self.runIntake(self.shooterSpeed if self.xboxMap.mechLeft < -.2 else 0)
self.runLoader(self.xboxMap.mechRight)
def execute(self):
if self.intake:
self.intakeMotor.set(self.intakeSpeed)
elif self.intake == False:
self.intakeMotor.set(0)
if self.loader:
self.loaderMotor.set(self.loaderSpeed)
elif self.loader == False:
self.loaderMotor.set(0)
class SideGear(AutonomousStateMachine):
drivetrain = DriveTrain
rotator = Rotator
turn = tunable(.73)
def on_enable(self):
super().on_enable()
self.p = wpilib.Timer()
self.p.start()
def execute(self):
super().execute()
if self.p.hasPeriodPassed(0.3):
self.logger.info("Distance: %0.3f", self.drivetrain.getDistance())
@timed_state(duration=1.5, first=True, next_state='initialRotate')
def drive_forward(self):
self.drivetrain.move(-0.7, 0)
@timed_state(duration=4, next_state='nextRotate')
def initialRotate(self):
self.drivetrain.move(0, self.sign * self.turn)
self.rotator.rotateTotarget()
if self.rotator.found:
self.next_state('detect')
@timed_state(duration=4, next_state='initialRotate')
def nextRotate(self):
self.drivetrain.move(0, self.sign * -self.turn)
self.rotator.rotateTotarget()
if self.rotator.found:
self.next_state('detect')
@timed_state(duration=3, next_state='drive_to_the_wall')
def detect(self):
self.drivetrain.move(0, self.sign * self.turn)
self.rotator.rotateTotarget()
if not self.rotator.found:
self.next_state('initialRotate')
@state
def drive_to_the_wall(self):
self.rotator.rotateTotarget()
self.drivetrain.driveToWall()
class Straight(AutonomousStateMachine):
MODE_NAME = 'Drive Straight'
DEFAULT = True
drivetrain = Drivetrain
drive_speed = tunable(0.5)
@timed_state(duration=2, first=True)
def dont_do_something(self):
'''This happens first'''
pass
@timed_state(duration=2)
def do_something(self):
'''This happens second'''
self.drivetrain.driveStraight()
class TwoSteps(AutonomousStateMachine):
MODE_NAME = "Two Steps"
DEFAULT = True
component2 = Component2
drive_speed = tunable(-1)
@timed_state(duration=2, next_state="do_something", first=True)
def dont_do_something(self):
"""This happens first"""
pass
@timed_state(duration=5)
def do_something(self):
"""This happens second"""
self.component2.do_something()
