class MyRobot(wpilib.TimedRobot): """Main robot class""" # NetworkTables API for controlling this #: Speed to set the motors to autospeed = ntproperty("/robot/autospeed", defaultValue=0, writeDefault=True) #: Test data that the robot sends back telemetry = ntproperty("/robot/telemetry", defaultValue=(0, ) * 9, writeDefault=False) prior_autospeed = 0 WHEEL_DIAMETER = 0.5 ENCODER_PULSE_PER_REV = 4096 PIDIDX = 0 def robotInit(self): """Robot-wide initialization code should go here""" self.lstick = wpilib.Joystick(0) # Left front left_front_motor = ctre.WPI_TalonSRX(1) left_front_motor.setInverted(False) left_front_motor.setSensorPhase(False) self.left_front_motor = left_front_motor # Right front right_front_motor = ctre.WPI_TalonSRX(2) right_front_motor.setInverted(False) right_front_motor.setSensorPhase(False) self.right_front_motor = right_front_motor # Left rear -- follows front left_rear_motor = ctre.WPI_TalonSRX(3) left_rear_motor.setInverted(False) left_rear_motor.setSensorPhase(False) left_rear_motor.follow(left_front_motor) # Right rear -- follows front right_rear_motor = ctre.WPI_TalonSRX(4) right_rear_motor.setInverted(False) right_rear_motor.setSensorPhase(False) right_rear_motor.follow(right_front_motor) # # Configure drivetrain movement # self.drive = DifferentialDrive(left_front_motor, right_front_motor) self.drive.setDeadband(0) # # Configure encoder related functions -- getpos and getrate should return # ft and ft/s # encoder_constant = ((1 / self.ENCODER_PULSE_PER_REV) * self.WHEEL_DIAMETER * math.pi) left_front_motor.configSelectedFeedbackSensor( left_front_motor.FeedbackDevice.QuadEncoder, self.PIDIDX, 10) self.l_encoder_getpos = ( lambda: left_front_motor.getSelectedSensorPosition( self.PIDIDX) * encoder_constant) self.l_encoder_getrate = ( lambda: left_front_motor.getSelectedSensorVelocity( self.PIDIDX) * encoder_constant * 10) right_front_motor.configSelectedFeedbackSensor( right_front_motor.FeedbackDevice.QuadEncoder, self.PIDIDX, 10) self.r_encoder_getpos = ( lambda: left_front_motor.getSelectedSensorPosition( self.PIDIDX) * encoder_constant) self.r_encoder_getrate = ( lambda: left_front_motor.getSelectedSensorVelocity( self.PIDIDX) * encoder_constant * 10) # # Configure gyro # # You only need to uncomment the below lines if you want to characterize wheelbase radius # Otherwise you can leave this area as-is self.gyro_getangle = lambda: 0 # Uncomment for the KOP gyro # Note that the angle from all implementors of the Gyro class must be negated because # getAngle returns a clockwise angle, and we require a counter-clockwise one # gyro = ADXRS450_Gyro() # self.gyro_getangle = lambda: -1 * gyro.getAngle() # Set the update rate instead of using flush because of a NetworkTables bug # that affects ntcore and pynetworktables # -> probably don't want to do this on a robot in competition NetworkTables.setUpdateRate(0.010) def disabledInit(self): self.logger.info("Robot disabled") self.drive.tankDrive(0, 0) def disabledPeriodic(self): pass def robotPeriodic(self): # feedback for users, but not used by the control program sd = wpilib.SmartDashboard sd.putNumber("l_encoder_pos", self.l_encoder_getpos()) sd.putNumber("l_encoder_rate", self.l_encoder_getrate()) sd.putNumber("r_encoder_pos", self.r_encoder_getpos()) sd.putNumber("r_encoder_rate", self.r_encoder_getrate()) sd.putNumber("gyro_angle", self.gyro_getangle()) def teleopInit(self): self.logger.info("Robot in operator control mode") def teleopPeriodic(self): self.drive.arcadeDrive(-self.lstick.getY(), self.lstick.getX()) def autonomousInit(self): self.logger.info("Robot in autonomous mode") def autonomousPeriodic(self): """ If you wish to just use your own robot program to use with the data logging program, you only need to copy/paste the logic below into your code and ensure it gets called periodically in autonomous mode Additionally, you need to set NetworkTables update rate to 10ms using the setUpdateRate call. Note that reading/writing self.autospeed and self.telemetry are NetworkTables operations (using pynetworktables's ntproperty), so if you don't read/write NetworkTables in your implementation it won't actually work. """ # Retrieve values to send back before telling the motors to do something now = wpilib.Timer.getFPGATimestamp() l_encoder_position = self.l_encoder_getpos() l_encoder_rate = self.l_encoder_getrate() r_encoder_position = self.r_encoder_getpos() r_encoder_rate = self.r_encoder_getrate() battery = self.ds.getBatteryVoltage() l_motor_volts = self.left_front_motor.getMotorOutputVoltage() r_motor_volts = self.right_front_motor.getMotorOutputVoltage() gyro_angle = self.gyro_getangle() # Retrieve the commanded speed from NetworkTables autospeed = self.autospeed self.prior_autospeed = autospeed # command motors to do things self.drive.tankDrive(autospeed, autospeed, False) # send telemetry data array back to NT self.telemetry = ( now, battery, autospeed, l_motor_volts, r_motor_volts, l_encoder_position, r_encoder_position, l_encoder_rate, r_encoder_rate, gyro_angle, )
class Drivetrain: navx = navx.AHRS left_motor_master = WPI_TalonFX left_motor_slave = WPI_TalonFX left_motor_slave2 = WPI_TalonFX right_motor_master = WPI_TalonFX right_motor_slave = WPI_TalonFX right_motor_slave2 = WPI_TalonFX shifter_solenoid = Solenoid arcade_cutoff = tunable(0.1) angle_correction_cutoff = tunable(0.05) angle_correction_factor_low_gear = tunable(0.1) angle_correction_factor_high_gear = tunable(0.1) angle_correction_max = tunable(0.2) little_rotation_cutoff = tunable(0.1) def __init__(self): self.pending_differential_drive = None self.force_differential_drive = False self.pending_gear = LOW_GEAR self.pending_position = None self.pending_reset = False self.og_yaw = None self.pending_manual_drive = None self.is_manual_mode = False # Set encoders self.left_motor_master.configSelectedFeedbackSensor( ctre.TalonFXFeedbackDevice.IntegratedSensor, 0, 0) self.right_motor_master.configSelectedFeedbackSensor( ctre.TalonFXFeedbackDevice.IntegratedSensor, 0, 0) self.left_motor_master.setSensorPhase(True) # Set slave motors self.left_motor_slave.set(ctre.TalonFXControlMode.Follower, self.left_motor_master.getDeviceID()) self.left_motor_slave2.set(ctre.TalonFXControlMode.Follower, self.left_motor_master.getDeviceID()) self.right_motor_slave.set(ctre.TalonFXControlMode.Follower, self.right_motor_master.getDeviceID()) self.right_motor_slave2.set(ctre.TalonFXControlMode.Follower, self.right_motor_master.getDeviceID()) # Set up drive control self.robot_drive = DifferentialDrive(self.left_motor_master, self.right_motor_master) self.robot_drive.setDeadband(0) self.robot_drive.setSafetyEnabled(False) def is_left_encoder_connected(self): return self.left_motor_master.getPulseWidthRiseToRiseUs() != 0 def is_right_encoder_connected(self): return self.right_motor_master.getPulseWidthRiseToRiseUs() != 0 def reset_position(self): self.left_motor_master.setQuadraturePosition(0, TALON_TIMEOUT) self.right_motor_master.setQuadraturePosition(0, TALON_TIMEOUT) def get_position(self): ''' Returns averaged quadrature position in inches. ''' left_position = self.get_left_encoder() right_position = self.get_right_encoder() position = (((left_position + right_position) / 2) * (1 / UNITS_PER_REV) * DISTANCE_PER_REV) return position def drive(self, *args): self.differential_drive(*args) def differential_drive(self, y, rotation=0, squared=True, force=False, quick_turn=False, use_curvature=False): if not self.force_differential_drive: self.pending_differential_drive = DifferentialDriveConfig( y=y, rotation=rotation, squared=squared, quick_turn=quick_turn, use_curvature=use_curvature) self.force_differential_drive = force def turn(self, rotation=0, force=False): self.differential_drive(0, rotation, squared=False, force=force) def reset_angle_correction(self): self.navx.reset() def angle_corrected_differential_drive(self, y, rotation=0): ''' Heading must be reset first. (drivetrain.reset_angle_correction()) ''' # Scale angle to reduce max turn rotation = util.scale(rotation, -1, 1, -0.65, 0.65) # Scale y-speed in high gear if self.pending_gear == HIGH_GEAR: y = util.scale(y, -1, 1, -0.75, 0.75) use_curvature = False quick_turn = False # Small rotation at lower speeds - and also do a quick_turn instead of # the normal curvature-based mode. if abs(y) <= self.little_rotation_cutoff: rotation = util.abs_clamp(rotation, 0, 0.7) quick_turn = True # NEVER USE CURVATURE # Curvature drive for high gear and high speedz # if self.pending_gear == HIGH_GEAR and abs(y) >= 0.5: # use_curvature = True # Angle correction if abs(rotation) <= self.angle_correction_cutoff: heading = self.navx.getYaw() if not self.og_yaw: self.og_yaw = heading factor = self.angle_correction_factor_high_gear if \ self.pending_gear == HIGH_GEAR else \ self.angle_correction_factor_low_gear rotation = util.abs_clamp(-factor * (heading - self.og_yaw), 0, self.angle_correction_max) else: self.og_yaw = None self.differential_drive(y, rotation, quick_turn=quick_turn, use_curvature=use_curvature) def shift_low_gear(self): self.pending_gear = LOW_GEAR def shift_high_gear(self): self.pending_gear = HIGH_GEAR def shift_toggle(self): if self.pending_gear == HIGH_GEAR: self.pending_gear = LOW_GEAR else: self.pending_gear = HIGH_GEAR def manual_drive(self, left, right): self.pending_manual_drive = [left, right] def get_left_encoder(self): return -self.left_motor_master.getQuadraturePosition() def get_right_encoder(self): return self.right_motor_master.getQuadraturePosition() def get_left_encoder_meters(self): return self.get_left_encoder() * \ (1 / UNITS_PER_REV) * DISTANCE_PER_REV_METERS def get_right_encoder_meters(self): return self.get_right_encoder() * \ (1 / UNITS_PER_REV) * DISTANCE_PER_REV_METERS def get_left_encoder_velocity(self): return -self.left_motor_master.getQuadratureVelocity() def get_right_encoder_velocity(self): return self.right_motor_master.getQuadratureVelocity() def get_left_encoder_velocity_meters(self): return self.get_left_encoder_velocity() * \ (1 / UNITS_PER_REV) * DISTANCE_PER_REV_METERS def get_right_encoder_velocity_meters(self): return self.get_right_encoder_velocity() * \ (1 / UNITS_PER_REV) * DISTANCE_PER_REV_METERS def set_manual_mode(self, is_manual): self.is_manual_mode = is_manual if not is_manual: self.pending_manual_drive = None def execute(self): # print('exec drivetrain', self.pending_differential_drive) # print('dist_traveled_meters', self.get_left_encoder_meters(), # self.get_right_encoder_meters()) # Shifter self.shifter_solenoid.set(self.pending_gear) # Manual if self.is_manual_mode: if self.pending_manual_drive: left, right = self.pending_manual_drive # left = self.robot_drive.applyDeadband(left, DEADBAND) # right = self.robot_drive.applyDeadband(right, DEADBAND) self.left_motor_master.set(-left) self.right_motor_master.set(right) self.pending_manual_drive = None return # Drive if self.pending_differential_drive: if self.pending_differential_drive.use_curvature and \ abs(self.pending_differential_drive.y) > \ self.arcade_cutoff and \ USE_CURVATURE_DRIVE: self.robot_drive.curvatureDrive( self.pending_differential_drive.y, -self.pending_differential_drive.rotation, isQuickTurn=self.pending_differential_drive.quick_turn) else: self.robot_drive.arcadeDrive( self.pending_differential_drive.y, -self.pending_differential_drive.rotation, squareInputs=self.pending_differential_drive.squared) self.pending_differential_drive = None self.force_differential_drive = False def on_disable(self): self.robot_drive.arcadeDrive(0, 0) def get_state(self): return { 'pending_gear': self.pending_gear, 'pending_differential_drive': self.pending_differential_drive } def put_state(self, state): self.pending_gear = state['pending_gear'] self.pending_differential_drive = DifferentialDriveConfig._make( state['pending_differential_drive']) def limelight_turn(self): self.llt = NetworkTables.getTable('limelight') self.tv = self.llt.getNumber('tv', 0) self.tx = self.llt.getNumber('tx', 0) if self.tv: self.turn(self.get_position() + self.tx)
class Robot(wpilib.IterativeRobot): WHEEL_DIAMETER = 0.1524 # Units: Meters # Currently unused # ENCODER_PULSE_PER_REV = 42 GEARING = 7.56 # 7.56:1 gear ratio auto_speed_entry = ntproperty('/robot/autospeed', 0.0) telemetry_entry = ntproperty('/robot/telemetry', [0.0], writeDefault=False) rotate_entry = ntproperty('/robot/rotate', False) l_encoder_pos = ntproperty('/l_encoder_pos', 0) l_encoder_rate = ntproperty('/l_encoder_rate', 0) r_encoder_pos = ntproperty('/r_encoder_pos', 0) r_encoder_rate = ntproperty('/r_encoder_rate', 0) def robotInit(self): self.prior_autospeed = 0 self.joystick = wpilib.Joystick(0) self.left_motor_master = CANSparkMax(1, MotorType.kBrushless) self.right_motor_master = CANSparkMax(4, MotorType.kBrushless) # Set up Speed Controller Groups self.left_motors = wpilib.SpeedControllerGroup( self.left_motor_master, CANSparkMax(3, MotorType.kBrushless) ) self.right_motors = wpilib.SpeedControllerGroup( self.right_motor_master, CANSparkMax(6, MotorType.kBrushless) ) # Configure Gyro # Note that the angle from the NavX and all implementors of wpilib Gyro # must be negated because getAngle returns a clockwise positive angle self.gyro = navx.AHRS.create_spi() # Configure drivetrain movement self.drive = DifferentialDrive(self.left_motors, self.right_motors) self.drive.setDeadband(0) # Configure encoder related functions -- getDistance and getrate should return # units and units/s self.encoder_constant = (1 / self.GEARING) * self.WHEEL_DIAMETER * math.pi self.left_encoder = self.left_motor_master.getEncoder() self.left_encoder.setPositionConversionFactor(self.encoder_constant) self.left_encoder.setVelocityConversionFactor(self.encoder_constant / 60) self.right_encoder = self.right_motor_master.getEncoder() self.right_encoder.setPositionConversionFactor(self.encoder_constant) self.right_encoder.setVelocityConversionFactor(self.encoder_constant / 60) self.left_encoder.setPosition(0) self.right_encoder.setPosition(0) # Set the update rate instead of using flush because of a ntcore bug # -> probably don't want to do this on a robot in competition NetworkTables.getDefault().setUpdateRate(0.010) def disabledInit(self): print('Robot disabled') self.drive.tankDrive(0, 0) def robotPeriodic(self): # feedback for users, but not used by the control program self.l_encoder_pos = self.left_encoder.getPosition() self.l_encoder_rate = self.left_encoder.getVelocity() self.r_encoder_pos = self.right_encoder.getPosition() self.r_encoder_rate = self.right_encoder.getVelocity() def teleopInit(self): print('Robot in operator control mode') def teleopPeriodic(self): self.drive.arcadeDrive(-self.joystick.getY(), self.joystick.getX()) print(f'Left Distance: {self.left_encoder.getPosition()}') def autonomousInit(self): print('Robot in autonomous mode') # If you wish to just use your own robot program to use with the data logging # program, you only need to copy/paste the logic below into your code and # ensure it gets called periodically in autonomous mode # Additionally, you need to set NetworkTables update rate to 10ms using the # setUpdateRate call. def autonomousPeriodic(self): # Retrieve values to send back before telling the motors to do somethin now = wpilib.Timer.getFPGATimestamp() leftPosition = self.left_encoder.getPosition() leftRate = self.left_encoder.getVelocity() rightPosition = self.right_encoder.getPosition() rightRate = self.right_encoder.getVelocity() battery = wpilib.RobotController.getInputVoltage() motorVolts = battery * abs(self.prior_autospeed) leftMotorVolts = motorVolts rightMotorVolts = motorVolts # Retrieve the commanded speed from NetworkTables autospeed = self.auto_speed_entry self.prior_autospeed = autospeed # command motors to do things self.drive.tankDrive((-1 if self.rotate_entry else 1) * autospeed, autospeed, False) # send telemetry data array back to NT number_array = [ now, battery, autospeed, leftMotorVolts, rightMotorVolts, leftPosition, rightPosition, leftRate, rightRate, math.radians(-self.gyro.getAngle()) ] self.telemetry_entry = number_array
class MyRobot(wpilib.TimedRobot): '''Main robot class''' # NetworkTables API for controlling this #: Speed to set the motors to autospeed = ntproperty('/robot/autospeed', defaultValue=0, writeDefault=True) #: Test data that the robot sends back telemetry = ntproperty('/robot/telemetry', defaultValue=(0, ) * 9, writeDefault=False) prior_autospeed = 0 WHEEL_DIAMETER = 0.5 ENCODER_PULSE_PER_REV = 360 def robotInit(self): '''Robot-wide initialization code should go here''' self.lstick = wpilib.Joystick(0) left_front_motor = wpilib.Spark(1) left_front_motor.setInverted(False) right_front_motor = wpilib.Spark(2) right_front_motor.setInverted(False) left_rear_motor = wpilib.Spark(3) left_rear_motor.setInverted(False) right_rear_motor = wpilib.Spark(4) right_rear_motor.setInverted(False) # # Configure drivetrain movement # l = wpilib.SpeedControllerGroup(left_front_motor, left_rear_motor) r = wpilib.SpeedControllerGroup(right_front_motor, right_rear_motor) self.drive = DifferentialDrive(l, r) self.drive.setSafetyEnabled(False) self.drive.setDeadband(0) # # Configure encoder related functions -- getpos and getrate should return # ft and ft/s # encoder_constant = ( 1 / self.ENCODER_PULSE_PER_REV) * self.WHEEL_DIAMETER * math.pi l_encoder = wpilib.Encoder(0, 1) l_encoder.setDistancePerPulse(encoder_constant) self.l_encoder_getpos = l_encoder.getDistance self.l_encoder_getrate = l_encoder.getRate r_encoder = wpilib.Encoder(2, 3) r_encoder.setDistancePerPulse(encoder_constant) self.r_encoder_getpos = r_encoder.getDistance self.r_encoder_getrate = r_encoder.getRate # Set the update rate instead of using flush because of a NetworkTables bug # that affects ntcore and pynetworktables # -> probably don't want to do this on a robot in competition NetworkTables.setUpdateRate(0.010) def disabledInit(self): self.logger.info("Robot disabled") self.drive.tankDrive(0, 0) def disabledPeriodic(self): pass def robotPeriodic(self): # feedback for users, but not used by the control program sd = wpilib.SmartDashboard sd.putNumber('l_encoder_pos', self.l_encoder_getpos()) sd.putNumber('l_encoder_rate', self.l_encoder_getrate()) sd.putNumber('r_encoder_pos', self.r_encoder_getpos()) sd.putNumber('r_encoder_rate', self.r_encoder_getrate()) def teleopInit(self): self.logger.info("Robot in operator control mode") def teleopPeriodic(self): self.drive.arcadeDrive(-self.lstick.getY(), self.lstick.getX()) def autonomousInit(self): self.logger.info("Robot in autonomous mode") def autonomousPeriodic(self): ''' If you wish to just use your own robot program to use with the data logging program, you only need to copy/paste the logic below into your code and ensure it gets called periodically in autonomous mode Additionally, you need to set NetworkTables update rate to 10ms using the setUpdateRate call. Note that reading/writing self.autospeed and self.telemetry are NetworkTables operations (using pynetworktables's ntproperty), so if you don't read/write NetworkTables in your implementation it won't actually work. ''' # Retrieve values to send back before telling the motors to do something now = wpilib.Timer.getFPGATimestamp() l_encoder_position = self.l_encoder_getpos() l_encoder_rate = self.l_encoder_getrate() r_encoder_position = self.r_encoder_getpos() r_encoder_rate = self.r_encoder_getrate() battery = self.ds.getBatteryVoltage() motor_volts = battery * abs(self.prior_autospeed) l_motor_volts = motor_volts r_motor_volts = motor_volts # Retrieve the commanded speed from NetworkTables autospeed = self.autospeed self.prior_autospeed = autospeed # command motors to do things self.drive.tankDrive(autospeed, autospeed, False) # send telemetry data array back to NT self.telemetry = (now, battery, autospeed, l_motor_volts, r_motor_volts, l_encoder_position, r_encoder_position, l_encoder_rate, r_encoder_rate)
class DriverComponent: CONV_FACTOR = 0.0524 * 0.846 LINEAR_SAMPLE_RATE = 28 ANGULAR_SAMPLE_RATE = 2 def __init__(self): left_front = WPI_TalonSRX(6) left_rear = WPI_TalonSRX(1) right_front = WPI_TalonSRX(4) right_rear = WPI_TalonSRX(7) left = SpeedControllerGroup(left_front, left_rear) right = SpeedControllerGroup(right_front, right_rear) self.left_encoder_motor = left_rear self.right_encoder_motor = right_front self.gear_solenoid = DoubleSolenoid(0, 1) self.driver_gyro = ADXRS450_Gyro() self.drive_train = DifferentialDrive(left, right) # setup encoders self.left_encoder_motor.setSensorPhase(True) self.drive_train.setDeadband(0.1) self.moving_linear = [0] * DriverComponent.LINEAR_SAMPLE_RATE self.moving_angular = [0] * DriverComponent.ANGULAR_SAMPLE_RATE def set_curve_raw(self, linear, angular): if -0.1 < linear < 0.1: self.drive_train.curvatureDrive(linear, angular, True) else: self.drive_train.curvatureDrive(linear, angular, False) def set_curve(self, linear, angular): self.moving_linear.append(linear) self.moving_angular.append(angular) if len(self.moving_linear) > DriverComponent.LINEAR_SAMPLE_RATE: self.moving_linear.pop(0) if len(self.moving_angular) > DriverComponent.ANGULAR_SAMPLE_RATE: self.moving_angular.pop(0) l_speed = sum([ x / DriverComponent.LINEAR_SAMPLE_RATE for x in self.moving_linear ]) a_speed = sum([ x / DriverComponent.ANGULAR_SAMPLE_RATE for x in self.moving_angular ]) l_speed = math.sin(l_speed * math.pi / 2) if -0.1 < l_speed < 0.1: self.drive_train.curvatureDrive(l_speed, a_speed, True) else: self.drive_train.curvatureDrive(l_speed, a_speed, False) def reset_drive_sensors(self): self.driver_gyro.reset() self.left_encoder_motor.setSelectedSensorPosition(0, 0, 0) self.right_encoder_motor.setSelectedSensorPosition(0, 0, 0) @property def current_distance(self): return DriverComponent.CONV_FACTOR * self.left_encoder_motor.getSelectedSensorPosition( 0) def current_gear(self): if self.gear_solenoid.get() is DoubleSolenoid.Value.kForward: return GearMode.HIGH if self.gear_solenoid.get() is DoubleSolenoid.Value.kReverse: return GearMode.LOW if self.gear_solenoid.get() is DoubleSolenoid.Value.kOff: return GearMode.OFF def toggle_gear(self): if self.current_gear() is GearMode.LOW: self.set_high_gear() if self.current_gear() is GearMode.HIGH: self.set_low_gear() def set_low_gear(self): print("shift low") self.gear_solenoid.set(DoubleSolenoid.Value.kReverse) def set_high_gear(self): print("shift high") self.gear_solenoid.set(DoubleSolenoid.Value.kForward)