def autonomousPeriodic(self): time = self.autonTimer.get() if time <= 1.62: self.setDrivePower(.793, .9) SmartDashboard.putNumber("autonTime", time) else: self.setDrivePower(0, 0)
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 execute(self): """ Measure how long it takes to run a bunch of iterations. """ SmartDashboard.putNumber("CPU Test Loops", self.loopCnts) self.timer.reset() self.timer.start() self.runTest(self.loopCnts) self.runSecs = self.timer.get()
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 drive_by_ticks(self, ticks, speed=0.25): offset = ticks - self.get_encoder_ticks() SmartDashboard.putNumber("Offset", offset) if abs(offset) > 50: self.arcade_drive(0, speed) return False self.stop() return True
class mySmartDash: def __init__(self): self.sd = SmartDashboard() def sendNumber(self, key, val): self.sd.putNumber(key, val) def sendRandomData(self, upper, lower): sendNumber(self, "Random", random.randrange(upper, lower))
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 robotInit(self): """ Initalizes all subsystems and user controls. """ if not self.bareBones: # Set up subsystems subsystems.initialize() # Set up user controls oi.initialize() if self.debug: self.performance = Performance() SmartDashboard.putData("Measure Performance", self.performance)
def turn_to_angle(self, angle, speed=0.25): offset = angle - self.get_gyro_angle() SmartDashboard.putDouble("angle_offset", offset) if abs(offset) > 0.1: if offset > 0: turn_speed = speed else: turn_speed = -speed self.arcade_drive(turn_speed, speed) return False return True
def _create_smartdashboard_buttons(self): self._auto_program_chooser = SendableChooser() #self._auto_program_chooser.addDefault("Cross Line", 1) #self._auto_program_chooser.addObject("Hang Gear", 2) self._auto_program_chooser.addObject("Do Nothing", 3) SmartDashboard.putData("Autonomous", self._auto_program_chooser) self._starting_chooser = SendableChooser() self._starting_chooser.addDefault("Left", 1) self._starting_chooser.addObject("Center", 2) self._starting_chooser.addObject("Right", 3) SmartDashboard.putData("Starting_Position", self._starting_chooser)
def isInGyroPosition(self): SD.putNumber('Is in gyro position', ((self.ahrs.getYaw() <= (self.turnController.getSetpoint() + self.robot.autonomous.ANGLE_TOLERANCE)) and (self.ahrs.getYaw() >= (self.turnController.getSetpoint() - self.robot.autonomous.ANGLE_TOLERANCE)))) return ((self.ahrs.getYaw() <= (self.turnController.getSetpoint() + self.robot.autonomous.ANGLE_TOLERANCE)) and (self.ahrs.getYaw() >= (self.turnController.getSetpoint() - self.robot.autonomous.ANGLE_TOLERANCE)))
def __init__(self): # Initialize all controllers self.driveLeftMaster = Talon(kDriveTrain.lmId) self.driveLeftSlave = Talon(kDriveTrain.lsId) self.driveRightMaster = Talon(kDriveTrain.rmId) self.driveRightSlave = Talon(kDriveTrain.rsId) # Connect the slaves to the masters on each side self.driveLeftSlave.follow(self.driveLeftMaster) self.driveRightSlave.follow(self.driveRightMaster) # Makes sure both sides' controllers show green and use positive # values to move the bot forward. # CURRENTLY DISABLED WHEN USING WITH DifferentialDrive # self.driveLeftSlave.setInverted(False) # self.driveLeftMaster.setInverted(False) self.driveRightSlave.setInverted(True) self.driveRightMaster.setInverted(True) # Configures each master to use the attached Mag Encoders self.driveLeftMaster.configSelectedFeedbackSensor( ctre.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0) self.driveRightMaster.configSelectedFeedbackSensor( ctre.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0) # Reverses the encoder direction so forward movement always # results in a positive increase in the encoder ticks. self.driveLeftMaster.setSensorPhase(True) self.driveRightMaster.setSensorPhase(True) # Throw data on the SmartDashboard so we can work with it. SD.putNumber('Left Quad Pos.', self.driveLeftMaster.getQuadraturePosition()) SD.putNumber('Right Quad Pos.', self.driveRightMaster.getQuadraturePosition()) # these may give the derivitive an integral of the PID once # they are set. For now, they just show 0 SD.putNumber('Left Derivative', self.driveLeftMaster.getErrorDerivative(0)) SD.putNumber('Left Integral', self.driveLeftMaster.getIntegralAccumulator(0)) self.leftVel = None self.leftPos = None self.rightVel = None self.rightPos = None # self.driveLeftMaster.config_kP(0, .3, 10) # kP = self.driveLeftMaster.configGetParameter( # self.driveLeftMaster.ParamEnum.eProfileParamSlot_P, 0, 10) # SmartDashboard.putNumber('Left Proportional', kP) self.driveControllerLeft = SpeedControllerGroup(self.driveLeftMaster) self.driveControllerRight = SpeedControllerGroup(self.driveRightMaster) self.driveControllerRight.setInverted(True) self.drive = DifferentialDrive(self.driveControllerLeft, self.driveControllerRight)
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 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 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 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 climb(self, speed): if abs( speed ) > robotMap.WINCHTHRESHOLD and wpilib.DriverStation.getInstance( ).isOperatorControl(): self.winchL.set(speed) self.winchR.set(-speed) else: self.winchL.set(0) self.winchR.set(0) if not self.initInClimb: from common.oi import oi self.oi = oi self.initInClimb = True SmartDashboard.putNumber("Winch Throttle", self.oi.getWinchSpeed())
def approach(self, state_tm): if SmartDashboard.getBoolean("Vision/Found Hook"): self.next_state('found_hook') else: if state_tm < self.forward_time: self.drive.arcade_drive(0, 0.25) else: self.next_state('stop')
def _update_sd(self): SmartDashboard.putNumber("Left Encoder Position", self.left_talon0.getEncPosition()) SmartDashboard.putNumber("Right Encoder Position", self.right_talon0.getEncPosition()) SmartDashboard.putNumber("heading", self.get_gyro_angle()) self.reversed = SmartDashboard.getBoolean("Reversed", False) self.turn_controller.setPID(self.turning_P.value, self.turning_I.value, self.turning_D.value, 0)
def __init__(self): super().__init__("Drive") Command.pageDrive = lambda x=0: self self.frontRight = wpilib.Talon(0) self.rearRight = wpilib.Talon(1) self.frontRight.setInverted(True) self.rearRight.setInverted(True) self.right = wpilib.SpeedControllerGroup(self.frontRight, self.rearRight) self.frontLeft = wpilib.Talon(2) self.rearLeft = wpilib.Talon(3) self.left = wpilib.SpeedControllerGroup(self.frontLeft, self.rearLeft) self.leftEncoder = wpilib.Encoder(8, 9) self.leftEncoder.setDistancePerPulse(1 / 3 * math.pi / 256) # 4 inch wheels? self.leftEncoder.setSamplesToAverage(10) SmartDashboard.putNumber("distance", 0) SmartDashboard.putString("DriveStyle", "Tank")
def initialize(self): if self.initialized: return # initializing auto chooser self.preferredElement = wpilib.SendableChooser() self.preferredElement.addObject('crossLine', 0) self.preferredElement.addObject('switch', 1) self.preferredElement.addObject('scale', 2) self.startingPosition = wpilib.SendableChooser() self.startingPosition.addObject('left', 0) self.startingPosition.addObject('middle', 1) self.startingPosition.addObject('right', 2) SmartDashboard.putData("Preferred Element", self.preferredElement) SmartDashboard.putData("Starting Position", self.startingPosition) print("AutoManager: Initialized") self.initialized = True
def __init__(self, robot): self.robot = robot # update OI with logs Event_Manager.add_listeners({ 'auto.periodic' : self.log, 'teleop.periodic' : self.log }) # notify listeners when SmartDashboard is updated sd = NetworkTable.getTable('SmartDashboard') sd.addTableListener(self.dashboard_listener, True) # add autonomous mode chooser to smartdashboard self.auto_choose = SendableChooser() self.auto_choose.addObject('Auto Do Nothing', Autonomous.Modes.DO_NOTHING) self.auto_choose.addObject('Auto Move Forward', Autonomous.Modes.MOVE_FORWARD) self.auto_choose.addObject('Auto One Object', Autonomous.Modes.ONE_OBJECT) SmartDashboard.putData('Autonomous Mode', self.auto_choose) # joystick events self.joystick = Joystick(0) l_bumper = Joystick_Button(self.joystick, lc.L_BUMPER) l_trigger = Joystick_Button(self.joystick, lc.L_TRIGGER) r_bumper = Joystick_Button(self.joystick, lc.R_BUMPER) r_trigger = Joystick_Button(self.joystick, lc.R_TRIGGER) btn_one = Joystick_Button(self.joystick, 1) btn_two = Joystick_Button(self.joystick, 2) # update the joysick axis periodically Event_Manager.add_listener('teleop.periodic', self.update_axis) l_bumper.while_pressed(lambda: Event_Manager.trigger('joystick.l_bumper.while_pressed')) l_trigger.while_pressed(lambda: Event_Manager.trigger('joystick.l_trigger.while_pressed')) l_bumper.when_pressed(lambda: Event_Manager.trigger('joystick.l_bumper.when_pressed')) l_trigger.when_pressed(lambda: Event_Manager.trigger('joystick.l_trigger.when_pressed')) l_bumper.when_released(lambda: Event_Manager.trigger('joystick.l_bumper.when_released')) l_trigger.when_released(lambda: Event_Manager.trigger('joystick.l_trigger.when_released')) r_bumper.when_pressed(lambda: Event_Manager.trigger('joystick.r_bumper.when_pressed')) btn_two.when_pressed(lambda: Event_Manager.trigger('joystick.btn_two.when_pressed')) btn_one.when_pressed(lambda: Event_Manager.trigger('joystick.btn_one.when_pressed'))
def change_PIDs(self, factor=1, dict_0=None, dict_1=None): '''Pass a value to the and update the PIDs, probably use 1.5 and 0.67 to see how they change can also pass it a dictionary {'kP': 0.06, 'kI': 0.0, 'kD': 0, 'kIz': 0, 'kFF': 0} to set slot 0 (position) or slot 1 (velocity) ''' keys = ['kP', 'kI', 'kD', 'kIz', 'kFF'] for key in keys: if dict_0 == None: self.PID_dict_pos[key] *= factor else: self.PID_dict_pos[key] = dict_0[key] / self.PID_multiplier SmartDashboard.putString("alert", f"Pos: kP: {self.PID_dict_pos['kP']} kI: {self.PID_dict_pos['kI']} kD: {self.PID_dict_pos['kD']} kIz: {self.PID_dict_pos['kIz']} kFF: {self.PID_dict_pos['kFF']}") if dict_1 == None: self.PID_dict_vel[key] *= factor else: self.PID_dict_vel[key] = dict_1[key] / self.PID_multiplier SmartDashboard.putString("alert", f"Vel: kP: {self.PID_dict_vel['kP']} kI: {self.PID_dict_vel['kI']} kD: {self.PID_dict_vel['kD']} kIz: {self.PID_dict_vel['kIz']} kFF: {self.PID_dict_vel['kFF']}") self.configure_controllers(pid_only=True) self.display_PIDs()
def pid_turn_to_angle(self, angle, speed=0.25): offset = angle - self.get_gyro_angle() SmartDashboard.putNumber("angle_offset", offset) if abs(offset) > 2: self.iErr += offset turn = (offset * self.turning_P.value) + (self.turning_I.value * self.iErr) turn = max(min(speed, turn), -speed) if angle < 0: #Negative angle, negative turn turn = abs(turn) else: turn = -abs(turn) SmartDashboard.putNumber('turn_value', turn) self.arcade_drive(turn, 0) return False self.logger.info("Turned to angle " + str(angle)) self.iErr = 0 return True
class MyRobot(wpilib.SampleRobot): def robotInit(self): self.controller = XboxController(0) self.lmotor = wpilib.CANTalon(0) self.rmotor = wpilib.CANTalon(1) self.dashTimer = wpilib.Timer() # Timer for SmartDashboard updating self.dashTimer.start() #self.autonomous_modes = AutonomousModeSelector('autonomous') # Initialize the smart dashboard display elements self.sd = SmartDashboard() self.sd.putNumber("Random", 0) # Send initialization packet def sendRandomData(self, upper, lower, step): self.sd.putNumber("Random", random.randrange(upper, lower, step)) def disabled(self): while self.isDisabled(): wpilib.Timer.delay(0.01) # Wait for 0.01 seconds def autonomous(self): self.autonomous_modes.run() Timer.delay(CONTROL_LOOP_WAIT_TIME) def operatorControl(self): wpilib.Timer.delay(CONTROL_LOOP_WAIT_TIME) while self.isOperatorControl() and self.isEnabled(): self.lmotor.set(self.controller.getLeftY()) self.rmotor.set(self.controller.getRightY()) self.sendRandomData(-10,10,1) def test(self): wpilib.LiveWindow.run()
def initialize(self): """Called just before this Command runs the first time.""" strip_name = lambda x: str(x)[1 + str(x).rfind('.'):-2] print( "\n" + f"** Started {self.name} with input {self.factor} at {round(Timer.getFPGATimestamp() - self.robot.enabled_time, 1)} s **" ) keys = ['kP', 'kI', 'kD', 'kIz', 'kFF'] if self.from_dashboard == 'position': dict_0 = {} for key in keys: value = float(SmartDashboard.getNumber(str(key) + '_0', 0)) dict_0.update({key: value}) self.robot.drivetrain.change_PIDs(factor=1, dict_0=dict_0) if self.from_dashboard == 'velocity': dict_1 = {} for key in keys: value = float(SmartDashboard.getNumber(str(key) + '_1', 0)) dict_1.update({key: value}) self.robot.drivetrain.change_PIDs(factor=1, dict_1=dict_1) else: self.robot.drivetrain.change_PIDs(self.factor)
def LeftStickX(self): creep_mode = SmartDashboard.getBoolean("Creep", False) if creep_mode: return self.conditonAxis( self.getRawAxis(robotmap.XBOX_LEFT_X_AXIS), self.creep_x_deadband, self.creep_x_rate, self.creep_x_expo, self.creep_x_power, self.creep_x_min, self.creep_x_max) else: return self.conditonAxis( self.getRawAxis(robotmap.XBOX_LEFT_X_AXIS), self.left_x_deadband, self.left_x_rate, self.left_x_expo, self.left_x_power, self.left_x_min, self.left_x_max)
def loopFunc(self): """This version of loopFunc passes the event loop to all init() and periodic() functions.""" if self.isDisabled(): if self.last_mode is not self.Mode.kDisabled: LiveWindow.setEnabled(False) self._flush_commands() self.disabledInit() self.last_mode = self.Mode.kDisabled hal.observeUserProgramDisabled() self.disabledPeriodic() elif self.isAutonomous(): if self.last_mode is not self.Mode.kAutonomous: LiveWindow.setEnabled(False) self._flush_commands() self.autonomousInit() self.last_mode = self.Mode.kAutonomous hal.observeUserProgramAutonomous() self.autonomousPeriodic() elif self.isOperatorControl(): if self.last_mode is not self.Mode.kTeleop: LiveWindow.setEnabled(False) self._flush_commands() self.teleopInit() self.last_mode = self.Mode.kTeleop hal.observeUserProgramTeleop() self.teleopPeriodic() else: if self.last_mode is not self.Mode.kTest: LiveWindow.setEnabled(True) self._flush_commands() self.testInit() self.last_mode = self.Mode.kTest hal.observeUserProgramTest() self.testPeriodic() self.robotPeriodic() self._run_commands() SmartDashboard.updateValues() LiveWindow.updateValues()
def robotInit(self): self.controller = XboxController(0) self.lmotor = wpilib.CANTalon(0) self.rmotor = wpilib.CANTalon(1) self.dashTimer = wpilib.Timer() # Timer for SmartDashboard updating self.dashTimer.start() #self.autonomous_modes = AutonomousModeSelector('autonomous') # Initialize the smart dashboard display elements self.sd = SmartDashboard() self.sd.putNumber("Random", 0) # Send initialization packet
def diagnosticsToSmartDash(self): SmartDashboard.putNumber("Elevator Speed", self.elevatorMotor1.getMotorOutputPercent()) SmartDashboard.putNumber( "Elevator Motor 1 Amperage", self.PDP.getCurrent(robotMap.elevatorPDPPort1)) SmartDashboard.putNumber( "Elevator Motor 2 Amperage", self.PDP.getCurrent(robotMap.elevatorPDPPort2))
def __init__(self, default=ledstrip.LEDStrip.Command.RED): ''' Constructor ''' self.command = default self.chooser = wpilib.SendableChooser() self.chooser.addDefault("Red", ledstrip.LEDStrip.Command.RED) self.chooser.addObject("Blue", ledstrip.LEDStrip.Command.BLUE) self.chooser.addObject("Green", ledstrip.LEDStrip.Command.GREEN) self.chooser.addObject("Rainbow", ledstrip.LEDStrip.Command.RAINBOW) self.chooser.addObject("Theater Blue", ledstrip.LEDStrip.Command.THEATER_BLUE) self.chooser.addObject("Theater Red", ledstrip.LEDStrip.Command.THEATER_RED) self.chooser.addObject("Theater Rainbow", ledstrip.LEDStrip.Command.THEATER_RAINBOW) self.chooser.addObject("Off", ledstrip.LEDStrip.Command.OFF) SmartDashboard.putData("LEDs", self.chooser) self.r = 0 self.g = 0 self.b = 0
def initialize(self): """ Read in and apply current drive choices. """ DriveHuman.setupDashboardControls() global modeChooser self.mode = modeChooser.getSelected() self.quickTurn = SmartDashboard.getBoolean("Quick Turn", self.quickTurn) self.squareInputs = SmartDashboard.getBoolean("Square Inputs", self.squareInputs) self.fixedLeft = SmartDashboard.getNumber("Fixed Left", self.fixedLeft) self.fixedRight = SmartDashboard.getNumber("Fixed Right", self.fixedRight) self.rotationGain = SmartDashboard.getNumber("Rotation Gain", self.rotationGain) self.slowGain = SmartDashboard.getNumber("Slow Gain", self.slowGain)
def robotInit(self): """ Initalizes all subsystems and user controls. """ if self.numSubsystems > 0: # Create drive subsystems pwmOfs: int = 0 if self.enableDrive: leftMotors = SpeedControllerGroup(wpilib.VictorSP(0), wpilib.VictorSP(1)) rightMotors = SpeedControllerGroup(wpilib.VictorSP(2), wpilib.VictorSP(3)) gamePad: GenericHID = XboxController(0) drive: Drive = Drive(99, leftMotors, rightMotors) drive.setDefaultCommand(DriveArcade(drive, leftMotors, rightMotors, gamePad)) pwmOfs += 4 for i in range(0, self.numSubsystems): pwm: int = pwmOfs + i * 2 leftMotor: SpeedController = wpilib.VictorSP(pwm) rightMotor: SpeedController = wpilib.VictorSP(pwm + 1) drive = Drive(i, leftMotor, rightMotor) SmartDashboard.putData("Forward " + str(i), DrivePower(drive, 0.2, 0.2)) SmartDashboard.putData("Backward " + str(i), DrivePower(drive, -0.2, -0.2)) # Add command to dashboard to track time for one periodic pass self.performance = Performance() SmartDashboard.putData("Measure Performance", self.performance)
def _update_smartdashboard_tank_drive(self, left, right): SmartDashboard.putNumber("Drivetrain Left Speed", left) SmartDashboard.putNumber("Drivetrain Right Speed", right)
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 pid_periodic(self,move, powerMultiplier=1): #prev 0.023 0.4 0.13 const_kP = 0.028 const_kI = 0.5 const_kFF = 0.17 error = self.pid_calc_error() if abs(error) > 720: self.arcadeDrive(0, 0, False, 12) return ##if error < 5: # const_kI = 1.66 - (error / 6) self.integral_accum = self.integral_accum + (min(1, max(-1, error)) * 0.005) if (self.integral_accum > 0 and error < 0) or (self.integral_accum < 0 and error > 0): self.integral_accum = 0 kP = min(0.4, max(-0.4, const_kP * error)) kI = const_kI * self.integral_accum #kI = kI * kI * 1.0 if kI > 0 else -1.0 kFF = const_kFF * (1.0 if error > 0 else -1.0) SmartDashboard.putNumber("Drive kI", kI) SmartDashboard.putNumber("Drive kP", kP) SmartDashboard.putNumber("Drive kFF", kFF) SmartDashboard.putNumber("Integral Accum", self.integral_accum) SmartDashboard.putNumber("Drive Pid Error", error) SmartDashboard.putNumber("Drive Pid Setpoint", self.setpoint) SmartDashboard.putNumber("Total", -(kP + kI + kFF)) self.arcadeDrive(move, (-(kP + kI)) * powerMultiplier - kFF, False, 12)
def __init__(self): self.sd = SmartDashboard()
def _update_smartdashboard(self, speed): SmartDashboard.putBoolean("Feeder Has Ball", self._has_ball) SmartDashboard.putNumber("Feeder Speed", speed)
def reverse(self): SmartDashboard.putBoolean("Reversed", not SmartDashboard.getBoolean("Reversed", False))
def __init__(self, robot): self.robot = robot # Controllers # Xbox self.xbox_controller_1 = controls.xbox_controller.Xbox_Controller(0) self.xbox_controller_2 = controls.xbox_controller.Xbox_Controller(1) ## COMMANDS ## ##Controller 1 Commands # DRIVE COMMANDS self.drive_command = commands.drivetrain.Drive_With_Tank_Values( self.robot, self._get_axis(self.xbox_controller_1, controls.xbox_controller.XBOX_AXIS_RIGHT_X, inverted=INVERT_XBOX_RIGHT_X), self._get_axis(self.xbox_controller_1, controls.xbox_controller.XBOX_AXIS_LEFT_Y, inverted=INVERT_XBOX_LEFT_Y), self._get_axis(self.xbox_controller_1, controls.xbox_controller.XBOX_AXIS_LEFT_X, inverted=INVERT_XBOX_LEFT_X), ) self.robot.drive.setDefaultCommand(self.drive_command) self.auto_choose = SendableChooser() self.auto_choose.addObject('Do Nothing', commands.autonomous.Do_Nothing(self.robot)) self.auto_choose.addDefault( 'Cross Baseline', commands.autonomous.Cross_Baseline(self.robot)) self.auto_choose.addObject( 'Center Gear', commands.autonomous.Move_To_Gear(self.robot, const.ID_AUTO_CENTER_GEAR)) self.auto_choose.addDefault( 'Center Gear No Camera', commands.autonomous.Center_Gear_Without_Camera(self.robot)) self.auto_choose.addObject( 'Right Gear', commands.autonomous.Move_To_Gear(self.robot, const.ID_AUTO_RIGHT_GEAR)) self.auto_choose.addObject( 'Right Gear And Shoot Red', commands.autonomous.Move_To_Gear_And_Shoot(self.robot, const.ID_AUTO_RED_SIDE)) self.auto_choose.addObject( 'Left Gear', commands.autonomous.Move_To_Gear(self.robot, const.ID_AUTO_LEFT_GEAR)) self.auto_choose.addObject( 'Left Gear And Shoot Blue', commands.autonomous.Move_To_Gear_And_Shoot( self.robot, const.ID_AUTO_BLUE_SIDE)) self.auto_choose.addObject( 'Shoot From Hopper Red', commands.autonomous.Shoot_From_Hopper(self.robot, const.ID_AUTO_RED_SIDE)) self.auto_choose.addObject( 'Shoot From Hopper Blue', commands.autonomous.Shoot_From_Hopper(self.robot, const.ID_AUTO_BLUE_SIDE)) self.auto_choose.addObject( 'Shoot From Start', commands.autonomous.Shoot_From_Start(self.robot)) SmartDashboard.putData('Autonomous Mode', self.auto_choose) # Toggle Drive State self.button_drive_shift = Xbox_Trigger(self.xbox_controller_1, XBOX_BTN_LEFT_TRIGGER) self.button_drive_shift.whenPressed( commands.drivetrain.Toggle_Drive_State(self.robot)) # Toggle drivetrain gear if not const.DEMO_MODE: self.button_gear_shift = Xbox_Button(self.xbox_controller_1, XBOX_BTN_LEFT_BUMPER) self.button_gear_shift.whenPressed( commands.drivetrain.Toggle_Gear_State(self.robot)) # Slow speed for driving in Y direction so that we slow down before the gear peg self.button_slow_driving = Xbox_Trigger(self.xbox_controller_1, XBOX_BTN_RIGHT_TRIGGER) self.button_slow_driving.whenPressed( commands.drivetrain.Set_Sensitivity_Low(self.robot)) self.button_slow_driving.whenReleased( commands.drivetrain.Set_Sensitivity_High(self.robot)) # Drives backwards small distance, to align better for gear feeding self.button_drive_gear_feeder_reverse = Xbox_Button( self.xbox_controller_1, XBOX_BTN_X) self.button_drive_gear_feeder_reverse.whenPressed( commands.drivetrain.Back_Up(self.robot)) if not const.DEMO_MODE: # For testing self.button_rotate_to_boiler = Xbox_Button(self.xbox_controller_1, XBOX_BTN_Y) self.button_rotate_to_boiler.whenPressed( commands.drivetrain.Rotate_To_Boiler(self.robot)) # For testing self.rotate = Xbox_Button(self.xbox_controller_1, XBOX_BTN_RIGHT_BUMPER) self.rotate.whenPressed( commands.drivetrain.Rotate_To_Angle(self.robot, -52)) self.rotate_to_gear = Xbox_Button(self.xbox_controller_1, XBOX_BTN_A) self.rotate_to_gear.whenPressed( commands.drivetrain.Rotate_To_Gear(self.robot)) ## Controller 2 Commands # Run Shooter if const.DEMO_MODE: # Lower top speed on shooter when in demo mode self.button_run_shooter = Xbox_Trigger(self.xbox_controller_2, XBOX_BTN_RIGHT_TRIGGER) self.button_run_shooter.whenPressed( commands.shooter.Run_Shooter_Wheel(self.robot)) self.button_run_shooter.whenReleased( commands.shooter.Stop_Shooter_Wheel(self.robot)) else: self.button_run_shooter = Xbox_Trigger(self.xbox_controller_2, XBOX_BTN_RIGHT_TRIGGER) self.button_run_shooter.whenPressed( commands.shooter.Run_Shooter_Wheel_Full_Speed(self.robot)) self.button_run_shooter.whenReleased( commands.shooter.Stop_Shooter_Wheel(self.robot)) # Run Agitator self.button_run_agitator = Xbox_Trigger(self.xbox_controller_2, XBOX_BTN_LEFT_TRIGGER) self.button_run_agitator.whenPressed( commands.agitator.Run_Agitator(self.robot, 0.8)) self.button_run_agitator.whenReleased( commands.agitator.Stop_Agitator(self.robot)) # FEEDER COMMANDS # Toggles feeder on/off when pressed self.button_run_feeder_forward = Xbox_Button(self.xbox_controller_2, XBOX_BTN_RIGHT_BUMPER) self.button_run_feeder_forward.whenPressed( commands.feeder.Run_Feeder(self.robot, 1.0)) self.button_run_feeder_forward.whenReleased( commands.feeder.Stop_Feeder(self.robot)) self.button_run_feeder_backward = Xbox_Button(self.xbox_controller_2, XBOX_BTN_LEFT_BUMPER) self.button_run_feeder_backward.whenPressed( commands.feeder.Run_Feeder(self.robot, -1.0)) self.button_run_feeder_backward.whenReleased( commands.feeder.Stop_Feeder(self.robot)) if not const.DEMO_MODE: # GILLOTUINNEE self.button_toggle_gear_lexan = Xbox_Button( self.xbox_controller_2, XBOX_BTN_Y) self.button_toggle_gear_lexan.whenPressed( commands.gear_lexan.Toggle_Gear_Lexan_State(self.robot)) #self.button_toggle_gear_lexan.whenPressed( commands.gear_lexan.Set_Gear_Lexan_State( self.robot, const.ID_GEAR_LEXAN_OPEN ) ) #self.button_toggle_gear_lexan.whenReleased( commands.gear_lexan.Set_Gear_Lexan_State( self.robot, const.ID_GEAR_LEXAN_CLOSED) ) # Release gear self.button_toggle_gear_release = Xbox_Button(self.xbox_controller_2, XBOX_BTN_A) self.button_toggle_gear_release.whenPressed( commands.gear_claw.Open_Container(self.robot)) self.button_toggle_gear_release.whenReleased( commands.gear_claw.Close_Container(self.robot)) # Toggle funnel self.button_toggle_gear_funnel = Xbox_Button(self.xbox_controller_2, XBOX_BTN_B) self.button_toggle_gear_funnel.whenPressed( commands.gear_funnel.Toggle_Gear_Funnel_State(self.robot)) self.button_toggle_gear_funnel.whenPressed( commands.gear_lexan.Set_Gear_Lexan_State( self.robot, const.ID_GEAR_LEXAN_CLOSED)) self.button_toggle_gear_funnel.whenReleased( commands.gear_funnel.Toggle_Gear_Funnel_State(self.robot)) # CLIMBER COMMANDS self.button_run_climber_up = Joystick_POV( self.xbox_controller_2, controls.joystick_pov.JOY_POV_UP) self.button_run_climber_up.whenPressed( commands.climber.Run_Climber(self.robot, 1.0)) self.button_run_climber_up.whenReleased( commands.climber.Stop_Climber(self.robot)) #self.button_run_climber_down = Joystick_POV( self.xbox_controller_1, controls.joystick_pov.JOY_POV_DOWN ) #self.button_run_climber_down.whenPressed( commands.climber.Run_Climber( self.robot, -0.2 ) ) #self.button_run_climber_down.whenReleased( commands.climber.Stop_Climber( self.robot ) ) #self.button_change_gear_camera_type = Xbox_Button( self.xbox_controller_2, XBOX_BTN_BACK ) #self.button_change_gear_camera_type.whenPressed( commands.gear_camera.Toggle_Gear_Camera_Mode( self.robot ) ) #self.button_change_shooter_camera_type = Xbox_Button( self.xbox_controller_2, XBOX_BTN_START ) #self.button_change_shooter_camera_type.whenPressed( commands.shooter_camera.Toggle_Shooter_Camera_Mode( self.robot ) ) #smart dashboard changing PID values dp_trigger = SmartDashboard_Update_Trigger('Drive P: ', self.robot.drive.default_kP) dp_trigger.whenActive( Command_Call(lambda: self.robot.drive.update_pid(p=dp_trigger. get_key_value()))) di_trigger = SmartDashboard_Update_Trigger('Drive I: ', self.robot.drive.default_kI) di_trigger.whenActive( Command_Call(lambda: self.robot.drive.update_pid(i=di_trigger. get_key_value()))) dd_trigger = SmartDashboard_Update_Trigger('Drive D: ', self.robot.drive.default_kD) dd_trigger.whenActive( Command_Call(lambda: self.robot.drive.update_pid(d=dd_trigger. get_key_value()))) #smart dashboard changing Shooter PID values sp_trigger = SmartDashboard_Update_Trigger( 'Shooter P: ', self.robot.shooter.default_kP) sp_trigger.whenActive( Command_Call(lambda: self.robot.shooter.update_pid( p=sp_trigger.get_key_value()))) si_trigger = SmartDashboard_Update_Trigger( 'Shooter I: ', self.robot.shooter.default_kI) si_trigger.whenActive( Command_Call(lambda: self.robot.shooter.update_pid( i=si_trigger.get_key_value()))) sd_trigger = SmartDashboard_Update_Trigger( 'Shooter D: ', self.robot.shooter.default_kD) sd_trigger.whenActive( Command_Call(lambda: self.robot.shooter.update_pid( d=sd_trigger.get_key_value()))) sf_trigger = SmartDashboard_Update_Trigger( 'Shooter F: ', self.robot.shooter.default_kF) sf_trigger.whenActive( Command_Call(lambda: self.robot.shooter.update_pid( f=sf_trigger.get_key_value()))) enc_trigger = SmartDashboard_Update_Trigger( 'ENC Autocorrect constant ', const.DRIVE_CORRECTION_PROPORTION_FORWARD_ENC) enc_trigger.whenActive( Command_Call( lambda: const.update_enc_const(enc_trigger.get_key_value()))) drive_corr_trigger = SmartDashboard_Update_Trigger( 'Drive Correction Enbaled ', const.DRIVE_CORRECTION_ENABLED) drive_corr_trigger.whenActive( Command_Call(lambda: const.update_auto_correct(drive_corr_trigger. get_key_value()))) shooter_trigger = SmartDashboard_Update_Trigger( 'Shooter Speed ', const.SHOOTER_SPEED) shooter_trigger.whenActive( Command_Call(lambda: const.update_shooter_speed(shooter_trigger. get_key_value()))) # Trigger for changing cameras exposure cam_exp_trigger = SmartDashboard_Update_Trigger( 'Cam Exposure ', const.DEFAULT_CAMERA_EXPOSURE) cam_exp_trigger.whenActive( Command_Call(lambda: self._update_cameras_exposure( cam_exp_trigger.get_key_value())))
def _update_smartdashboard(self, speed): SmartDashboard.putNumber("Arm Encoder", self._encoder_value) SmartDashboard.putNumber("Arm Speed", speed)
def _update_smartdashboard_sensors(self): SmartDashboard.putNumber("Drivetrain Encoder", self._encoder_count) SmartDashboard.putNumber("Gyro Angle", self._gyro_angle) SmartDashboard.putNumber("Pitch Angle", self._pitch_gyro_angle)
def _update_smartdashboard_arcade_drive(self, linear, turn): SmartDashboard.putNumber("Drivetrain Linear Speed", linear) SmartDashboard.putNumber("Drivetrain Turn Speed", turn)