def pytest_runtest_teardown():
import networktables
networktables.NetworkTables.shutdown()
from wpilib import SmartDashboard
SmartDashboard._reset()
def __init__(self, robot):
self.robot = robot
self.table = SmartDashboard.getTable().getSubTable(self.k_TableName)
# controlled via driverstation
self.AutoAimEnabled = False
self.TargetHigh = True
# controlled via remote vision server
self.FPS = 0
self.TargetAcquired = False
self.TargetX = 0
self.TargetY = 0
# our listener is "bound" to self, called when we receive notifications
# from the vision server.
def _listener(table, key, value, isNew):
if key == "AutoAimEnabled":
self.AutoAimEnabled = value
elif key == "FPS":
self.FPS = value
elif key == "TargetAcquired":
self.TargetAcquired = value
elif key == "TargetX":
self.TargetX = value
elif key == "TargetY":
self.TargetY = value
elif key == "~TYPE~":
pass
else:
self.robot.warning("Unexpected Vision key: " + key)
self.table.addTableListener(_listener)
def updateDashboard(self):
SmartDashboard.putBoolean("Portcullis Upper Right Limit Switch",
self.rightAtTop())
SmartDashboard.putBoolean("Portcullis Upper Left Limit Switch",
self.leftAtTop())
SmartDashboard.putBoolean("Portcullis Lower Right Limit Switch",
self.rightAtBottom())
SmartDashboard.putBoolean("Portcullis Lower Left Limit Switch",
self.leftAtBottom())
def __init__(self, robot):
self.joystick = wpilib.Joystick(0)
JoystickButton(self.joystick, 12).whenPressed(LowGoal(robot))
JoystickButton(self.joystick, 10).whenPressed(Collect(robot))
JoystickButton(self.joystick, 11).whenPressed(SetPivotSetpoint(robot, Pivot.SHOOT))
JoystickButton(self.joystick, 9).whenPressed(SetPivotSetpoint(robot, Pivot.SHOOT_NEAR))
DoubleButton(self.joystick, 2, 3).whenActive(Shoot(robot))
#SmartDashboard Buttons
SmartDashboard.putData("Drive Forward", DriveForward(robot,2.25))
SmartDashboard.putData("Drive Backward", DriveForward(robot,-2.25))
SmartDashboard.putData("Start Rollers", SetCollectionSpeed(robot,Collector.FORWARD))
SmartDashboard.putData("Stop Rollers", SetCollectionSpeed(robot,Collector.STOP))
SmartDashboard.putData("Reverse Rollers", SetCollectionSpeed(robot,Collector.REVERSE))
def robotInit(self):
#self.neo: CANSparkMax = CANSparkMax(5, rev.MotorType.kBrushless)
self.neo: Spark = Spark(0)
sd.putNumber("Neo Power", 0.5)
def initializeBoolean(label: str, defaultValue: bool) -> bool:
value: bool = SmartDashboard.getBoolean(label, defaultValue)
SmartDashboard.putBoolean(label, value)
return value
def getNumber(self, key, defVal):
val = SmartDashboard.getNumber(key, None)
if val == None:
val = defVal
SmartDashboard.putNumber(key, val)
return val
def robotPeriodic(self):
#self.limelight.readLimelightData()
if (self.dashboard): self.updateDashboardPeriodic()
SmartDashboard.putBoolean("Passed Hatch", self.drive.isCargoPassed())
def teleopPeriodic(self):
a = 0
while self.isOperatorControl() and self.isEnabled():
self.controls.periodic()
self.limelight.mutiPipeline()
self.intake.periodic()
self.lift.periodic()
b = self.lift.getEncoderVal()
if a < b:
a = b
SmartDashboard.putNumber("pipeline id",
self.limelight.getPipeline())
SmartDashboard.putBoolean("inake hasCube", self.intake.hasCube())
#drive.periodic()
#SmartDashboard.putNumber("liftRTS hz", self.liftRTS.getHz())
SmartDashboard.putNumber("0", self.a0.getAverageVoltage())
SmartDashboard.putNumber("1", self.a1.getAverageVoltage())
SmartDashboard.putNumber("2", self.a2.getAverageVoltage())
SmartDashboard.putNumber("3", self.a3.getAverageVoltage())
SmartDashboard.putNumber("Lift", a)
SmartDashboard.putNumber("Drive Right Dist",
self.drive.getRightDistance())
SmartDashboard.putNumber("Drive Left Dist",
self.drive.getLeftDistance())
SmartDashboard.putNumber("pitch", self.navx.getPitch())
def teleopPeriodic(self):
try:
if self.robot_mode == RobotModes.MANUAL_DRIVE:
if robotmap.drivetrain_enabled:
if self.subsystem_drivetrain.drive_mode == drivetrain.DrivetrainMode.MANUAL_DRIVE_CURVATURE:
self.subsystem_drivetrain.curvature_drive(
self.oi.controller_driver.getY()**3,
-self.oi.controller_driver.getX()**3)
if self.subsystem_drivetrain.drive_mode == drivetrain.DrivetrainMode.MANUAL_DRIVE_ARCADE:
self.subsystem_drivetrain.arcade_drive(
self.oi.controller_driver.getY()**3,
-self.oi.controller_driver.getX()**3)
if self.navx.isConnected(
) and self.oi.check_drivetrain_straight(
self.oi.controller_driver.getX(),
self.oi.controller_driver.getY()):
self.subsystem_drivetrain.drive_to_angle(
self.oi.controller_driver.getY()**3)
elif self.subsystem_drivetrain.drive_mode == drivetrain.DrivetrainMode.ASSIST_DRIVE_ARCADE:
self.subsystem_drivetrain.drive_mode = drivetrain.DrivetrainMode.MANUAL_DRIVE_CURVATURE
if robotmap.shooter_enabled:
target_shooter_speed = dash.getNumber(
"target shooter velocity", 0)
shooter_enabled = dash.getBoolean("shooter enabled", False)
if shooter_enabled:
self.subsystem_shooter.enable_at_speed(
self.oi.controller_driver.getY(
wpilib.XboxController.Hand.kLeft))
else:
self.subsystem_shooter.disable()
dash.putNumber("shooter speed error",
self.subsystem_shooter.get_error())
if robotmap.loader_enabled:
if self.oi.controller_driver.getPOV() == 0: # up on dpad
self.subsystem_loader.position = LoaderPosition.UP
if self.oi.controller_driver.getPOV(
) == 180: # down on dpad
self.subsystem_loader.position = LoaderPosition.DOWN
if self.oi.controller_driver.getXButtonPressed():
self.subsystem_loader.enabled = not self.subsystem_loader.enabled
if self.oi.controller_driver.getBButtonPressed():
self.robot_mode = RobotModes.AUTO_TARGETTING
elif self.robot_mode == RobotModes.AUTO_TARGETTING:
# this is the control loop that will deal with automatically shooting into a target
# inputs: distance from target, horizontal offset from target
# outputs: speed of shooter, turning speed of robot, linear speed of robot
# the speed of the shooter is controlled by the distance
# the linear speed of the robot is controlled by the distance
# the turning speed of the robot is controlled by the horizontal offset of the robot
if not robotmap.limelight_enabled:
# we can't do anything in this case.
self.robot_mode = RobotModes.MANUAL_DRIVE
distance_to_target = self.limelight.get_distance_trig()
x_offset = self.limelight.get_horizontal_angle_offset()
drivetrain_on_target = True
if robotmap.drivetrain_enabled:
# use proportional techniques to make the robot drive into a good position to shoot
angular_error = 0
distance_error = 0
if abs(x_offset
) > robotmap.auto_targetting_alignment_tolerance:
drivetrain_on_target = False
angular_error = 0 - x_offset
if distance_to_target > robotmap.auto_targetting_max_distance:
drivetrain_on_target = False
distance_error = distance_to_target - robotmap.auto_targetting_max_distance
if distance_to_target < robotmap.auto_targetting_min_distance:
drivetrain_on_target = False
distance_to_target = distance_to_target - robotmap.auto_targetting_min_distance
drivetrain_rotation = drivetrain.DrivetrainConstants.K_PROPORTIONAL_TURNING * angular_error
drivetrain_power = drivetrain.DrivetrainConstants.K_PROPORTIONAL_DRIVING * distance_error
self.subsystem_drivetrain.arcade_drive(
drivetrain_power, drivetrain_rotation)
if robotmap.shooter_enabled:
# set the shooter to an appropriate speed if the drivetrain is on target
if drivetrain_on_target:
self.subsystem_shooter.enabled = True
self.subsystem_shooter.shoot_from_distance(
distance_to_target, angle=45)
else:
self.subsystem_shooter.enabled = False
if robotmap.loader_enabled:
# TODO: we don't have a working loader right now so (logically) there's no code here
pass
if self.oi.controller_driver.getBButtonPressed():
self.robot_mode = RobotModes.MANUAL_DRIVE
if robotmap.limelight_enabled:
dash.putString("distance to target",
self.limelight.get_distance_trig())
dash.putString("horizontal angle to target",
self.limelight.get_horizontal_angle_offset())
dash.putString("vertical angle to target",
self.limelight.get_vertical_angle_offset())
dash.putNumber("PID Output",
self.subsystem_drivetrain.turn_pid.get())
except:
self.onException()
def dashboardInit(self):
SmartDashboard.putData("test command", LimeLightAutoAlign(self.robot))
SmartDashboard.putData("Turn Angle", TurnAngle())
SmartDashboard.putData("Drive Combined", DriveStraight())
def dashboardPeriodic(self):
SmartDashboard.putNumber("Left Counts", self.leftEncoder.get())
SmartDashboard.putNumber("Left Distance",
self.leftEncoder.getDistance())
SmartDashboard.putNumber("Right Counts", self.rightEncoder.get())
SmartDashboard.putNumber("Right Distance",
self.rightEncoder.getDistance())
SmartDashboard.putNumber("DT_DistanceAvg", self.getAvgDistance())
SmartDashboard.putNumber("DT_DistanceLeft", self.getDistance()[0])
SmartDashboard.putNumber("DT_DistanceRight", self.getDistance()[1])
SmartDashboard.putNumber("DT_Angle", self.getAngle())
SmartDashboard.putNumber("DT_PowerLeft", self.left.get())
SmartDashboard.putNumber("DT_PowerRight", self.right.get())
SmartDashboard.putNumber("DT_VelocityLeft", self.getVelocity()[0])
SmartDashboard.putNumber("DT_VelocityRight", self.getVelocity()[1])
SmartDashboard.putNumber("DT_CounLeft", self.getRaw()[0])
SmartDashboard.putNumber("DT_CountRight", self.getRaw()[1])
SmartDashboard.putNumber("angle correction", self.anglePID)
SmartDashboard.putNumber("DriveAmps", self.getOutputCurrent())
def put_tuple(key, t, op=lambda x: x):
dash.putString(key, ", ".join([str(op(i)) for i in t]))
def put_pid(key, pid: PIDValue):
dash.putNumber(f"{key}_kP", pid.p)
dash.putNumber(f"{key}_kI", pid.i)
dash.putNumber(f"{key}_kD", pid.d)
def push(self):
dash.putNumber(self.key, self.value)
def get(self, default_value=None):
if default_value == None:
return dash.getNumber(self.key, self.value)
else:
return dash.getNumber(self.key, default_value)
def set(self, value):
self.value = value
dash.putNumber(self.key, value)
def get_pid(key):
p = dash.getNumber(f"{key}_kP", 0)
i = dash.getNumber(f"{key}_kI", 0)
d = dash.getNumber(f"{key}_kD", 0)
return PIDValue(p, i, d)
def __init__(self, robot):
super().__init__('Drive')
SmartDashboard.putNumber("DriveStraight_P", 0.075)
SmartDashboard.putNumber("DriveStraight_I", 0.0)
SmartDashboard.putNumber("DriveStraight_D", 0.42)
# OLD GAINS 0.075, 0, 0.42
SmartDashboard.putNumber("DriveAngle_P", 0.009)
SmartDashboard.putNumber("DriveAngle_I", 0.0)
SmartDashboard.putNumber("DriveAngle_D", 0.025)
SmartDashboard.putNumber("DriveStraightAngle_P", 0.025)
SmartDashboard.putNumber("DriveStraightAngle_I", 0.0)
SmartDashboard.putNumber("DriveStraightAngle_D", 0.01)
self.robot = robot
self.lime = self.robot.limelight
self.nominalPID = 0.15
self.nominalPIDAngle = 0.22 # 0.11 - v2
self.switch = False
self.preferences = Preferences.getInstance()
timeout = 0
TalonLeft = Talon(map.driveLeft1)
TalonRight = Talon(map.driveRight1)
leftInverted = True
rightInverted = False
TalonLeft.setInverted(leftInverted)
TalonRight.setInverted(rightInverted)
VictorLeft1 = Victor(map.driveLeft2)
VictorLeft2 = Victor(map.driveLeft3)
VictorLeft1.follow(TalonLeft)
VictorLeft2.follow(TalonLeft)
VictorRight1 = Victor(map.driveRight2)
VictorRight2 = Victor(map.driveRight3)
VictorRight1.follow(TalonRight)
VictorRight2.follow(TalonRight)
for motor in [VictorLeft1, VictorLeft2]:
motor.clearStickyFaults(timeout)
motor.setSafetyEnabled(False)
#motor.setExpiration(2 * self.robot.period)
motor.setInverted(leftInverted)
for motor in [VictorRight1, VictorRight2]:
motor.clearStickyFaults(timeout)
motor.setSafetyEnabled(False)
#motor.setExpiration(2 * self.robot.period)
motor.setInverted(rightInverted)
for motor in [TalonLeft, TalonRight]:
motor.setSafetyEnabled(False)
#motor.setExpiration(2 * self.robot.period)
motor.clearStickyFaults(timeout) #Clears sticky faults
motor.configContinuousCurrentLimit(40, timeout) #15 Amps per motor
motor.configPeakCurrentLimit(
70, timeout) #20 Amps during Peak Duration
motor.configPeakCurrentDuration(
300, timeout) #Peak Current for max 100 ms
motor.enableCurrentLimit(True)
motor.configVoltageCompSaturation(12,
timeout) #Sets saturation value
motor.enableVoltageCompensation(
True) #Compensates for lower voltages
motor.configOpenLoopRamp(0.2,
timeout) #number of seconds from 0 to 1
self.left = TalonLeft
self.right = TalonRight
self.navx = navx.AHRS.create_spi()
self.leftEncoder = Encoder(map.leftEncoder[0], map.leftEncoder[1])
self.leftEncoder.setDistancePerPulse(self.leftConv)
self.leftEncoder.setSamplesToAverage(10)
self.rightEncoder = Encoder(map.rightEncoder[0], map.rightEncoder[1])
self.rightEncoder.setDistancePerPulse(self.rightConv)
self.rightEncoder.setSamplesToAverage(10)
#PID for Drive
self.TolDist = 0.1 #feet
[kP, kI, kD, kF] = [0.027, 0.00, 0.20, 0.00]
if wpilib.RobotBase.isSimulation():
[kP, kI, kD, kF] = [0.25, 0.00, 1.00, 0.00]
distController = PIDController(kP,
kI,
kD,
kF,
source=self.__getDistance__,
output=self.__setDistance__)
distController.setInputRange(0, 50) #feet
distController.setOutputRange(-0.6, 0.6)
distController.setAbsoluteTolerance(self.TolDist)
distController.setContinuous(False)
self.distController = distController
self.distController.disable()
'''PID for Angle'''
self.TolAngle = 2 #degrees
[kP, kI, kD, kF] = [0.025, 0.00, 0.01, 0.00]
if RobotBase.isSimulation():
[kP, kI, kD, kF] = [0.005, 0.0, 0.01, 0.00]
angleController = PIDController(kP,
kI,
kD,
kF,
source=self.__getAngle__,
output=self.__setAngle__)
angleController.setInputRange(-180, 180) #degrees
angleController.setOutputRange(-0.5, 0.5)
angleController.setAbsoluteTolerance(self.TolAngle)
angleController.setContinuous(True)
self.angleController = angleController
self.angleController.disable()
def initialize(self):
"""Called just before this Command runs the first time."""
self.start_time = round(Timer.getFPGATimestamp()-self.robot.enabled_time, 1)
print("\n" + f"** Started {self.getName()} at {self.start_time} s **", flush=True)
SmartDashboard.putString("alert", f"** Started {self.getName()} at {self.start_time - self.robot.enabled_time:2.2f} s **")
def disabledPeriodic(self):
Scheduler.getInstance().run()
#Tanklift data
SmartDashboard.putBoolean("Front IR", subsystems.drivelift.frontIR.state)
SmartDashboard.putBoolean("Back IR", subsystems.drivelift.backIR.state)
#Elevator Data
SmartDashboard.putNumber("Elevator Ticks", subsystems.elevator.elevatorEncoder.get())
SmartDashboard.putNumber("Elevator Height", subsystems.elevator.elevatorEncoder.getDistance())
SmartDashboard.putNumber("Limit Switch", subsystems.elevator.btmLimitSwitch.get())
#Game Objective States
SmartDashboard.putBoolean("Ramp Tog", subsystems.ramp.rampToggle)
SmartDashboard.putBoolean("Hatch Tog", subsystems.hatchgrab.hatchToggle)
SmartDashboard.putBoolean("Suction Extend", subsystems.hatchgrab.hatchExtendToggle)
SmartDashboard.putBoolean("Cargo Tog", subsystems.cargograb.cargoToggle)
SmartDashboard.putNumber("R Servo Angle", subsystems.cargograb.rightServo.getAngle())
SmartDashboard.putNumber("L Servo Angle", subsystems.cargograb.leftServo.getAngle())
SmartDashboard.putBoolean("Relay State", subsystems.hatchgrab.hatchGrabExtendSol.get())
#Driveline data
SmartDashboard.putNumber("Left Speed", subsystems.driveline.leftSpdCtrl.get())
SmartDashboard.putNumber("Right Speed", subsystems.driveline.rightSpdCtrl.get())
SmartDashboard.putNumber("Right Sensors", subsystems.driveline.rSensor.getVoltage())
SmartDashboard.putNumber("Left Sensors", subsystems.driveline.lSensor.getVoltage())
SmartDashboard.putNumber("Sensor Dif", subsystems.driveline.lineSensorCompare)
def on_iteration(self, time_elapsed):
"""
Called on each iteration of the control loop for both auton and tele
"""
# TODO Since this is the same code as teleop, there should be some way to consolidate it into one function
# TODO Individual try catches
try:
# DRIVETRAIN
if self.drivetrain.drive_mode == DriveModes.ARCADEDRIVE:
self.drivetrain.arcade_drive(
self.oi.drivetrain_curve(
self.oi.driver.getY(self.oi.driver.Hand.kLeft)),
-self.oi.driver.getX(self.oi.driver.Hand.kRight))
elif self.drivetrain.drive_mode == DriveModes.TANKDRIVE:
self.drivetrain.tank_drive(
self.oi.drivetrain_curve(
self.oi.driver.getY(self.oi.driver.Hand.kLeft)),
self.oi.drivetrain_curve(
self.oi.driver.getY(self.oi.driver.Hand.kRight)))
if self.navx.isConnected() and self.oi.check_drivetrain_straight(
self.oi.driver.getX(self.oi.driver.Hand.kRight),
self.oi.driver.getY(self.oi.driver.Hand.kLeft)):
self.drivetrain.drive_straight(
self.oi.drivetrain_curve(
self.oi.driver.getY(self.oi.driver.Hand.kLeft)))
elif self.drivetrain.drive_mode == DriveModes.DRIVETOANGLE:
self.drivetrain.drive_mode = DriveModes.ARCADEDRIVE
if self.oi.get_drivetrain_shift():
self.drivetrain_mechanism.toggle_shift()
# if self.oi.get_drive_mode_switch():
# self.drivetrain.toggle_mode()
# HATCHES
if self.oi.get_hatch_grabber():
self.hatch_mechanism.toggle_grab()
if self.oi.get_hatch_rack():
self.hatch_mechanism.toggle_extended()
# CARGO
cargo_power = self.oi.process_deadzone(
self.oi.sysop.getY(self.oi.sysop.Hand.kLeft),
robotmap.Tuning.CargoMechanism.deadzone)
if cargo_power > 0:
self.cargo_mechanism.raise_lift(cargo_power)
if cargo_power < 0:
self.cargo_mechanism.lower_lift(-cargo_power)
if self.oi.get_cargo_lock():
self.cargo_mechanism.toggle_lock()
# SENSORS
if self.oi.get_camera_switch():
self.current_camera = 0 if self.current_camera == 1 else 1
if self.oi.get_calibrate_pressure():
self.pressure_sensor.calibrate_pressure()
# SMARTDASHBOARD
dash.putNumber("Calibrated Pressure: ",
self.pressure_sensor.get_pressure())
dash.putString("Grabber: ", self.hatch_grabber.state)
dash.putString("Rack: ", self.hatch_rack.state)
dash.putString("Drive Mode: ", self.drivetrain.drive_mode)
except:
self.onException()
def setMotionMagicLeft(self, setpoint):
SmartDashboard.putNumber("setpoint_left_units",
self.inchesToUnits(setpoint))
self.left_master.set(ControlMode.MotionMagic,
self.inchesToUnits(setpoint))
def outputToSmartDashboard(self):
Dash.putNumber("Left Encoder Ticks", self.drive.getDistanceTicksLeft())
Dash.putNumber("Right Encoder Ticks",
self.drive.getDistanceTicksRight())
Dash.putNumber("Left Encoder Inches",
self.drive.getDistanceInchesLeft())
Dash.putNumber("Right Encoder Inches",
self.drive.getDistanceInchesRight())
Dash.putNumber("Pos X", self.pose.pos.x)
Dash.putNumber("Pos Y", self.pose.pos.y)
Dash.putNumber("Angle", self.getAngle())
def teleopPeriodic(self):
try:
# DRIVETRAIN
if self.drivetrain.drive_mode == DriveModes.ARCADEDRIVE:
self.drivetrain.arcade_drive(
self.oi.drivetrain_curve(
self.oi.driver.getY(self.oi.driver.Hand.kLeft)),
-self.oi.driver.getX(self.oi.driver.Hand.kRight))
elif self.drivetrain.drive_mode == DriveModes.TANKDRIVE:
self.drivetrain.tank_drive(
self.oi.drivetrain_curve(
self.oi.driver.getY(self.oi.driver.Hand.kLeft)),
self.oi.drivetrain_curve(
self.oi.driver.getY(self.oi.driver.Hand.kRight)))
if self.navx.isConnected() and self.oi.check_drivetrain_straight(
self.oi.driver.getX(self.oi.driver.Hand.kRight),
self.oi.driver.getY(self.oi.driver.Hand.kLeft)):
self.drivetrain.drive_straight(
self.oi.drivetrain_curve(
self.oi.driver.getY(self.oi.driver.Hand.kLeft)))
elif self.drivetrain.drive_mode == DriveModes.DRIVETOANGLE:
self.drivetrain.drive_mode = DriveModes.ARCADEDRIVE
if self.oi.get_drivetrain_shift():
self.drivetrain_mechanism.toggle_shift()
# if self.oi.get_drive_mode_switch():
# self.drivetrain.toggle_mode()
# HATCHES
if self.oi.get_hatch_grabber():
self.hatch_mechanism.toggle_grab()
if self.oi.get_hatch_rack():
self.hatch_mechanism.toggle_extended()
# CARGO
cargo_power = self.oi.process_deadzone(
self.oi.sysop.getY(self.oi.sysop.Hand.kLeft),
robotmap.Tuning.CargoMechanism.deadzone)
if cargo_power > 0:
self.cargo_mechanism.raise_lift(cargo_power)
if cargo_power < 0:
self.cargo_mechanism.lower_lift(-cargo_power)
if self.oi.get_cargo_lock():
self.cargo_mechanism.toggle_lock()
# SENSORS
if self.oi.get_camera_switch():
self.current_camera = 0 if self.current_camera == 1 else 1
if self.oi.get_calibrate_pressure():
self.pressure_sensor.calibrate_pressure()
# SMARTDASHBOARD
dash.putNumber("Calibrated Pressure: ",
self.pressure_sensor.get_pressure())
dash.putString("Grabber: ", self.hatch_grabber.state)
dash.putString("Rack: ", self.hatch_rack.state)
dash.putString("Drive Mode: ", self.drivetrain.drive_mode)
self.camera_table.putString("Selected Camera",
f"{self.current_camera}")
except:
self.onException()
def dashboardPeriodic(self):
SmartDashboard.putNumber("Cargomech Wrist Output", self.out)
self.wristPowerSet = SmartDashboard.getNumber("Wrist Power Set", 0)
def execute(self):
pid_z = 0
if self.hold_heading:
if self.momentum and abs(self.bno055.getHeadingRate()) < 0.005:
self.momentum = False
if self.vz not in [0.0, None]:
self.momentum = True
if not self.momentum:
pid_z = self.heading_pid_out.output
else:
self.set_heading_sp_current()
input_vz = 0
if self.vz is not None:
input_vz = self.vz
vz = input_vz + pid_z
for module in self.modules:
module_dist = math.hypot(module.x_pos, module.y_pos)
module_angle = math.atan2(module.y_pos, module.x_pos)
# Calculate the additional vx and vy components for this module
# required to achieve our desired angular velocity
vz_x = -module_dist*vz*math.sin(module_angle)
vz_y = module_dist*vz*math.cos(module_angle)
# TODO: re enable this and test field-oriented mode
if self.field_oriented:
angle = self.bno055.getAngle()
vx, vy = self.robot_orient(self.vx, self.vy, angle)
else:
vx, vy = self.vx, self.vy
module.set_velocity(vx+vz_x, vy+vz_y)
odometry_outputs = np.zeros((8, 1))
velocity_outputs = np.zeros((8, 1))
heading = self.bno055.getAngle()
heading_delta = constrain_angle(heading - self.last_heading)
heading_adjustment_factor = 1
adjusted_heading = heading - heading_adjustment_factor * heading_delta
timestep_average_heading = adjusted_heading - heading_delta / 2
for i, module in enumerate(self.modules):
odometry_x, odometry_y = module.get_cartesian_delta()
velocity_x, velocity_y = module.get_cartesian_vel()
odometry_outputs[i*2, 0] = odometry_x
odometry_outputs[i*2+1, 0] = odometry_y
velocity_outputs[i*2, 0] = velocity_x
velocity_outputs[i*2+1, 0] = velocity_y
module.reset_encoder_delta()
v_x, v_y, v_z = self.robot_movement_from_odometry(velocity_outputs, heading)
delta_x, delta_y, delta_z = self.robot_movement_from_odometry(odometry_outputs, heading, z_vel=v_z)
self.odometry_x += delta_x
self.odometry_y += delta_y
self.odometry_x_vel = v_x
self.odometry_y_vel = v_y
self.odometry_z_vel = v_z
SmartDashboard.putNumber('module_a_speed', self.modules[0].current_speed)
SmartDashboard.putNumber('module_b_speed', self.modules[1].current_speed)
SmartDashboard.putNumber('module_c_speed', self.modules[2].current_speed)
SmartDashboard.putNumber('module_d_speed', self.modules[3].current_speed)
SmartDashboard.putNumber('module_a_pos', self.modules[0].current_measured_azimuth)
SmartDashboard.putNumber('module_b_pos', self.modules[1].current_measured_azimuth)
SmartDashboard.putNumber('module_c_pos', self.modules[2].current_measured_azimuth)
SmartDashboard.putNumber('module_d_pos', self.modules[3].current_measured_azimuth)
SmartDashboard.putNumber('odometry_x', self.odometry_x)
SmartDashboard.putNumber('odometry_y', self.odometry_y)
SmartDashboard.putNumber('odometry_delta_x', delta_x)
SmartDashboard.putNumber('odometry_delta_y', delta_y)
SmartDashboard.putNumber('odometry_x_vel', self.odometry_x_vel)
SmartDashboard.putNumber('odometry_y_vel', self.odometry_y_vel)
SmartDashboard.putNumber('odometry_z_vel', self.odometry_z_vel)
SmartDashboard.putNumber('imu_heading', heading)
SmartDashboard.putNumber('heading_delta', heading_delta)
SmartDashboard.putNumber('average_heading', timestep_average_heading)
NetworkTables.flush()
self.last_heading = heading
def teleopPeriodic(self):
self.neo.set(sd.getNumber("Neo Power", 0.5))
def updateDashboard(self):
# TODO: additional items?
SmartDashboard.putNumber("IMU heading", self.getCurrentHeading())
SmartDashboard.putNumber("IMU calibration", self.imu.getCalibration())
def initializeNumber(label: str, defaultValue: float) -> float:
value: float = SmartDashboard.getNumber(label, defaultValue)
SmartDashboard.putNumber(label, value)
return value
def __init__(self, robot):
self.joy = wpilib.Joystick(0)
# Put Some buttons on the SmartDashboard
SmartDashboard.putData("Elevator Bottom",
SetElevatorSetpoint(robot, 0))
SmartDashboard.putData("Elevator Platform",
SetElevatorSetpoint(robot, 0.2))
SmartDashboard.putData("Elevator Top", SetElevatorSetpoint(robot, 0.3))
SmartDashboard.putData("Wrist Horizontal", SetWristSetpoint(robot, 0))
SmartDashboard.putData("Raise Wrist", SetWristSetpoint(robot, -45))
SmartDashboard.putData("Open Claw", OpenClaw(robot))
SmartDashboard.putData("Close Claw", CloseClaw(robot))
SmartDashboard.putData("Deliver Soda", Autonomous(robot))
# Create some buttons
d_up = JoystickButton(self.joy, 5)
d_right = JoystickButton(self.joy, 6)
d_down = JoystickButton(self.joy, 7)
d_left = JoystickButton(self.joy, 8)
l2 = JoystickButton(self.joy, 9)
r2 = JoystickButton(self.joy, 10)
l1 = JoystickButton(self.joy, 11)
r1 = JoystickButton(self.joy, 12)
# Connect the buttons to commands
d_up.whenPressed(SetElevatorSetpoint(robot, 0.2))
d_down.whenPressed(SetElevatorSetpoint(robot, -0.2))
d_right.whenPressed(CloseClaw(robot))
d_left.whenPressed(OpenClaw(robot))
r1.whenPressed(PrepareToPickup(robot))
r2.whenPressed(Pickup(robot))
l1.whenPressed(Place(robot))
l2.whenPressed(Autonomous(robot))
def __init__(self, robot):
self.joy = wpilib.Joystick(0)
# Put Some buttons on the SmartDashboard
SmartDashboard.putData("Elevator Bottom", SetElevatorSetpoint(robot, 0));
SmartDashboard.putData("Elevator Platform", SetElevatorSetpoint(robot, 0.2));
SmartDashboard.putData("Elevator Top", SetElevatorSetpoint(robot, 0.3));
SmartDashboard.putData("Wrist Horizontal", SetWristSetpoint(robot, 0));
SmartDashboard.putData("Raise Wrist", SetWristSetpoint(robot, -45));
SmartDashboard.putData("Open Claw", OpenClaw(robot));
SmartDashboard.putData("Close Claw", CloseClaw(robot));
SmartDashboard.putData("Deliver Soda", Autonomous(robot));
# Create some buttons
d_up = JoystickButton(self.joy, 5)
d_right = JoystickButton(self.joy, 6)
d_down = JoystickButton(self.joy, 7)
d_left = JoystickButton(self.joy, 8)
l2 = JoystickButton(self.joy, 9)
r2 = JoystickButton(self.joy, 10)
l1 = JoystickButton(self.joy, 11)
r1 = JoystickButton(self.joy, 12)
# Connect the buttons to commands
d_up.whenPressed(SetElevatorSetpoint(robot, 0.2))
d_down.whenPressed(SetElevatorSetpoint(robot, -0.2))
d_right.whenPressed(CloseClaw(robot))
d_left.whenPressed(OpenClaw(robot))
r1.whenPressed(PrepareToPickup(robot))
r2.whenPressed(Pickup(robot))
l1.whenPressed(Place(robot))
l2.whenPressed(Autonomous(robot))
def initSmartDashboard(self):
SmartDashboard.putData("AutoFieldPosition", self.startingFieldPosition)
SmartDashboard.putData("AutoBarrierType", self.barrierType)
SmartDashboard.putData("AutoStrategy", self.strategy)
def _setMotors(self, signal):
signal = signal if abs(
signal) > Constants.TURN_TO_ANGLE_MIN_OUTPUT else math.copysign(
Constants.TURN_TO_ANGLE_MIN_OUTPUT, signal)
Dash.putNumber("Turn To Angle Output", signal)
self.drive.setPercentOutput(signal, -signal, signal, -signal)
def setupDashboardCommands(self):
# Set up auton chooser
self.autonChooser = SendableChooser()
self.autonChooser.setDefaultOption(
"Do Nothing", wpilib.command.WaitCommand(3.0, "Do Nothing"))
self.autonChooser.addOption("Drive Forward", DriveTickTimed(1.0, 1.0))
self.autonChooser.addOption("Drive Backward",
DriveTickTimed(-1.0, -1.0))
self.autonChooser.addOption("Rotate Right", DriveTickTimed(1.0, -1.0))
self.autonChooser.addOption("Rotate Left", DriveTickTimed(-1.0, 1.0))
SmartDashboard.putData("Auto mode", self.autonChooser)
# Set up utility controls
SmartDashboard.putData("Measure", Measure())
# Drive controls
DriveHuman.setupDashboardControls()
SmartDashboard.putData("Brake Control",
DriveHuman.createBrakeModeToggleCommand())
SmartDashboard.putData("Flip Front",
DriveHuman.createFlipFrontCommand())
# Debug tools (if enabled)
if self.debug:
SmartDashboard.putData("CPU Load Test", LoadTest())
SmartDashboard.putData("Drive Subsystem", subsystems.drive)
dd = subsystems.drive.getDifferentialDrive()
if dd != None:
SmartDashboard.putData("DifferentialDrive", dd)
liftBottomSwitch = wpilib.DigitalInput(robotMap.liftBottomSwitch) liftTopSwitch = wpilib.DigitalInput(robotMap.liftTopSwitch) #compressor compressor = wpilib.Compressor(robotMap.pressureSwitch,robotMap.compressorSpike) #encoders shootEncoder = wpilib.Encoder( robotMap.shootEncoder1 , robotMap.shootEncoder2 , True, wpilib.CounterBase.k1X) feedEncoder = wpilib.Encoder(robotMap.feedEncoder1, robotMap.feedEncoder2, True, wpilib.CounterBase.k1X) leftDriveEncoder = wpilib.Encoder( robotMap.leftDriveEncoder1 , robotMap.leftDriveEncoder2 , True, wpilib.CounterBase.k4X) rightDriveEncoder = wpilib.Encoder( robotMap.rightDriveEncoder1 , robotMap.rightDriveEncoder2 , True, wpilib.CounterBase.k4X) encoderHighTest = wpilib.DigitalOutput(9) #initialize smartDashboard SmartDashboard.init() analog = wpilib.AnalogModule(robotMap.analogModule) #variables frontValue = 0 deltaFront = 0 backValue = 0 deltaBack = 0 direction = -1 fnum = 0 bnum = 0 lsense = (lstick.GetThrottle() -1)*(-0.5) rsense = (rstick.GetThrottle() -1)*(-0.5) mode = 0 stage = 0
def execute(self):
Dash.putNumber("Turn To Angle Error", self.PID.getError())
def initialize(self):
self.robot.info("SpinIntakeWheelsCommand %s init" % self.direction)
self.setTimeout(10) # TODO: finialize timing
SmartDashboard.putString("Intakewheels spinning", self.direction)
def teleopPeriodic(self):
sd.putNumber("Neo Encoder", self.neo0.getEncoder().getPosition())
speed = sd.getNumber("Neo Power", 0)
self.neo0.set(speed)
self.neo1.set(speed)
