class MyRobot(TimedRobot):
leftMotor: TalonSRX
rightMotor: TalonSRX
mySolenoid = Solenoid(0)
navx = AHRS.create_i2c()
def robotInit(self):
self.leftMotor = VictorSPX(0)
self.rightMotor = TalonSRX("pizza")
def teleopInit(self):
self.leftMotor.setSpeed(0.3)
self.rightMotor.setSpeed(0.3)
self.mySolenoid.set(True)
angle = self.navx.getAngle()
def __init__(self):
self.timer = Timer()
self.driver_station = DriverStation(controller=XboxController(0))
# self.gyroscope = AHRS.create_i2c()
# self.left_encoder = Encoder()
# self.right_encoder = Encoder()
self.drivetrain = TankDrivetrain(timer=self.timer, left_motors=[TalonSRX(10), TalonSRX(6)],
right_motors=[TalonSRX(12), TalonSRX(18)])
self.pneumatics = Pneumatics(compressor=Compressor(0), start_compressor=True, timer=self.timer)
self.beak = BeakMechanism(beak_solenoid=DoubleSolenoid(0, 4, 5), diag_solenoid=DoubleSolenoid(0, 0, 1),
driver_station=self.driver_station, timer=self.timer, cooldown=0.5)
# self.odometry = EncoderOdometry(left_encoder=self.left_encoder, right_encoder=self.right_encoder, gyro=self.gyroscope)
'''
def init(self):
self.logger.info("DeepSpaceLift::init()")
self.timer = wpilib.Timer()
self.timer.start()
self.robot_mode = RobotMode.TEST
self.last_lift_adjust_time = 0
self.lift_adjust_timer = wpilib.Timer()
self.lift_adjust_timer.start()
self.state_timer = wpilib.Timer()
self.current_state = LiftState.LIFT_START_CONFIGURATION
self.current_lift_preset = LiftPreset.LIFT_PRESET_STOW
self.current_lift_preset_val = self.lift_stow_position
self.lift_setpoint = self.min_lift_position
self.lift_adjust_val = 0
self.lift_talon = TalonSRX(robotmap.LIFT_CAN_ID)
'''Select and zero sensor in init() function. That way the zero position doesn't get reset every time we enable/disable robot'''
self.lift_talon.configSelectedFeedbackSensor(FeedbackDevice.Analog, 0,
robotmap.CAN_TIMEOUT_MS)
self.lift_talon.setSelectedSensorPosition(0, 0,
robotmap.CAN_TIMEOUT_MS)
self.lift_pneumatic_extend = Solenoid(robotmap.PCM1_CANID,
robotmap.LIFT_RAISE_SOLENOID)
self.lift_pneumatic_retract = Solenoid(robotmap.PCM1_CANID,
robotmap.LIFT_LOWER_SOLENOID)
self.lift_pneumatic_extend.set(False)
self.lift_pneumatic_retract.set(True)
self.test_lift_pneumatic = sendablechooser.SendableChooser()
self.test_lift_pneumatic.setDefaultOption("Retract", 1)
self.test_lift_pneumatic.addOption("Extend", 2)
SmartDashboard.putData("TestLiftPneumatic", self.test_lift_pneumatic)
def __init__(self):
super().__init__("Elevator")
# Initializes the elevator Talon, put it as one for now
self.elevatorleader = TalonSRX(CAN_ELEVATOR_LEADER)
set_motor(self.elevatorleader, TalonSRX.NeutralMode.Brake, False)
self.elevatorleader.configSelectedFeedbackSensor(
FeedbackDevice.QuadEncoder, 0, 0)
self.elevatorleader.setSelectedSensorPosition(0, 0, 0)
self.elevatorleader.setSensorPhase(False)
self.elevatorfollower = VictorSPX(CAN_ELEVATOR_FOLLOWER)
set_motor(self.elevatorfollower, TalonSRX.NeutralMode.Brake, False)
self.elevatorfollower.follow(self.elevatorleader)
# The preference table is used to get values to the code while the
# robot is running, useful
self.prefs = Preferences.getInstance()
# for tuning the PIDs and stuff
self.elevator_zero = False
self.set_values()
def __init__(self):
super().__init__("Payload")
# TODO Two limit switches on the leader
self.prefs = Preferences.getInstance()
self.elbowleader = TalonSRX(CAN_ELBOW_LEADER)
set_motor(self.elbowleader, TalonSRX.NeutralMode.Brake, False)
self.elbowleader.configSelectedFeedbackSensor(
FeedbackDevice.QuadEncoder, 0, 0)
self.elbowleader.setSelectedSensorPosition(0, 0, 0)
self.elbowleader.selectProfileSlot(0, 0)
self.elbowleader.setSensorPhase(True)
self.elbowfollower = VictorSPX(CAN_ELBOW_FOLLOWER)
set_motor(self.elbowfollower, VictorSPX.NeutralMode.Brake, False)
self.elbowfollower.follow(self.elbowleader)
self.baller = TalonSRX(CAN_BALL_INTAKE)
set_motor(self.baller, TalonSRX.NeutralMode.Brake, False)
self.elbow_zero = False
self.DS = DoubleSolenoid(2, 3)
self.set_values()
def __init__(
self,
name: str,
steer_talon: ctre.TalonSRX,
drive_talon: ctre.TalonSRX,
x_pos: float,
y_pos: float,
reverse_steer_direction: bool = False,
reverse_steer_encoder: bool = True,
reverse_drive_direction: bool = False,
reverse_drive_encoder: bool = False,
):
if hal.isSimulation():
# we aren't using the PID simulation here
steer_talon._use_notifier = False
drive_talon._use_notifier = False
self.name = name
self.steer_motor = steer_talon
self.drive_motor = drive_talon
self.x_pos = x_pos
self.y_pos = y_pos
self.dist = math.hypot(self.x_pos, self.y_pos)
self.angle = math.atan2(self.y_pos, self.x_pos)
self.reverse_steer_direction = reverse_steer_direction
self.reverse_steer_encoder = reverse_steer_encoder
self.reverse_drive_direction = reverse_drive_direction
self.reverse_drive_encoder = reverse_drive_encoder
self.steer_enc_offset = self.steer_motor.configGetCustomParam(
0, timoutMs=10)
self.nt = NetworkTables.getTable("SwerveDrive").getSubTable(name)
self.steer_enc_offset_entry = self.nt.getEntry("steer_enc_offset")
self.steer_enc_offset_entry.setDouble(self.steer_enc_offset)
self.steer_enc_offset_entry.addListener(
self.nt_offset_changed, NetworkTables.NotifyFlags.UPDATE)
self.last_speed = 0.0
self.update_odometry()
# NOTE: In all the following config* calls to the drive and steer
# motors, the last argument is the timeout in milliseconds. See
# robotpy-ctre documentation for details.
self.steer_motor.configSelectedFeedbackSensor(
ctre.FeedbackDevice.CTRE_MagEncoder_Absolute, 0, 10)
# changes direction of motor encoder
self.steer_motor.setSensorPhase(self.reverse_steer_encoder)
# changes sign of motor throttle vilues
self.steer_motor.setInverted(self.reverse_steer_direction)
# self.steer_motor.setSelectedSensorPosition(0)
self.steer_motor.config_kP(0, 1, 10)
self.steer_motor.config_kI(0, 0.0, 10)
self.steer_motor.config_kD(0, 0.02, 10)
self.steer_motor.selectProfileSlot(0, 0)
self.steer_motor.config_kF(0, 0, 10)
self.steer_motor.setNeutralMode(ctre.NeutralMode.Coast)
self.drive_motor.configSelectedFeedbackSensor(
ctre.FeedbackDevice.QuadEncoder, 0, 10)
# changes direction of motor encoder
self.drive_motor.setSensorPhase(self.reverse_drive_encoder)
# changes sign of motor throttle values
self.drive_motor.setInverted(self.reverse_drive_direction)
# Reset drive encoder to 0
self.drive_motor.setSelectedSensorPosition(0)
# TODO: change back to original constants once we get on to real robot
self.drive_motor.config_kP(0, 0.005, 10)
self.drive_motor.config_kI(0, 0, 10)
self.drive_motor.config_kD(0, 0, 10)
self.drive_motor.config_kF(0, 1024.0 / self.DRIVE_FREE_SPEED, 10)
self.drive_motor.configClosedLoopRamp(0.3, 10)
self.drive_motor.selectProfileSlot(0, 0)
self.drive_motor.setNeutralMode(ctre.NeutralMode.Brake)
self.reset_encoder_delta()
# self.steer_motor.configPeakCurrentLimit(50, timeoutMs=10)
# self.steer_motor.configContinuousCurrentLimit(40, timeoutMs=10)
# self.steer_motor.enableCurrentLimit(True)
self.drive_motor.configVoltageCompSaturation(9, timeoutMs=10)
self.drive_motor.configPeakCurrentLimit(50, timeoutMs=10)
self.drive_motor.configContinuousCurrentLimit(40, timeoutMs=10)
self.drive_motor.enableCurrentLimit(True)
self.drive_motor.enableVoltageCompensation(True)
def createMotor(self, MotorSpec):
motr = None
if MotorSpec['ContType'] == 'CAN':
if MotorSpec['Type'] == 'TalonSRX':
if MotorSpec['jobType'] == 'master':
motr = TalonSRX(MotorSpec['port'])
elif MotorSpec['jobType'] == 'slave':
motr = TalonSRX(MotorSpec['port'])
motr.setInverted(MotorSpec['inverted'])
motr.set(ctre.ControlMode.Follower, MotorSpec['masterPort'])
if MotorSpec['Type'] == 'TalonFX':
if MotorSpec['jobType'] == 'master':
motr = TalonFX(MotorSpec['port'])
elif MotorSpec['jobType'] == 'slave':
motr = TalonFX(MotorSpec['port'])
motr.setInverted(MotorSpec['inverted'])
motr.set(ctre.ControlMode.Follower, MotorSpec['masterPort'])
if MotorSpec['Type'] == 'VictorSPX':
if MotorSpec['jobType'] == 'master':
motr = VictorSPX(MotorSpec['port'])
elif MotorSpec['jobType'] == 'slave':
motr = VictorSPX(MotorSpec['port'])
motr.setInverted(MotorSpec['inverted'])
motr.set(ctre.ControlMode.Follower, MotorSpec['masterPort'])
if MotorSpec['Type'] == 'SparkMax':
motr = SparkMax(MotorSpec['port'])
else:
print("IDK your motor")
return motr
def robotInit(self):
self.leftMotor = VictorSPX(0)
self.rightMotor = TalonSRX("pizza")
class Elevator(Subsystem):
"""
For both Hatches and Balls
"""
def __init__(self):
super().__init__("Elevator")
# Initializes the elevator Talon, put it as one for now
self.elevatorleader = TalonSRX(CAN_ELEVATOR_LEADER)
set_motor(self.elevatorleader, TalonSRX.NeutralMode.Brake, False)
self.elevatorleader.configSelectedFeedbackSensor(
FeedbackDevice.QuadEncoder, 0, 0)
self.elevatorleader.setSelectedSensorPosition(0, 0, 0)
self.elevatorleader.setSensorPhase(False)
self.elevatorfollower = VictorSPX(CAN_ELEVATOR_FOLLOWER)
set_motor(self.elevatorfollower, TalonSRX.NeutralMode.Brake, False)
self.elevatorfollower.follow(self.elevatorleader)
# The preference table is used to get values to the code while the
# robot is running, useful
self.prefs = Preferences.getInstance()
# for tuning the PIDs and stuff
self.elevator_zero = False
self.set_values()
def set_position(self, position):
if self.elevator_zero:
self.elevatorleader.set(ControlMode.MotionMagic, position)
else:
print("Position Not Set")
def elevator_speed(self, speed):
self.elevatorleader.set(ControlMode.PercentOutput, speed)
def set_values(self):
# This will set the PIDs, velocity, and acceleration, values from the
# preference table so we can tune them easily
#self.elevatorleader.config_kP(0, self.prefs.getFloat("Elevator P", 0.1), 0)
#self.elevatorleader.config_kI(0, self.prefs.getFloat("Elevator I", 0.0), 0)
#self.elevatorleader.config_kD(0, self.prefs.getFloat("Elevator D", 0.0), 0)
#self.elevatorleader.configMotionCruiseVelocity(
# int(self.prefs.getInt("Elevator Velocity", 1024)), 0)
#self.elevatorleader.configMotionAcceleration(
# int(self.prefs.getInt("Elevator Acceleration", 1024)), 0)
self.elevatorleader.config_kP(0, 3.2, 0)
self.elevatorleader.config_kI(0, 0.0, 0)
self.elevatorleader.config_kD(0, 0.0, 0)
self.elevatorleader.configMotionCruiseVelocity(int(1000), 0)
self.elevatorleader.configMotionAcceleration(int(1000), 0)
def publish_data(self):
# This will print the position and velocity to the smartDashboard
SmartDashboard.putNumber(
"Elevator Position",
self.elevatorleader.getSelectedSensorPosition(0))
#SmartDashboard.putNumber("Elevator Velocity", self.elevatorleader.getSelectedSensorVelocity(0))
#SmartDashboard.putNumber("Elevator Current", self.elevatorleader.getOutputCurrent())
#SmartDashboard.putNumber("Elevator Output", self.elevatorleader.getMotorOutputPercent())
def check_for_zero(self):
if not self.elevator_zero:
if self.elevatorleader.isFwdLimitSwitchClosed():
self.elevatorleader.setSelectedSensorPosition(0, 0, 0)
self.elevator_zero = True
def initDefaultCommand(self):
self.setDefaultCommand(elv_zero())
def get_position(self):
return self.elevatorleader.getSelectedSensorPosition(0)
class Payload(Subsystem):
"""
For both Hatches and Balls
"""
def __init__(self):
super().__init__("Payload")
# TODO Two limit switches on the leader
self.prefs = Preferences.getInstance()
self.elbowleader = TalonSRX(CAN_ELBOW_LEADER)
set_motor(self.elbowleader, TalonSRX.NeutralMode.Brake, False)
self.elbowleader.configSelectedFeedbackSensor(
FeedbackDevice.QuadEncoder, 0, 0)
self.elbowleader.setSelectedSensorPosition(0, 0, 0)
self.elbowleader.selectProfileSlot(0, 0)
self.elbowleader.setSensorPhase(True)
self.elbowfollower = VictorSPX(CAN_ELBOW_FOLLOWER)
set_motor(self.elbowfollower, VictorSPX.NeutralMode.Brake, False)
self.elbowfollower.follow(self.elbowleader)
self.baller = TalonSRX(CAN_BALL_INTAKE)
set_motor(self.baller, TalonSRX.NeutralMode.Brake, False)
self.elbow_zero = False
self.DS = DoubleSolenoid(2, 3)
self.set_values()
def initDefaultCommand(self):
self.setDefaultCommand(BallZ())
def set_wheels_speed(self, speed):
self.baller.set(ControlMode.PercentOutput, speed)
#print(speed)
def hatch_punch_out(self):
subsystems.SERIAL.fire_event('Punch It')
self.DS.set(DoubleSolenoid.Value.kForward)
def hatch_punch_in(self):
self.DS.set(DoubleSolenoid.Value.kReverse)
def set_position(self, pos):
if self.elbow_zero:
self.elbowleader.set(mode=ControlMode.MotionMagic, demand0=pos)
else:
print("Elbow Not Set")
def set_speed(self, speed):
self.elbowleader.set(mode=ControlMode.PercentOutput, demand0=speed)
def set_values(self):
self.elbowleader.config_kP(0, 0.8, 0)
self.elbowleader.config_kI(0, 0.0, 0)
self.elbowleader.config_kD(0, 0.0, 0)
self.elbowleader.configMotionCruiseVelocity(int(200), 0)
self.elbowleader.configMotionAcceleration(int(200), 0)
def get_position(self):
return self.elbowleader.getSelectedSensorPosition(0)
def check_for_zero(self):
if not self.elbow_zero:
if self.elbowleader.isRevLimitSwitchClosed():
self.elbowleader.setSelectedSensorPosition(0, 0, 0)
self.elbow_zero = True
def publish_data(self):
#print(self.elbowleader.getSelectedSensorPosition(0))
SmartDashboard.putNumber("elbowPosition",
self.elbowleader.getSelectedSensorPosition(0))
class DeepSpaceLift():
min_lift_position = ntproperty("/LiftSettings/MinLiftPosition",
75,
persistent=True)
max_lift_position = ntproperty("/LiftSettings/MaxLiftPosition",
225,
persistent=True)
max_lift_adjust_rate = ntproperty("/LiftSettings/MaxLiftAdjustRate",
10,
persistent=True)
max_lift_adjust_value = ntproperty("/LiftSettings/MaxLiftAdjustValue",
15,
persistent=True)
lift_stow_position = ntproperty("/LiftSettings/LiftStowPosition",
90,
persistent=True)
lift_front_port_low = ntproperty("/LiftSettings/LiftFrontPortLow",
150,
persistent=True)
lift_front_port_middle = ntproperty("/LiftSettings/LiftFrontPortMiddle",
175,
persistent=True)
lift_front_port_high = ntproperty("/LiftSettings/LiftFrontPortHigh",
200,
persistent=True)
lift_side_hatch_low = ntproperty("/LiftSettings/LiftSideHatchLow",
135,
persistent=True)
lift_side_hatch_middle = ntproperty("/LiftSettings/LiftSideHatchMiddle",
160,
persistent=True)
lift_side_hatch_high = ntproperty("/LiftSettings/LiftSideHatchHigh",
185,
persistent=True)
on_target = False
def __init__(self, logger):
self.logger = logger
def init(self):
self.logger.info("DeepSpaceLift::init()")
self.timer = wpilib.Timer()
self.timer.start()
self.robot_mode = RobotMode.TEST
self.last_lift_adjust_time = 0
self.lift_adjust_timer = wpilib.Timer()
self.lift_adjust_timer.start()
self.state_timer = wpilib.Timer()
self.current_state = LiftState.LIFT_START_CONFIGURATION
self.current_lift_preset = LiftPreset.LIFT_PRESET_STOW
self.current_lift_preset_val = self.lift_stow_position
self.lift_setpoint = self.min_lift_position
self.lift_adjust_val = 0
self.lift_talon = TalonSRX(robotmap.LIFT_CAN_ID)
'''Select and zero sensor in init() function. That way the zero position doesn't get reset every time we enable/disable robot'''
self.lift_talon.configSelectedFeedbackSensor(FeedbackDevice.Analog, 0,
robotmap.CAN_TIMEOUT_MS)
self.lift_talon.setSelectedSensorPosition(0, 0,
robotmap.CAN_TIMEOUT_MS)
self.lift_pneumatic_extend = Solenoid(robotmap.PCM1_CANID,
robotmap.LIFT_RAISE_SOLENOID)
self.lift_pneumatic_retract = Solenoid(robotmap.PCM1_CANID,
robotmap.LIFT_LOWER_SOLENOID)
self.lift_pneumatic_extend.set(False)
self.lift_pneumatic_retract.set(True)
self.test_lift_pneumatic = sendablechooser.SendableChooser()
self.test_lift_pneumatic.setDefaultOption("Retract", 1)
self.test_lift_pneumatic.addOption("Extend", 2)
SmartDashboard.putData("TestLiftPneumatic", self.test_lift_pneumatic)
def config(self, simulation):
self.logger.info("DeepSpaceLift::config()")
'''Generic config'''
self.lift_talon.configNominalOutputForward(0.0,
robotmap.CAN_TIMEOUT_MS)
self.lift_talon.configNominalOutputReverse(0.0,
robotmap.CAN_TIMEOUT_MS)
self.lift_talon.configPeakOutputForward(1.0, robotmap.CAN_TIMEOUT_MS)
self.lift_talon.configPeakOutputReverse(-1.0, robotmap.CAN_TIMEOUT_MS)
self.lift_talon.enableVoltageCompensation(True)
self.lift_talon.configVoltageCompSaturation(11.5,
robotmap.CAN_TIMEOUT_MS)
self.lift_talon.configOpenLoopRamp(0.125, robotmap.CAN_TIMEOUT_MS)
self.lift_talon.setInverted(True)
'''sensor config'''
self.lift_talon.configSelectedFeedbackCoefficient(
1.0, 0, robotmap.CAN_TIMEOUT_MS)
self.lift_talon.setSensorPhase(False)
self.lift_talon.setStatusFramePeriod(StatusFrame.Status_2_Feedback0,
10, robotmap.CAN_TIMEOUT_MS)
def deploy_lift(self):
self.current_state = LiftState.LIFT_DEPLOY_BEGIN
def go_to_preset(self, preset):
self.lift_adjust_val = 0
if preset == LiftPreset.LIFT_PRESET_PORT_LOW:
self.current_lift_preset = LiftPreset.LIFT_PRESET_PORT_LOW
self.current_lift_preset_val = self.lift_front_port_low
self.lift_pneumatic_extend.set(False)
self.lift_pneumatic_retract.set(True)
self.state_timer.stop()
self.state_timer.reset()
elif preset == LiftPreset.LIFT_PRESET_PORT_MIDDLE:
self.current_lift_preset = LiftPreset.LIFT_PRESET_PORT_MIDDLE
self.current_lift_preset_val = self.lift_front_port_middle
self.lift_pneumatic_extend.set(False)
self.lift_pneumatic_retract.set(True)
self.state_timer.stop()
self.state_timer.reset()
elif preset == LiftPreset.LIFT_PRESET_PORT_HIGH:
self.current_lift_preset = LiftPreset.LIFT_PRESET_PORT_HIGH
self.current_lift_preset_val = self.lift_front_port_high
self.lift_pneumatic_extend.set(True)
self.lift_pneumatic_retract.set(False)
self.state_timer.stop()
self.state_timer.reset()
elif preset == LiftPreset.LIFT_PRESET_HATCH_LOW:
self.current_lift_preset = LiftPreset.LIFT_PRESET_HATCH_LOW
self.current_lift_preset_val = self.lift_side_hatch_low
self.lift_pneumatic_extend.set(False)
self.lift_pneumatic_retract.set(True)
self.state_timer.stop()
self.state_timer.reset()
elif preset == LiftPreset.LIFT_PRESET_HATCH_MIDDLE:
self.current_lift_preset = LiftPreset.LIFT_PRESET_HATCH_MIDDLE
self.current_lift_preset_val = self.lift_side_hatch_middle
self.lift_pneumatic_extend.set(False)
self.lift_pneumatic_retract.set(True)
self.state_timer.stop()
self.state_timer.reset()
elif preset == LiftPreset.LIFT_PRESET_HATCH_HIGH:
self.current_lift_preset = LiftPreset.LIFT_PRESET_HATCH_HIGH
self.current_lift_preset_val = self.lift_side_hatch_high
self.lift_pneumatic_extend.set(True)
self.lift_pneumatic_retract.set(False)
self.state_timer.stop()
self.state_timer.reset()
elif preset == LiftPreset.LIFT_PRESET_STOW:
self.current_lift_preset = LiftPreset.LIFT_PRESET_STOW
self.current_lift_preset_val = self.lift_stow_position
self.state_timer.reset()
self.state_timer.start()
def iterate(self, robot_mode, simulation, pilot_stick, copilot_stick):
self.robot_mode = robot_mode
lift_position = self.lift_talon.getAnalogInRaw()
if lift_position > self.lift_stow_position + 5:
SmartDashboard.putBoolean("Creep", True)
else:
SmartDashboard.putBoolean("Creep", False)
if robot_mode == RobotMode.TEST:
if self.test_lift_pneumatic.getSelected() == 1:
self.lift_pneumatic_extend.set(False)
self.lift_pneumatic_retract.set(True)
else:
self.lift_pneumatic_extend.set(True)
self.lift_pneumatic_retract.set(False)
#need to check these separately so we don't disable the mechanism completely if we end up one tick outside our allowable range
if lift_position > self.min_lift_position or lift_position < self.max_lift_position:
self.lift_talon.set(
ControlMode.PercentOutput,
-1.0 * pilot_stick.getRawAxis(robotmap.XBOX_RIGHT_Y_AXIS))
elif lift_position < self.min_lift_position:
#allow upward motion
self.lift_talon.set(
ControlMode.PercentOutput,
max(
-1.0 *
pilot_stick.getRawAxis(robotmap.XBOX_RIGHT_Y_AXIS), 0))
elif lift_position > self.max_lift_position:
#allow downward motion
self.lift_talon.set(
ControlMode.PercentOutput,
min(
-1.0 *
pilot_stick.getRawAxis(robotmap.XBOX_RIGHT_Y_AXIS), 0))
else:
self.lift_talon.set(ControlMode.PercentOutput, 0.0)
else:
self.iterate_state_machine(pilot_stick, copilot_stick)
SmartDashboard.putString("Lift State", self.current_state.name)
SmartDashboard.putString("Lift Preset", self.current_lift_preset.name)
SmartDashboard.putNumber("Lift Setpoint", self.lift_setpoint)
SmartDashboard.putNumber("Lift Position", lift_position)
def iterate_state_machine(self, pilot_stick, copilot_stick):
#****************DEPLOY SEQUENCE**************************
if self.current_state == LiftState.LIFT_DEPLOY_EXTEND_PNEUMATIC:
self.lift_pneumatic_extend.set(True)
self.lift_pneumatic_retract.set(False)
self.state_timer.reset()
self.state_timer.start()
self.current_state = LiftState.LIFT_DEPLOY_WAIT1
elif self.current_state == LiftState.LIFT_DEPLOY_WAIT1:
if self.state_timer.hasPeriodPassed(0.5):
self.current_state = LiftState.LIFT_DEPLOY_MOVE_TO_STOW
elif self.current_state == LiftState.LIFT_DEPLOY_MOVE_TO_STOW:
self.go_to_preset(LiftPreset.LIFT_PRESET_STOW)
if self.bang_bang_to_setpoint():
self.current_state = LiftState.LIFT_DEPLOY_RETRACT_PNEUMATIC
if self.current_state == LiftState.LIFT_DEPLOY_RETRACT_PNEUMATIC:
self.lift_pneumatic_extend.set(False)
self.lift_pneumatic_retract.set(True)
self.state_timer.reset()
self.state_timer.start()
self.current_state = LiftState.LIFT_DEPLOY_WAIT2
elif self.current_state == LiftState.LIFT_DEPLOY_WAIT2:
if self.state_timer.hasPeriodPassed(0.25):
self.current_state = LiftState.LIFT_DEPLOYED
#****************DEPLOY SEQUENCE**************************
#****************DEPLOYED*********************************
elif self.current_state == LiftState.LIFT_DEPLOYED:
if self.robot_mode == RobotMode.TELE:
if copilot_stick.LeftBumper().get() and copilot_stick.X().get(
):
if not self.current_lift_preset == LiftPreset.LIFT_PRESET_PORT_LOW:
self.go_to_preset(LiftPreset.LIFT_PRESET_PORT_LOW)
elif copilot_stick.LeftBumper().get() and copilot_stick.Y(
).get():
if not self.current_lift_preset == LiftPreset.LIFT_PRESET_PORT_MIDDLE:
self.go_to_preset(LiftPreset.LIFT_PRESET_PORT_MIDDLE)
elif copilot_stick.LeftBumper().get() and copilot_stick.B(
).get():
if not self.current_lift_preset == LiftPreset.LIFT_PRESET_PORT_HIGH:
self.go_to_preset(LiftPreset.LIFT_PRESET_PORT_HIGH)
elif copilot_stick.RightBumper().get() and copilot_stick.X(
).get():
if not self.current_lift_preset == LiftPreset.LIFT_PRESET_HATCH_LOW:
self.go_to_preset(LiftPreset.LIFT_PRESET_HATCH_LOW)
elif copilot_stick.RightBumper().get() and copilot_stick.Y(
).get():
if not self.current_lift_preset == LiftPreset.LIFT_PRESET_HATCH_MIDDLE:
self.go_to_preset(LiftPreset.LIFT_PRESET_HATCH_MIDDLE)
elif copilot_stick.RightBumper().get() and copilot_stick.B(
).get():
if not self.current_lift_preset == LiftPreset.LIFT_PRESET_HATCH_HIGH:
self.go_to_preset(LiftPreset.LIFT_PRESET_HATCH_HIGH)
else:
if not self.current_lift_preset == LiftPreset.LIFT_PRESET_STOW:
self.go_to_preset(LiftPreset.LIFT_PRESET_STOW)
current_lift_adjust_time = self.lift_adjust_timer.get()
dt = current_lift_adjust_time - self.last_lift_adjust_time
self.last_lift_adjust_time = current_lift_adjust_time
if self.bang_bang_to_setpoint():
max_down_adjust = float(
abs(self.current_lift_preset_val -
self.lift_stow_position))
max_up_adjust = float(
abs(self.current_lift_preset_val - self.max_lift_position))
self.lift_adjust_val += dt * self.max_lift_adjust_rate * pilot_stick.RightStickY(
)
if self.lift_adjust_val > 0:
self.lift_adjust_val = min(self.lift_adjust_val,
self.max_lift_adjust_value)
else:
self.lift_adjust_val = max(self.lift_adjust_val,
-self.max_lift_adjust_value)
self.lift_adjust_val = max(
min(self.lift_adjust_val, max_up_adjust), -max_down_adjust)
#****************DEPLOYED*********************************
def bang_bang_to_setpoint(self):
if self.current_lift_preset == LiftPreset.LIFT_PRESET_STOW and (
self.state_timer.get() > 1.0
or self.robot_mode == RobotMode.AUTO):
self.lift_pneumatic_extend.set(False)
self.lift_pneumatic_retract.set(True)
self.set_lift_setpoint(self.current_lift_preset_val +
int(self.lift_adjust_val))
if not self.current_lift_preset == LiftPreset.LIFT_PRESET_STOW:
self.set_lift_setpoint(self.current_lift_preset_val +
int(self.lift_adjust_val))
lift_position = self.lift_talon.getSelectedSensorPosition(0)
err = abs(lift_position - self.lift_setpoint)
if err > 1:
self.on_target = False
if lift_position < self.lift_setpoint:
self.lift_talon.set(ControlMode.PercentOutput, 1.0)
elif lift_position > self.lift_setpoint:
self.lift_talon.set(ControlMode.PercentOutput, -1.0)
else:
self.lift_talon.set(ControlMode.PercentOutput, 0.0)
self.on_target = True
return self.on_target
def disable(self):
self.logger.info("DeepSpaceLift::disable()")
self.lift_talon.set(ControlMode.PercentOutput, 0)
def set_lift_setpoint(self, ticks):
self.lift_setpoint = max(min(ticks, self.max_lift_position),
self.min_lift_position)