def setup(self):
self.left = 0
self.right = 0
self.sd = NetworkTable.getTable("/SmartDashboard")
#Turn to angle PI values
self.turning_P = self.sd.getAutoUpdateValue("TurnToAngle/P", 1)
self.turning_I = self.sd.getAutoUpdateValue("TurnToAngle/I", 0)
self.turning_D = self.sd.getAutoUpdateValue("TurnToAngle/D", 0)
self.turn_controller = wpilib.PIDController(Kp=self.turning_P.value, Ki=self.turning_I.value, Kd=self.turning_D.value, source=self, output=self)
self.turn_controller.setInputRange(-180, 180)
self.turn_controller.setOutputRange(-1, 1)
self.turn_controller.setContinuous(True)
self.pid_value = 0
self.drive_constant = self.sd.getAutoUpdateValue("Drive/Drive Constant", 0.0001)
self.drive_multiplier = self.sd.getAutoUpdateValue("Drive/Drive Multiplier", 0.75)
SmartDashboard.putBoolean("Reversed", False)
self.reversed = False
self.logger = logging.getLogger("drive")
self.iErr = 0
def turn_right(self, state_tm, initial_call):
if initial_call:
self.drive.reset_encoders()
if SmartDashboard.getDouble("angle_offset") < (self.turn_right_angle /
2):
SmartDashboard.putBoolean("run_vision", True)
if self.drive.turn_to_angle(self.turn_right_angle):
self.next_state('approach')
def turn(self):
station = SmartDashboard.getString("auto_position/selected",
"Position 1")
SmartDashboard.putBoolean("run_vision", False)
if station is "Position 1":
self.next_state('turn_right')
elif station is "Position 3":
self.next_state('turn_left')
else:
self.next_state('approach')
def teleopPeriodic(self):
self.update_sd()
self.drive.tankdrive(self.left_joystick.getRawAxis(1),
self.right_joystick.getRawAxis(1))
if self.climber_joystick.getRawButton(3):
self.climber.enable()
self.climber.set(self.climber_joystick.getRawAxis(1))
if self.buttons.getButton(11) and self.last_button_state is False:
self.last_button_state = True
SmartDashboard.putBoolean("Reversed", not self.drive.reversed)
if not self.buttons.getButton(11) and self.last_button_state is True:
self.last_button_state = False
def log(self):
self.counter += 1
if self.counter % 10 == 0:
if self.connected:
SmartDashboard.putBoolean("IsConnected",
self.navx.isConnected())
SmartDashboard.putNumber("Angle", self.navx.getAngle())
SmartDashboard.putNumber("Pitch", self.navx.getPitch())
SmartDashboard.putNumber("Yaw", self.navx.getYaw())
SmartDashboard.putNumber("Roll", self.navx.getRoll())
SmartDashboard.putNumber("Analog",
round(self.analog.getVoltage(), 3))
SmartDashboard.putNumber("Timestamp",
self.navx.getLastSensorTimestamp())
def initialize(self):
self.register_sd_var('initial_distance', 60, vmin=0, vmax=150)
self.register_sd_var("middle_peg_distance", 24, vmin=1, vmax=100)
self.register_sd_var('turning_time', 1.5, vmin=1, vmax=5)
self.register_sd_var('turning_speed', 0.25)
self.register_sd_var('turn_to_peg_P', 0.075)
self.register_sd_var("wait_to_look", 0.5, vmin=0, vmax=3)
self.register_sd_var("turn_left_angle", 300, vmin=0, vmax=360)
self.register_sd_var("turn_right_angle", 60, vmin=0, vmax=360)
self.register_sd_var("forward_time", 3, vmin=1, vmax=10)
self.turnt = False
self.found = False
SmartDashboard.putBoolean("run_vision", False)
self.logger = logging.Logger("Vision Auto")
def forwards(self, initial_call, state_tm):
self.found = False
SmartDashboard.putBoolean("Found", False)
if initial_call:
self.drive.reset_encoders()
self.logger.info(
SmartDashboard.getString("auto_position/selected"))
if SmartDashboard.getString("auto_position/selected",
"Position 1") is "Position 2":
distance = self.middle_peg_distance
else:
distance = self.initial_distance
if self.drive.drive_distance(distance, speed=0.25):
self.drive.stop()
self.next_state('turn')
def setupDashboardControls():
# Only need to set up dashbard drive controls once
global modeChooser
if modeChooser == None:
SmartDashboard.putBoolean("Quick Turn", False)
SmartDashboard.putBoolean("Square Inputs", True)
SmartDashboard.putNumber("Fixed Left", 0.4)
SmartDashboard.putNumber("Fixed Right", 0.4)
SmartDashboard.putNumber("Rotation Gain", 0.5)
SmartDashboard.putNumber("Slow Gain", 0.5)
mc = SendableChooser()
mc.addDefault("Arcade", kModeArcade)
mc.addOption("Tank", kModeTank)
mc.addOption("Curvature", kModeCurvature)
mc.addDefault("Indexed Arcade", kModeIndexedArcade)
mc.addDefault("Indexed Tank", kModeIndexedTank)
mc.addOption("Fixed", kModeFixed)
SmartDashboard.putData("Drive Mode", mc)
modeChooser = mc
def execute(self):
yawRaw = self.drive.getAngle()
yaw = yawRaw - self.yawLast
leftDist = self.left.getDistance() - self.leftDistLast
leftCnts = self.left.getCounts() - self.leftCntsLast
leftVel = self.left.getVelocity()
rightDist = self.right.getDistance() - self.rightDistLast
rightCnts = self.right.getCounts() - self.rightCntsLast
rightVel = self.right.getVelocity()
SmartDashboard.putNumber("Yaw NavX", yawRaw)
SmartDashboard.putNumber("Yaw Measured", yaw)
SmartDashboard.putNumber("Left Dist", leftDist)
SmartDashboard.putNumber("Left Cnts", leftCnts)
SmartDashboard.putNumber("Left Vel", leftVel)
SmartDashboard.putNumber("Right Dist", rightDist)
SmartDashboard.putNumber("Right Cnts", rightCnts)
SmartDashboard.putNumber("Right Vel", rightVel)
SmartDashboard.putNumber("Accel X", subsystems.drive.getAccelX())
SmartDashboard.putNumber("Accel Y", subsystems.drive.getAccelY())
SmartDashboard.putBoolean("Bump", subsystems.drive.bumpCheck())
def log(self):
self.counter += 1
if self.counter % 10 == 0:
# start keeping track of where the robot is with an x and y position
try:
distance = 0.5 * (self.sparkneo_encoder_1.getPosition() - self.sparkneo_encoder_3.getPosition())
except:
print(f"Problem: encoder position returns {self.sparkneo_encoder_1.getPosition()}")
distance = 0
self.x = self.x + (distance - self.previous_distance) * math.sin(
math.radians(self.robot.navigation.get_angle()))
self.y = self.y + (distance - self.previous_distance) * math.cos(
math.radians(self.robot.navigation.get_angle()))
self.previous_distance = distance
# send values to the dash to make sure encoders are working well
SmartDashboard.putNumber("Robot X", round(self.x, 2))
SmartDashboard.putNumber("Robot Y", round(self.y, 2))
SmartDashboard.putNumber("Position Enc1", round(self.sparkneo_encoder_1.getPosition(), 2))
SmartDashboard.putNumber("Position Enc2", round(self.sparkneo_encoder_2.getPosition(), 2))
SmartDashboard.putNumber("Position Enc3", round(self.sparkneo_encoder_3.getPosition(), 2))
SmartDashboard.putNumber("Position Enc4", round(self.sparkneo_encoder_4.getPosition(), 2))
SmartDashboard.putNumber("Velocity Enc1", round(self.sparkneo_encoder_1.getVelocity(), 2))
SmartDashboard.putNumber("Velocity Enc2", round(self.sparkneo_encoder_2.getVelocity(), 2))
SmartDashboard.putNumber("Velocity Enc3", round(self.sparkneo_encoder_3.getVelocity(), 2))
SmartDashboard.putNumber("Velocity Enc4", round(self.sparkneo_encoder_4.getVelocity(), 2))
#SmartDashboard.putNumber("Power M1", round(self.spark_neo_left_front.getAppliedOutput(), 2))
#SmartDashboard.putNumber("Power M3", round(self.spark_neo_right_front.getAppliedOutput(), 2))
SmartDashboard.putNumber("Current M1", round(self.spark_neo_left_front.getOutputCurrent(), 2))
SmartDashboard.putNumber("Current M3", round(self.spark_neo_right_front.getOutputCurrent(), 2))
SmartDashboard.putBoolean('AccLimit', self.is_limited)
if self.counter % 1000 == 0:
self.display_PIDs()
SmartDashboard.putString("alert",
f"Position: ({round(self.x, 1)},{round(self.y, 1)}) Time: {round(Timer.getFPGATimestamp() - self.robot.enabled_time, 1)}")
SmartDashboard.putString("Controller1 Idle", str(self.spark_neo_left_front.getIdleMode()))
SmartDashboard.putNumber("Enc1 Conversion", self.sparkneo_encoder_1.getPositionConversionFactor())
def reverse(self):
SmartDashboard.putBoolean("Reversed", not SmartDashboard.getBoolean("Reversed", False))
class MyRobot(wpilib.SampleRobot):
def robotInit(self):
self.controller = XboxController(0)
self.controller2 = XboxController(1)
#self.lmotor = wpilib.CANTalon(1)
#self.rmotor = wpilib.CANTalon(0)
self.drive = driveTrain(self)
self.robotArm = arm(self)
self.climber = lift(self)
self.pixy = Pixy()
self.drive.reset()
self.dashTimer = wpilib.Timer() # Timer for SmartDashboard updating
self.dashTimer.start()
# Initialize Components functions
self.components = {
'drive' : self.drive,
'arm' : self.robotArm,
'lift' : self.climber,
'pixy' : self.pixy
}
# Initialize Smart Dashboard
self.dash = SmartDashboard()
self.autonomous_modes = AutonomousModeSelector('autonomous', self.components)
self.potentiometer = ('Arm Potentiometer', 0)
self.dash.putNumber('ControlType', 0)
self.dash.putBoolean('Front Switch', 0)
self.dash.putBoolean('Back Switch', 0)
self.drive.log()
def disabled(self):
self.drive.reset()
#self.drive.disablePIDs()
while self.isDisabled():
wpilib.Timer.delay(0.01) # Wait for 0.01 seconds
def autonomous(self):
self.drive.reset()
self.drive.enablePIDs()
while self.isAutonomous() and self.isEnabled():
# Run the actual autonomous mode
self.potentiometer = ('Arm Potentiometer', self.robotArm.getPOT())
self.drive.log()
self.autonomous_modes.run()
def operatorControl(self):
# Resetting encoders
self.drive.reset()
#self.drive.enablePIDs()
while self.isOperatorControl() and self.isEnabled():
self.drive.xboxTankDrive(self.controller.getLeftY(), self.controller.getRightY(), self.controller.getLeftBumper(), self.controller.getRightBumper(), self.controller.getLeftTrigger(), self.controller.getRightTrigger())
self.robotArm.armUpDown(self.controller2.getLeftTriggerRaw(), self.controller2.getRightTriggerRaw(), self.controller2.getButtonA(), rate=0.5)
self.robotArm.wheelSpin(self.controller2.getLeftY())
self.climber.climbUpDown(self.controller2.getLeftBumper(), self.controller2.getRightBumper())
self.drive.log()
wpilib.Timer.delay(CONTROL_LOOP_WAIT_TIME)
# Send encoder data to the smart dashboard
self.dash.putNumber('Arm Potentiometer', self.robotArm.getPOT())
#self.dash.putBoolean('Back Arm Switch', self.robotArm.getFrontSwitch())
#self.dash.putBoolean('Front Arm Switch', self.robotArm.getBackSwitch())
def test(self):
wpilib.LiveWindow.run()
self.drive.reset()
self.drive.enablePIDs()
while self.isTest() and self.isEnabled():
self.drive.xboxTankDrive(self.controller.getLeftY(), self.controller.getRightY(), self.controller.getLeftBumper(), self.controller.getRightBumper(), self.controller.getRightTrigger())
self.robotArm.armUpDown(self.controller2.getLeftTriggerRaw(), self.controller2.getRightTriggerRaw())
'''
def updateDashboard(self):
global enableBrakeMode
SmartDashboard.putBoolean("Brake Mode", enableBrakeMode)
def _update_smartdashboard(self, speed):
SmartDashboard.putBoolean("Feeder Has Ball", self._has_ball)
SmartDashboard.putNumber("Feeder Speed", speed)
def updateDashboard(self):
global isFlipped
SmartDashboard.putBoolean("Flipped Front", isFlipped)
def disabledInit(self):
SmartDashboard.putBoolean("time_running", False)
SmartDashboard.putBoolean("run_vision", False)
self.drive.reset_encoders()
magicbot.MagicRobot.disabledInit(self)
def update_sd(self):
SmartDashboard.putBoolean("time_running", True)
SmartDashboard.putNumber(
"time_remaining",
DriverStation.getInstance().getMatchTime() - 15)
def createObjects(self):
#navx
self.navx = AHRS.create_spi()
self.navx.setPIDSourceType(PIDSource.PIDSourceType.kDisplacement)
#Drivetrain
self.left_talon0 = ctre.CANTalon(0)
self.left_talon1 = ctre.CANTalon(1)
self.right_talon0 = ctre.CANTalon(2)
self.right_talon1 = ctre.CANTalon(3)
#Set up talon slaves
self.left_talon1.setControlMode(ctre.CANTalon.ControlMode.Follower)
self.left_talon1.set(self.left_talon0.getDeviceID())
self.right_talon1.setControlMode(ctre.CANTalon.ControlMode.Follower)
self.right_talon1.set(self.right_talon0.getDeviceID())
#Set talon feedback device
self.left_talon0.setFeedbackDevice(
ctre.CANTalon.FeedbackDevice.QuadEncoder)
self.right_talon0.setFeedbackDevice(
ctre.CANTalon.FeedbackDevice.QuadEncoder)
#Set the Ticks per revolution in the talons
self.left_talon0.configEncoderCodesPerRev(1440)
self.right_talon0.configEncoderCodesPerRev(1440)
#Reverse left talon
self.left_talon0.setInverted(True)
self.right_talon0.setInverted(False)
#Climber
self.climber_motor = wpilib.Spark(0)
self.climber_2 = wpilib.Spark(1)
#Sensors
self.left_enc = encoder.Encoder(self.left_talon0)
self.right_enc = encoder.Encoder(self.right_talon0, True)
#Controls
self.left_joystick = wpilib.Joystick(0)
self.right_joystick = wpilib.Joystick(1)
self.climber_joystick = wpilib.Joystick(2)
self.buttons = unifiedjoystick.UnifiedJoystick(
[self.left_joystick, self.right_joystick])
self.last_button_state = False
#Bling
self.leds = ledstrip.LEDStrip()
#Autonomous Placement
self.auto_positions = wpilib.SendableChooser()
self.auto_positions.addDefault("Position 1", 1)
self.auto_positions.addObject("Position 2", 2)
self.auto_positions.addObject("Position 3", 3)
SmartDashboard.putData("auto_position", self.auto_positions)
#SD variables
SmartDashboard.putNumber("Vision/Turn", 0)
SmartDashboard.putBoolean("Reversed", True)
#LiveWindow
LiveWindow.addActuator("Drive", "Left Master Talon", self.left_talon0)
LiveWindow.addActuator("Drive", "Right Master Talon",
self.right_talon0)
def teleopInit(self):
SmartDashboard.putBoolean("time_running", True)
SmartDashboard.putNumber("time_remaining", 215)
