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)