def updateDashboardPeriodic(self):
#SmartDashboard.putNumber("Timer", self.timer.get())
SmartDashboard.putNumber("PressureSwitch",
self.compressor.getPressureSwitchValue())
#self.drive.dashboardPeriodic()
#self.hatch.dashboardPeriodic()
self.cargo.dashboardPeriodic()
def periodic(self, updateDashboard: bool):
""" Periodic checks and dashboard updates. """
if updateDashboard:
if self.debug == True:
n = self.name
SmartDashboard.putNumber(n + " Floor Volts",
self.floorSensor.getVoltage())
def display_PIDs(self):
keys = ['kP', 'kI', 'kD', 'kIz', 'kFF']
for key in keys:
SmartDashboard.putNumber(
key + '_0', self.PID_multiplier * self.PID_dict_pos[key])
SmartDashboard.putNumber(
key + '_1', self.PID_multiplier * self.PID_dict_vel[key])
def initialize_dashboard(self):
# dummy setpoints to speed up testing from the dashboard
SmartDashboard.putNumber('z_distance', 2.0)
SmartDashboard.putNumber('z_angle', 60)
self.drive_fwd_command = AutonomousDriveTimed(self.robot, timeout=1)
self.rotate_command = AutonomousRotate(self.robot,
setpoint=45,
timeout=3,
source='dashboard')
self.autonomous_test_command = AutonomousSlalom(self.robot)
self.autonomous_test_ramsete_command = AutonomousRamsete(self.robot)
self.autonomous_pid_command = AutonomousDrivePID(self.robot,
setpoint=2,
timeout=4,
source='dashboard')
# these don't work right in 2021 as of 20210131 - keep interrupting and restarting - old ocmmand structure issue?
#SmartDashboard.putData("Auto Ramsete", self.autonomous_test_ramsete_command)
#SmartDashboard.putData("Auto PID", self.autonomous_pid_command)
#SmartDashboard.putData("Auto Drive", self.drive_fwd_command )
SmartDashboard.putData('Rotate command', self.rotate_command)
# set up the dashboard chooser for the autonomous options
self.obstacle_chooser = SendableChooser()
routes = ['slalom', 'barrel', 'bounce', 'none']
for ix, position in enumerate(routes):
if ix == 3:
self.obstacle_chooser.setDefaultOption(position, position)
else:
self.obstacle_chooser.addOption(position, position)
wpilib.SmartDashboard.putData('obstacles', self.obstacle_chooser)
self.path_chooser = SendableChooser()
wpilib.SmartDashboard.putData('ramsete path', self.path_chooser)
#choices = ['loop', 'poses', 'points', 'test', 'slalom_pw0_0.75','slalom_pw1_0.75', 'slalom_pw2_0.75', 'slalom_pw0_1.25', 'slalom_pw1_1.25',
# 'slalom_pw2_1.25', 'slalom_pw1_1.80', 'barrel_pw1_0.75', 'barrel_pw1_1.25', 'bounce_pw1_0.75', 'bounce_pw1_1.25', 'student_pw0_0p75',
# 'student_pw1_0p75','student_pw0_1p25', 'student_pw1_1p25']
#choices = drive_constants.get_pathweaver_files() + ['z_loop', 'z_poses', 'z_points', 'z_test']
choices = drive_constants.get_pathweaver_paths(
simulation=self.robot.isSimulation()) + [
'z_loop', 'z_poses', 'z_points', 'z_test'
]
for ix, position in enumerate(choices):
if ix == 0:
self.path_chooser.setDefaultOption(position, position)
else:
self.path_chooser.addOption(position, position)
self.velocity_chooser = SendableChooser()
wpilib.SmartDashboard.putData('path velocity', self.velocity_chooser)
velocities = [
0.5, 1.0, 1.5, 2.0, 2.5, 3.0, 3.5, 4.0, 4.5, 5.0, 6.0, 7.0
]
for ix, position in enumerate(velocities):
if ix == 4: # 2.5 will be the default
self.velocity_chooser.setDefaultOption(str(position), position)
else:
self.velocity_chooser.addOption(str(position), position)
def dashboardPeriodic(self):
SmartDashboard.putBoolean("Fully Extended Front",
self.isFullyExtendedFront())
SmartDashboard.putBoolean("Fully Extended Back",
self.isFullyExtendedBack())
SmartDashboard.putBoolean("Fully Retracted Front",
self.isFullyRetractedFront())
SmartDashboard.putBoolean("Fully Retracted Back",
self.isFullyRetractedBack())
SmartDashboard.putBoolean("Front Over Ground",
self.isFrontOverGround())
SmartDashboard.putBoolean("Back Over Ground", self.isBackOverGround())
SmartDashboard.putNumber("self.state", self.state)
self.climbSpeedFront = SmartDashboard.getNumber(
"ClimbSpeedFront", self.climbSpeedFront)
self.climbSpeedBack = SmartDashboard.getNumber("ClimbSpeedBack",
self.climbSpeedBack)
self.kP = SmartDashboard.getNumber("Climber kP", self.kP)
self.backHold = SmartDashboard.getNumber("BackHold", self.backHold)
self.frontHold = SmartDashboard.getNumber("FrontHold", self.frontHold)
SmartDashboard.putNumber("Lean", self.getLean())
SmartDashboard.putNumber("FloorSensor", self.backUp())
SmartDashboard.putBoolean("Auto Climb Indicator",
self.autoClimbIndicator)
SmartDashboard.putNumber("Retract Start", self.frontRetractStart)
SmartDashboard.putNumber("Wheels Start", self.wheelsStart2)
def isFinished(self):
if self.numSamples > 0:
avgRate = sum(self.turnRateSamples) / self.ratePasses
self.logCounter += 1
if self.logCounter > 25:
self.logCounter = 0
if self.numSamples > 0:
print(
"CMD TurnToHeading isFinished - target: {}, current: {}, tolerance: {}, avgRate: {}, steadyRate: {}"
.format(self.target, robotmap.sensors.ahrs.getYaw(),
self.tolerance, avgRate, self.steadyRate))
else:
print(
"CMD TurnToHeading isFinished - target: {}, current: {}, tolerance: {}"
.format(self.target, robotmap.sensors.ahrs.getYaw(),
self.tolerance))
if self.numSamples > 0:
if self.useSmartDashboardValues:
SmartDashboard.putNumber("AvgRateYawDPS", avgRate)
if avgRate < self.steadyRate:
if math.fabs(self.target -
robotmap.sensors.ahrs.getYaw()) < self.tolerance:
# print("CMD TurnToHeading isFinished is True")
return True
else:
if math.fabs(self.target -
robotmap.sensors.ahrs.getYaw()) < self.tolerance:
# print("CMD TurnToHeading isFinished is True")
return True
return False
def teleopPeriodic(self):
if subsystems_enabled["limelight"]:
dash.putNumber("Trig Calculated Distance: ", self.limelight_component.get_distance_trig(
config_data['limelight']['target_height']
))
if subsystems_enabled["navx"]:
if self.driver.get_navx_reset():
self.navx_component.reset()
put_tuple("Displacement", self.navx_component.displacement, int)
put_tuple("Velocity", self.navx_component.velocity, int)
put_tuple("Acceleration", self.navx_component.acceleration, int)
if subsystems_enabled["shooter"]:
shooter_power = -self.driver.get_shooter_axis()
if self.driver.get_shooter_full():
shooter_power = -1
self.shooter_component.power = shooter_power
if subsystems_enabled["neo"]:
neo_kP = dash.getNumber("Shooter kP", 0)
neo_kF = dash.getNumber("Shooter kF", 0)
self.neo_component.update_pid(neo_kP, neo_kF)
neo_power = dash.getNumber("Target RPM", 0)
self.neo_component.set_rpm(neo_power)
if self.driver.get_update_telemetry():
dash.putNumber("Target RPM", 0)
dash.putNumber("Shooter kP", 0)
dash.putNumber("Shooter kF", 0)
dash.putNumber("Shooter RPM", self.neo_component.get_raw_velocity())
def iterate(self):
SmartDashboard.putNumber("Auto2 State", self.state)
if self.state == 0:
self.robot.harpoon.deploy_harpoon()
self.timer.reset()
if self.hab_level == HabLevel.LEVEL2:
self.state = 1
else:
self.state = 2
elif self.state == 1:
self.robot.drive.drive_straight(0.4)
if self.timer.get() > 1.0:
self.state = 2
elif self.state == 2:
self.robot.drive.drive_straight(0.0)
self.robot.lift.deploy_lift()
self.state = 3
elif self.state == 3:
if self.robot.lift.current_state == LiftState.LIFT_DEPLOYED:
self.state = 4
elif self.state == 4:
pass
def dashboardPeriodic(self):
self.climbSpeed = SmartDashboard.getNumber("ClimbSpeed",
self.climbSpeed)
self.kP = SmartDashboard.getNumber("Climber kP", self.kP)
self.backHold = SmartDashboard.getNumber("BackHold", self.backHold)
self.frontHold = SmartDashboard.getNumber("FrontHold", self.frontHold)
SmartDashboard.putNumber("Lean", self.getLean())
def setLoc(self, target):
target = abs(target)
if target > 1:
target = 1
elif target <= 0.1:
target = 0
SmartDashboard.putNumber("loc dfliusafusd", target)
self._liftPID.setSetpoint(target)
def follow_target(self, initial_call):
#print();
if initial_call:
self.limelight.switch_pipeline(0)
self.drivetrain.drive_to_limelight_target(0.2, 0.15)
#print(self.drivetrain.limelight_distance_pid.get_error())
SmartDashboard.putNumber("power: ", self.powertrain.power)
SmartDashboard.putNumber("rotation: ", self.powertrain.rotation)
def dumpInfo(self):
if isEnabled:
SmartDashboard.putNumber("Yaw: ", Sensors.__init__.NavX.getYaw())
SmartDashboard.putNumber("Pitch: ", Sensors.__init__.NavX.getPitch())
self.infont.putNumber("BatteryVoltage", self.ds.getBatteryVoltage())
color = self.ds.getAlliance()
self.infont.putNumber("color", color.ordinal())
def initialize(self):
timeout = 15
SmartDashboard.putNumber("Wrist Power Set", 0)
SmartDashboard.putNumber("Gravity Power", 0)
self.F = 0.25
SmartDashboard.putNumber("F Gain", self.F)
#self.angle = 0
#SmartDashboard.putNumber("angle", self.angle)
self.range = -1200
self.povPressed = False
self.maxVolts = 10
self.wristUpVolts = 4
self.wristDownVolts = -4
SmartDashboard.putNumber("Wrist Up Volts", self.wristUpVolts)
SmartDashboard.putNumber("Wrist Down Volts", self.wristDownVolts)
self.xbox = oi.getJoystick(2)
self.joystick0 = oi.getJoystick(0)
#below is the talon on the intake
self.intake = Talon(map.intake)
self.intake.setNeutralMode(2)
self.intake.configVoltageCompSaturation(self.maxVolts)
self.intake.configContinuousCurrentLimit(20,
timeout) #20 Amps per motor
self.intake.configPeakCurrentLimit(
30, timeout) #30 Amps during Peak Duration
self.intake.configPeakCurrentDuration(
100, timeout) #Peak Current for max 100 ms
self.intake.enableCurrentLimit(True)
#Talon motor object created
self.wrist = Talon(map.wrist)
if not wpilib.RobotBase.isSimulation():
self.wrist.configFactoryDefault()
self.wrist.setInverted(True)
self.wrist.configVoltageCompSaturation(self.maxVolts)
self.wrist.setNeutralMode(2)
self.wrist.configClearPositionOnLimitF(True)
self.wrist.configContinuousCurrentLimit(20,
timeout) #20 Amps per motor
self.wrist.configPeakCurrentLimit(
30, timeout) #30 Amps during Peak Duration
self.wrist.configPeakCurrentDuration(
100, timeout) #Peak Current for max 100 ms
self.wrist.enableCurrentLimit(True)
def periodic(self):
""" Periodic checks, sensor readings and dashboard updates. """
self.periodicCnt += 1
dashboardUpdate = ((self.periodicCnt % 10) == 0)
self.frontLeg.periodic(dashboardUpdate)
self.backLeg.periodic(dashboardUpdate)
if dashboardUpdate:
# Probably always want to see how much the robot is leaning
SmartDashboard.putNumber("Lean", self.getLean())
def run(self):
self.navx.resetAngle()
i = 0
while self.Robot.isAutonomous() and i < self.path.pointsLength:
print(i)
point = self.path.points[i]
''' create runnable start point methods here '''
if point.slowStop:
pass #slow down to point
self.drive.resetDistance()
self.scaledRuntime = RTS("scaledRuntime", 8)
''' create runnable start point methods here '''
#self.liftRTS.addTask(self.lift.periodic)
#self.liftRTS.start()
SmartDashboard.putNumber("break", 0)
SmartDashboard.putNumber("pointDist", point.distance)
overallDistance = self.getOverallDistance()
while self.Robot.isAutonomous(
) and point.distance > overallDistance:
overallDistance = self.getOverallDistance()
scaledLocation = overallDistance / point.distance
derivativesPresent = self.path.derivative1(
i + scaledLocation, 1)
navAngle = self.navx.getAngle() % 360
SmartDashboard.putNumber("navAngle", navAngle)
requiredAngle = math.atan2(derivativesPresent[1],
derivativesPresent[0])
requiredAngle = (requiredAngle if requiredAngle > 0 else
(2 * math.pi +
requiredAngle)) * 360 / (2 * math.pi)
turnSpeed = self.turnPID.getOutput(navAngle, requiredAngle)
SmartDashboard.putNumber("turnSpeed", turnSpeed)
if turnSpeed < 0: # turn left
turnSpeed *= -1
self.drive.tankDrive(AUTO_SPEED - turnSpeed, AUTO_SPEED)
else: # turn right
self.drive.tankDrive(AUTO_SPEED, AUTO_SPEED - turnSpeed)
#scaledRuntime.stop();
SmartDashboard.putNumber("break", 1)
#we should have a speed of zero here if slowStop is true and we are at our point
''' create runnable end point methods here '''
i += 1
self.drive.tankDrive(0, 0)
def outputToSmartDashboard(self):
"""Output values to the smart dashboard."""
lookahead = self.pursuit_points[self.last_lookahead_index].point
closest = self.pursuit_points[self.closest_point_index].point
Dash.putNumberArray("Lookahead Point", [lookahead.x, lookahead.y])
Dash.putNumber("Curvature", self.cur_curvature)
Dash.putNumberArray("Closes Point", [closest.x, closest.y])
Dash.putNumberArray(
"Target Velocities",
[self.target_velocities.x, self.target_velocities.y])
def initialize(self):
timeout = 15
SmartDashboard.putNumber("Wrist Power Set", 0)
self.lastMode = "unknown"
self.sb = []
self.targetPosUp = -300 #!!!!!
self.targetPosDown = -1500 #!!!!!
self.maxVolts = 10
self.simpleGain = 0.57
self.wristUpVolts = 5
self.wristDownVolts = 2
self.simpleGainGravity = 0.07
self.xbox = oi.getJoystick(2)
self.joystick0 = oi.getJoystick(0)
#below is the talon on the intake
self.motor = Talon(map.intake)
self.gPower = 0
self.input = 0
self.motor.configContinuousCurrentLimit(20,
timeout) #15 Amps per motor
self.motor.configPeakCurrentLimit(
30, timeout) #20 Amps during Peak Duration
self.motor.configPeakCurrentDuration(
100, timeout) #Peak Current for max 100 ms
self.motor.enableCurrentLimit(True)
#Talon motor object created
self.wrist = Talon(map.wrist)
if not wpilib.RobotBase.isSimulation():
self.wrist.configFactoryDefault()
self.wrist.configVoltageCompSaturation(self.maxVolts)
self.wrist.setInverted(True)
self.wrist.setNeutralMode(2)
self.motor.setNeutralMode(2)
self.motor.configVoltageCompSaturation(self.maxVolts)
self.wrist.configClearPositionOnLimitF(True)
#MOTION MAGIC CONFIG
self.loops = 0
self.timesInMotionMagic = 0
#self.wrist.setSelectedSensorPosition(0, self.kPIDLoopIdx, self.kTimeoutMs)
[self.kP, self.kI, self.kD] = [0, 0, 0]
cargoController = PIDController(self.kP,
self.kI,
self.kD,
source=self.getAngle,
output=self.__setGravity__)
self.cargoController = cargoController
self.cargoController.disable()
def execute(self):
self.timestamp = self.timeSinceInitialized()
dt = self.timestamp - self.last_timestamp
self.last_timestamp = self.timestamp
output = self.controller.update(self.odemetry.getAngle(), dt)
if abs(output) < Constants.TURN_TO_ANGLE_MIN_OUTPUT:
output = math.copysign(output, Constants.TURN_TO_ANGLE_MIN_OUTPUT)
Dash.putNumber("Turn To Angle Output", output)
Dash.putNumber("Turn To Angle Error",
units.radiansToDegrees(self.controller.cur_error))
self.drive.setPercentOutput(-output, output)
def execute(self):
self.timestamp = self.timeSinceInitialized()
dt = self.timestamp - self.last_timestamp
self.last_timestamp = self.timestamp
output = self.controller.update(self.odemetry.getAngle(), dt)
# print("Output: {}, Error: {}".format(
# output, units.radiansToDegrees(self.controller.cur_error)))
Dash.putNumber("Turn To Angle Output", output)
Dash.putNumber("Turn To Angle Error",
units.radiansToDegrees(self.controller.cur_error))
self.drive.setPercentOutput(-output, output)
def execute(self):
pov = oi.OI().driver.getPOV(self.pov_port)
if pov != -1 and not isinstance(self.drive.currentCommand,
turntoangle.TurnToAngle):
pov = angles.positiveAngleToMixedAngle(pov)
pov //= 45
to_turn_to = SnapListener.POV_ARRAY[int(pov)]
to_turn_to = int(to_turn_to)
Dash.putNumber("Snap Turning Target", to_turn_to)
# logging.debug("Starting snap to angle")
turntoangle.TurnToAngle(to_turn_to).start()
def dashboardPeriodic(self):
#commented out some values. DON'T DELETE THESE VALUES
#SmartDashboard.putNumber("Wrist Position", self.wrist.getQuadraturePosition())
SmartDashboard.putNumber("Wrist Angle", self.getAngle())
#SmartDashboard.putNumber("Output", self.out)
#self.F = SmartDashboard.getNumber("F Gain", 0)
#self.wristUpVolts = SmartDashboard.getNumber("Wrist Up Volts", 0)
#self.wristDownVolts = SmartDashboard.getNumber("Wrist Down Volts", 0)
SmartDashboard.putBoolean("Limit Switch",
self.wrist.isFwdLimitSwitchClosed())
SmartDashboard.putBoolean("Limit Switch Reverse",
self.wrist.isRevLimitSwitchClosed())
def robotInit(self):
self.neo0: CANSparkMax = CANSparkMax(3, rev.MotorType.kBrushless)
self.neo1: CANSparkMax = CANSparkMax(11, rev.MotorType.kBrushless)
neos = wpilib.SpeedControllerGroup(self.neo0, self.neo1)
self.neo0.clearFaults()
self.neo1.clearFaults()
sd.putNumber("Neo Power", 0)
self.neo0.setOpenLoopRampRate(5)
self.neo1.setOpenLoopRampRate(5)
def __init__(self, robot):
self.robot = robot
if not robot.debug:
self.initialize_joysitics()
else:
self.stick = wpilib.Joystick(0)
# SmartDashboard Buttons - test some autonomous commands here
SmartDashboard.putNumber("Auto Distance", 10)
SmartDashboard.putNumber("Auto Rotation", 10)
SmartDashboard.putData("Drive Forward", AutonomousDrive(robot, setpoint=None, control_type='position', timeout=6))
SmartDashboard.putData("Rotate X", AutonomousRotate(robot, setpoint=None, timeout=6))
SmartDashboard.putData("Update Pos PIDs", (UpdatePIDs(robot, factor=1, from_dashboard='position')))
SmartDashboard.putData("Update Vel PIDs", (UpdatePIDs(robot, factor=1, from_dashboard='velocity')))
def dashboardPeriodic(self):
#SmartDashboard.putNumber("xError", self.getXError())
#SmartDashboard.putNumber("yError", self.getYError())
#SmartDashboard.putNumber('Camtran', self.camtran[2])
#SmartDashboard.putNumber("Distance",self.getDistance())
#SmartDashboard.putNumber("thor", self.thor)
#SmartDashboard.putNumber("Angle1", self.tx)
#SmartDashboard.putNumber("Angle2", self.getAngle2())
#SmartDashboard.putNumber("NavXAngle", self.robot.drive.getAngle())
SmartDashboard.putNumberArray("Path", self.getPath())
#SmartDashboard.putNumber("dist1", self.dist1)
#SmartDashboard.putNumber("dist2", self.dist2)
SmartDashboard.putNumber("aligned", self.aligned)
SmartDashboard.putNumber("align", self.align)
def iterate(self, robot_mode, pilot_stick, copilot_stick):
if robot_mode == RobotMode.TEST:
self.test_mode()
return
if pilot_stick.Back().get():
self.stow_harpoon()
if pilot_stick.Start().get():
self.deploy_harpoon()
self.iterate_state_machine()
SmartDashboard.putString("Harpoon State", self.current_state.name)
SmartDashboard.putNumber("IRVolts", self.ir_harpoon.getVoltage())
def test_mode(self):
SmartDashboard.putNumber("IRVolts", self.ir_harpoon.getVoltage())
if self.test_harpoon_outside.getSelected() == 1:
self.harpoon_outside_extend.set(False)
self.harpoon_outside_retract.set(True)
else:
self.harpoon_outside_extend.set(True)
self.harpoon_outside_retract.set(False)
if self.test_harpoon_center.getSelected() == 1:
self.harpoon_center_extend.set(False)
self.harpoon_center_retract.set(True)
else:
self.harpoon_center_extend.set(True)
self.harpoon_center_retract.set(False)
def initialize(self):
timeout = 15
self.wristPowerSet = 0
SmartDashboard.putNumber("Wrist Power Set", self.wristPowerSet)
self.maxVolts = 10
self.wristUpVolts = 4
self.wristDownVolts = -4
self.xbox = oi.getJoystick(2)
self.out = 0
#below is the talon on the intake
self.intake = Talon(map.intake)
self.intake.setNeutralMode(2)
self.intake.configVoltageCompSaturation(self.maxVolts)
self.intake.configContinuousCurrentLimit(20,
timeout) #20 Amps per motor
self.intake.configPeakCurrentLimit(
30, timeout) #30 Amps during Peak Duration
self.intake.configPeakCurrentDuration(
100, timeout) #Peak Current for max 100 ms
self.intake.enableCurrentLimit(True)
#Talon motor object created
self.wrist = Talon(map.wrist)
if not wpilib.RobotBase.isSimulation():
self.wrist.configFactoryDefault()
self.wrist.setInverted(True)
self.wrist.configVoltageCompSaturation(self.maxVolts)
self.wrist.setNeutralMode(2)
self.wrist.configClearPositionOnLimitF(True)
self.wrist.configContinuousCurrentLimit(20,
timeout) #20 Amps per motor
self.wrist.configPeakCurrentLimit(
30, timeout) #30 Amps during Peak Duration
self.wrist.configPeakCurrentDuration(
100, timeout) #Peak Current for max 100 ms
self.wrist.enableCurrentLimit(True)
self.bottomSwitch = DI(map.bottomSwitch)
self.topSwitch = DI(map.topSwitch)
def execute(self):
"""Called repeatedly when this Command is scheduled to run"""
self.error = self.setpoint - (self.robot.navigation.get_angle() -
self.start_angle)
self.power = self.kp * self.error + self.kf * math.copysign(
1, self.error) + self.kd * (self.error - self.prev_error) / 0.02
self.prev_error = self.error
if self.power > 0:
self.power = min(self.max_power, self.power)
else:
self.power = max(-self.max_power, self.power)
#self.robot.drivetrain.smooth_drive(0,-self.power)
self.robot.drivetrain.smooth_drive(thrust=0,
strafe=0,
twist=self.power)
SmartDashboard.putNumber("error", self.error)
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 updateState(self, timestamp):
"""Use odometry to update the robot state."""
self.timestamp = timestamp
#delta_time = self.timestamp-self.last_timestamp
# update angle
self.pose.angle = self.getAngle()
# update x and y positions
self.pose.x += self.getDistanceDelta()*math.cos(self.pose.angle)
Dash.putNumber("Pos Y Test", self.getDistanceDelta()
* math.sin(self.pose.angle))
self.pose.y += self.getDistanceDelta()*math.sin(self.pose.angle)
# update last distances for next periodic
self.last_left_encoder_distance = self.drive.getDistanceInchesLeft()
self.last_right_encoder_distance = self.drive.getDistanceInchesRight()
# update last angle and timestamp for next periodic
self.last_angle = self.pose.angle
self.last_timestamp = self.timestamp
def updateDashboard(self):
# TODO: additional items?
SmartDashboard.putNumber("IMU heading", self.getCurrentHeading())
SmartDashboard.putNumber("IMU calibration", self.imu.getCalibration())
