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)