class SwerveModule(): def __init__(self, driveCanTalonId, steerCanTalonId, absoluteEncoder=True, reverseDrive=False): # Initialise private motor controllers self._drive = CANTalon(driveCanTalonId) self.reverse_drive = reverseDrive self._steer = CANTalon(steerCanTalonId) self.absoluteEncoder = absoluteEncoder # Set up the motor controllers # Different depending on whether we are using absolute encoders or not if absoluteEncoder: self._steer.changeControlMode(CANTalon.ControlMode.Position) self._steer.setFeedbackDevice( CANTalon.FeedbackDevice.AnalogEncoder) else: self._steer.changeControlMode(CANTalon.ControlMode.Position) self._steer.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder) self._steer.setPID(RobotMap.steering_p, 0.0, 0.0) self._steer.setPosition(0.0) # Private members to store the setpoints self._speed = 0.0 self._direction = 0.0 self._opposite_direction = math.pi # Always in radians. Right hand rule applies - Z is up! # Rescale values to the range [-pi, pi) def steer(self, direction, speed=0): # Set the speed and direction of the swerve module # Always choose the direction that minimises movement, # even if this means reversing the drive motor direction = constrain_angle(direction) # rescale to +/-pi current_heading = constrain_angle(self._direction) delta = min_angular_displacement(current_heading, direction) self._direction += delta if self.reverse_drive: speed = -speed if abs(constrain_angle(self._direction) - direction) < math.pi / 6.0: self._drive.set(speed) self._speed = speed else: self._drive.set(-speed) self._speed = -speed if self.absoluteEncoder: self._steer.set(self._direction / TAU * RobotMap.module_rotation_volts_per_revolution) else: self._steer.set(self._direction / TAU * RobotMap.module_rotation_counts_per_revolution) def getSpeed(self): return self._speed def getDirection(self): return self._direction
class SwerveModule(): def __init__(self, driveCanTalonId, steerCanTalonId, absoluteEncoder = True, reverseDrive = False): # Initialise private motor controllers self._drive = CANTalon(driveCanTalonId) self.reverse_drive = reverseDrive self._steer = CANTalon(steerCanTalonId) self.absoluteEncoder = absoluteEncoder # Set up the motor controllers # Different depending on whether we are using absolute encoders or not if absoluteEncoder: self._steer.changeControlMode(CANTalon.ControlMode.Position) self._steer.setFeedbackDevice(CANTalon.FeedbackDevice.AnalogEncoder) else: self._steer.changeControlMode(CANTalon.ControlMode.Position) self._steer.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder) self._steer.setPID(RobotMap.steering_p, 0.0, 0.0) self._steer.setPosition(0.0) # Private members to store the setpoints self._speed = 0.0 self._direction = 0.0 self._opposite_direction = math.pi # Always in radians. Right hand rule applies - Z is up! # Rescale values to the range [-pi, pi) def steer(self, direction, speed = 0): # Set the speed and direction of the swerve module # Always choose the direction that minimises movement, # even if this means reversing the drive motor direction = constrain_angle(direction) # rescale to +/-pi current_heading = constrain_angle(self._direction) delta = min_angular_displacement(current_heading, direction) self._direction += delta if self.reverse_drive: speed=-speed if abs(constrain_angle(self._direction)-direction)<math.pi/6.0: self._drive.set(speed) self._speed = speed else: self._drive.set(-speed) self._speed = -speed if self.absoluteEncoder: self._steer.set(self._direction/TAU*RobotMap.module_rotation_volts_per_revolution) else: self._steer.set(self._direction/TAU*RobotMap.module_rotation_counts_per_revolution) def getSpeed(self): return self._speed def getDirection(self): return self._direction
class SwerveModule(): def __init__(self, drive, steer, absolute=True, reverse_drive=False, reverse_steer=False, zero_reading=0, drive_encoder=False, reverse_drive_encoder=False): # Initialise private motor controllers self._drive = CANTalon(drive) self.reverse_drive = reverse_drive self._steer = CANTalon(steer) self.drive_encoder = drive_encoder self._distance_offset = 0 # Offset the drive distance counts # Set up the motor controllers # Different depending on whether we are using absolute encoders or not if absolute: self.counts_per_radian = 1024.0 / (2.0 * math.pi) self._steer.setFeedbackDevice(CANTalon.FeedbackDevice.AnalogEncoder) self._steer.changeControlMode(CANTalon.ControlMode.Position) self._steer.reverseSensor(reverse_steer) self._steer.reverseOutput(not reverse_steer) # Read the current encoder position self._steer.setPID(20.0, 0.0, 0.0) # PID values for abs self._offset = zero_reading - 256.0 if reverse_steer: self._offset = -self._offset else: self._steer.changeControlMode(CANTalon.ControlMode.Position) self._steer.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder) self._steer.setPID(6.0, 0.0, 0.0) # PID values for rel self.counts_per_radian = (497.0 * (40.0 / 48.0) * 4.0 / (2.0 * math.pi)) self._offset = self.counts_per_radian*2.0*math.pi/4.0 self._steer.setPosition(0.0) if self.drive_encoder: self.drive_counts_per_rev = 80*6.67 self.drive_counts_per_metre = (self.drive_counts_per_rev / (math.pi * 0.1016)) self.drive_max_speed = 570 self._drive.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder) self.changeDriveControlMode(CANTalon.ControlMode.Speed) self._drive.reverseSensor(reverse_drive_encoder) else: self.drive_counts_per_rev = 0.0 self.drive_max_speed = 1.0 self.changeDriveControlMode(CANTalon.ControlMode.PercentVbus) self._drive.setVoltageRampRate(150.0) def changeDriveControlMode(self, control_mode): if self._drive.getControlMode is not control_mode: if control_mode == CANTalon.ControlMode.Speed: self._drive.setPID(1.0, 0.00, 0.0, 1023.0 / self.drive_max_speed) elif control_mode == CANTalon.ControlMode.Position: self._drive.setPID(0.1, 0.0, 0.0, 0.0) self._drive.changeControlMode(control_mode) @property def direction(self): # Read the current direction from the controller setpoint setpoint = self._steer.getSetpoint() return float(setpoint - self._offset) / self.counts_per_radian @property def speed(self): # Read the current speed from the controller setpoint setpoint = self._drive.getSetpoint() return float(setpoint) @property def distance(self): # Read the current position from the encoder and remove the offset return self._drive.getEncPosition() - self._distance_offset def zero_distance(self): self._distance_offset = self._drive.getEncPosition() def steer(self, direction, speed=None): if self.drive_encoder: self.changeDriveControlMode(CANTalon.ControlMode.Speed) else: self.changeDriveControlMode(CANTalon.ControlMode.PercentVbus) # Set the speed and direction of the swerve module # Always choose the direction that minimises movement, # even if this means reversing the drive motor if speed is None: # Force the modules to the direction specified - don't # go to the closest one and reverse. delta = constrain_angle(direction - self.direction) # rescale to +/-pi self._steer.set((self.direction + delta) * self.counts_per_radian + self._offset) self._drive.set(0.0) return if abs(speed) > 0.05: direction = constrain_angle(direction) # rescale to +/-pi current_heading = constrain_angle(self.direction) delta = min_angular_displacement(current_heading, direction) if self.reverse_drive: speed = -speed if abs(constrain_angle(self.direction - direction)) < math.pi / 6.0: self._drive.set(speed*self.drive_max_speed) else: self._drive.set(-speed*self.drive_max_speed) self._steer.set((self.direction + delta) * self.counts_per_radian + self._offset) else: self._drive.set(0.0)
class DriveTrain(Subsystem): """DriveTrain: is the subsystem responsible for motors and devices associated with driving subystem. As a subsystem, we represent the single point of contact for all drivetrain-related controls. Specifically, commands that manipulate the drivetrain should 'require' the singleton instance (via require(robot.driveTrain)). Unless overridden, our default command, JoystickDriver, is the means by which driving occurs. """ k_minThrottleScale = 0.5 k_defaultDriveSpeed = 100.0 # ~13.0 ft/sec determined experimentally k_maxDriveSpeed = 150.0 # ~20 ft/sec k_maxTurnSpeed = 40.0 # ~3-4 ft/sec k_fastTurn = -1 k_mediumTurn = -.72 k_slowTurn = -.55 k_quadTicksPerWheelRev = 9830 k_wheelDiameterInInches = 14.0 k_wheelCircumferenceInInches = k_wheelDiameterInInches * math.pi k_quadTicksPerInch = k_quadTicksPerWheelRev / k_wheelCircumferenceInInches k_turnKp = .1 k_turnKi = 0 k_turnKd = .3 k_turnKf = .001 k_ControlModeSpeed = 0 k_ControlModeVBus = 1 k_ControlModeDisabled = 2 class _turnHelper(PIDOutput): """a private helper class for PIDController-based imu-guided turning. """ def __init__(self, driveTrain): super().__init__() self.driveTrain = driveTrain def pidWrite(self, output): self.driveTrain.turn(output * DriveTrain.k_maxTurnSpeed) def __init__(self, robot, name=None): super().__init__(name=name) self.robot = robot # STEP 1: instantiate the motor controllers self.leftMasterMotor = CANTalon(robot.map.k_DtLeftMasterId) self.leftFollowerMotor = CANTalon(robot.map.k_DtLeftFollowerId) self.rightMasterMotor = CANTalon(robot.map.k_DtRightMasterId) self.rightFollowerMotor = CANTalon(robot.map.k_DtRightFollowerId) # Step 2: Configure the follower Talons: left & right back motors self.leftFollowerMotor.changeControlMode(CANTalon.ControlMode.Follower) self.leftFollowerMotor.set(self.leftMasterMotor.getDeviceID()) self.rightFollowerMotor.changeControlMode(CANTalon.ControlMode.Follower) self.rightFollowerMotor.set(self.rightMasterMotor.getDeviceID()) # STEP 3: Setup speed control mode for the master Talons self.leftMasterMotor.changeControlMode(CANTalon.ControlMode.Speed) self.rightMasterMotor.changeControlMode(CANTalon.ControlMode.Speed) # STEP 4: Indicate the feedback device used for closed-loop # For speed mode, indicate the ticks per revolution self.leftMasterMotor.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder) self.rightMasterMotor.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder) self.leftMasterMotor.configEncoderCodesPerRev(self.k_quadTicksPerWheelRev) self.rightMasterMotor.configEncoderCodesPerRev(self.k_quadTicksPerWheelRev) # STEP 5: Set PID values & closed loop error self.leftMasterMotor.setPID(0.22, 0, 0) self.rightMasterMotor.setPID(0.22, 0, 0) # Add ramp up rate self.leftMasterMotor.setVoltageRampRate(48.0) # max allowable voltage # change /sec: reach to # 12V after 1sec self.rightMasterMotor.setVoltageRampRate(48.0) # Add SmartDashboard controls for testing # Add SmartDashboard live windowS LiveWindow.addActuator("DriveTrain", "Left Master %d" % robot.map.k_DtLeftMasterId, self.leftMasterMotor) LiveWindow.addActuator("DriveTrain", "Right Master %d" % robot.map.k_DtRightMasterId, self.rightMasterMotor) # init RobotDrive - all driving should occur through its methods # otherwise we expect motor saftey warnings self.robotDrive = RobotDrive(self.leftMasterMotor, self.rightMasterMotor) self.robotDrive.setSafetyEnabled(True) # init IMU - used for driver & vision feedback as well as for # some autonomous modes. self.visionState = self.robot.visionState self.imu = BNO055() self.turnPID = PIDController(self.k_turnKp, self.k_turnKd, self.k_turnKf, self.imu, DriveTrain._turnHelper(self)) self.turnPID.setOutputRange(-1, 1) self.turnPID.setInputRange(-180, 180) self.turnPID.setPercentTolerance(2) self.turnMultiplier = DriveTrain.k_mediumTurn self.maxSpeed = DriveTrain.k_defaultDriveSpeed # self.setContinuous() ? robot.info("Initialized DriveTrain") def initForCommand(self, controlMode): self.leftMasterMotor.setEncPosition(0) # async call self.rightMasterMotor.setEncPosition(0) self.robotDrive.stopMotor() self.robotDrive.setMaxOutput(self.maxSpeed) if controlMode == self.k_ControlModeSpeed: self.leftMasterMotor.changeControlMode(CANTalon.ControlMode.Speed) self.rightMasterMotor.changeControlMode(CANTalon.ControlMode.Speed) elif controlMode == self.k_ControlModeVBus: self.leftMasterMotor.changeControlMode(CANTalon.ControlMode.PercentVbus) self.rightMasterMotor.changeControlMode(CANTalon.ControlMode.PercentVbus) elif controlMode == self.k_ControlModeDisabled: # unverified codepath self.leftMasterMotor.disableControl() self.rightMasterMotor.disableControl() else: self.robot.error("Unexpected control mode") self.robot.info("driveTrain initDefaultCommand, controlmodes: %d %d" % (self.leftMasterMotor.getControlMode(), self.rightMasterMotor.getControlMode())) def initDefaultCommand(self): # control modes are integers values: # 0 percentvbux # 1 position # 2 velocity self.setDefaultCommand(JoystickDriver(self.robot)) self.robotDrive.stopMotor(); def updateDashboard(self): # TODO: additional items? SmartDashboard.putNumber("IMU heading", self.getCurrentHeading()) SmartDashboard.putNumber("IMU calibration", self.imu.getCalibration()) def stop(self): self.robotDrive.stopMotor() def joystickDrive(self, jsY, jsX, throttle): """ joystickDrive - called by JoystickDriver command. Inputs are always on the range [-1, 1]... These values can be scaled for a better "feel", but should always be in a subset of this range. """ if self.robot.isAutonomous or \ (math.fabs(jx) < 0.075 and math.fabs(jy) < .075): # joystick dead zone or auto (where we've been scheduled via # default command machinery) self.robotDrive.stopMotor() else: st = self.scaleThrottle(throttle) self.robotDrive.arcadeDrive(jsY*self.turnMultiplier, jsX*st) def drive(self, outputmag, curve): """ drive - used by drivestraight command.. """ self.robotDrive.drive(outputmag, curve) def driveStraight(self, speed): """driveStraight: invoked from AutoDriveStraight.. """ # NB: maxOuput isn't applied via set so # speed is supposedly measured in rpm but this depends on our # initialization establishing encoding ticks per revolution. # This is approximate so we rely on the observed values above. # (DEFAULT_SPEED_MAX_OUTPUT) if 0: self.leftMasterMotor.set(speed) self.rightMasterMotor.set(speed) else: # TODO: validate this codepath moveValue = speed / self.maxSpeed rotateValue = 0 self.robotDrive.arcadeDrive(moveValue, rotateValue) def startAutoTurn(self, degrees): self.robot.info("start autoturn from %d to %d" % (self.getHeading(), degrees)) self.turnPID.reset() self.turnPID.setSetpoint(degrees) self.turnPID.enable() def endAutoTurn(self): if self.turnPID.isEnabled(): self.turnPID.disable() def isAutoTurning(self): return self.turnPID.isEnabled() def isAutoTurnFinished(self): return self.turnPID.onTarget() def turn(self, speed): """turn takes a speed, not an angle... A negative speed is interpretted as turn left. Note that we bypass application of setMaxOutput Which only applies to calls to robotDrive. """ # In order to turn left, we want the right wheels to go # forward and left wheels to go backward (cf: tankDrive) # Oddly, our right master motor is reversed, so we compensate here. # speed < 0: turn left: rightmotor(negative) (forward), # leftmotor(negative) (backward) # speed > 0: turn right: rightmotor(positive) (backward) # leftmotor(positive) (forward) if 0: self.leftMasterMotor.set(speed) self.rightMasterMotor.set(speed) else: # TODO: validate this codepath moveValue = 0 rotateValue = speed / self.maxSpeed self.robotDrive.arcadeDrive(moveValue, rotateValue) def trackVision(self): """presumed called by either autonomous or teleop commands to allow the vision subsystem to guide the robot """ if self.visionState.DriveLockedOnTarget: self.stop() else: if self.isAutoTurning(): if self.isAutoTurnFinished(): self.endAutoTurn() self.visionState.DriveLockedOnTarget = True else: # setup for an auto-turn h = self.getCurrentHeading() tg = self.getTargetHeading(h) self.startAutoTurn(tg) def getCurrentHeading(self): """ getCurrentHeading returns a value between -180 and 180 """ return math.degrees(self.imu.getHeading()) # getHeading: -pi, pi def scaleThrottle(self, rawThrottle): """ scaleThrottle: returns a scaled value between MIN_THROTTLE_SCALE and 1.0 MIN_THROTTLE_SCALE must be set to the lowest useful scale value through experimentation Scale the joystick values by throttle before passing to the driveTrain +1=bottom position; -1=top position """ # Throttle in the range of -1 to 1. We would like to change that to a # range of MIN_THROTTLE_SCALE to 1. #First, multiply the raw throttle # value by -1 to reverse it (makes "up" maximum (1), and "down" minimum (-1)) # Then, add 1 to make the range 0-2 rather than -1 to +1 # Then multiply by ((1-k_minThrottleScale)/2) to change the range to 0-(1-k_minThrottleScale) # Finally add k_minThrottleScale to change the range to k_minThrottleScale to 1 # # Check the results are in the range of k_minThrottleScale to 1, and clip # it in case the math went horribly wrong. result = ((rawThrottle * -1) + 1) * ((1-self.k_minThrottleScale) / 2) + self.k_minThrottleScale if result < self.k_minThrottleScale: # Somehow our math was wrong. Our value was too low, so force it to the minimum result = self.k_mintThrottleScale elif result > 1: # Somehow our math was wrong. Our value was too high, so force it to the maximum result = 1.0 return result def modifyTurnSpeed(self, speedUp): if speedUp: self.turnMultiplier = self.k_mediumTurn else: self.turnMultiplier = self.k_slowTurn def inchesToTicks(self, inches): return int(self.k_quadTicksPerInch * inches) def destinationReached(self, distance): return math.fabs(self.leftMasterMotor.getEncPosition()) >= distance or \ math.fabs(self.rightMasterMotr.getEncPosition()) >= distance
class driveTrain(Component) : def __init__(self, robot): super().__init__() self.robot = robot # Constants WHEEL_DIAMETER = 8 PI = 3.1415 ENCODER_TICK_COUNT_250 = 250 ENCODER_TICK_COUNT_360 = 360 ENCODER_GOAL = 0 # default ENCODER_TOLERANCE = 1 # inch0 self.RPM = 4320/10.7 self.INCHES_PER_REV = WHEEL_DIAMETER * 3.1415 self.CONTROL_TYPE = False # False = disable PID components self.LEFTFRONTCUMULATIVE = 0 self.LEFTBACKCUMULATIVE = 0 self.RIGHTFRONTCUMULATIVE = 0 self.RIGHTBACKCUMULATIVE = 0 self.rfmotor = CANTalon(0) self.rbmotor = CANTalon(1) self.lfmotor = CANTalon(2) self.lbmotor = CANTalon(3) self.lfmotor.reverseOutput(True) self.lbmotor.reverseOutput(True) #self.rfmotor.reverseOutput(True) #self.rbmotor.reverseOutput(True)#practice bot only self.rfmotor.enableBrakeMode(True) self.rbmotor.enableBrakeMode(True) self.lfmotor.enableBrakeMode(True) self.lbmotor.enableBrakeMode(True) absolutePosition = self.lbmotor.getPulseWidthPosition() & 0xFFF; # mask out the bottom12 bits, we don't care about the wrap arounds use the low level API to set the quad encoder signal self.lbmotor.setEncPosition(absolutePosition) absolutePosition = self.lfmotor.getPulseWidthPosition() & 0xFFF; # mask out the bottom12 bits, we don't care about the wrap arounds use the low level API to set the quad encoder signal self.lfmotor.setEncPosition(absolutePosition) absolutePosition = self.rbmotor.getPulseWidthPosition() & 0xFFF; # mask out the bottom12 bits, we don't care about the wrap arounds use the low level API to set the quad encoder signal self.rbmotor.setEncPosition(absolutePosition) absolutePosition = self.rfmotor.getPulseWidthPosition() & 0xFFF; # mask out the bottom12 bits, we don't care about the wrap arounds use the low level API to set the quad encoder signal self.rfmotor.setEncPosition(absolutePosition) self.rfmotor.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative) self.rbmotor.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative) self.lfmotor.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative) self.lbmotor.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative) #setting up the distances per rotation self.lfmotor.configEncoderCodesPerRev(4096) self.rfmotor.configEncoderCodesPerRev(4096) self.lbmotor.configEncoderCodesPerRev(4096) self.rbmotor.configEncoderCodesPerRev(4096) self.lfmotor.setPID(0.0005, 0, 0.0, profile=0) self.rfmotor.setPID(0.0005, 0, 0.0, profile=0) self.lbmotor.setPID(0.0005, 0, 0.0, profile=0) self.rbmotor.setPID(0.0005, 0, 0.0, profile=0) self.lbmotor.configNominalOutputVoltage(+0.0, -0.0) self.lbmotor.configPeakOutputVoltage(+12.0, -12.0) self.lbmotor.setControlMode(CANTalon.ControlMode.Speed) self.lfmotor.configNominalOutputVoltage(+0.0, -0.0) self.lfmotor.configPeakOutputVoltage(+12.0, -12.0) self.lfmotor.setControlMode(CANTalon.ControlMode.Speed) self.rbmotor.configNominalOutputVoltage(+0.0, -0.0) self.rbmotor.configPeakOutputVoltage(+12.0, -12.0) self.rbmotor.setControlMode(CANTalon.ControlMode.Speed) self.rfmotor.configNominalOutputVoltage(+0.0, -0.0) self.rfmotor.configPeakOutputVoltage(+12.0, -12.0) self.rfmotor.setControlMode(CANTalon.ControlMode.Speed) self.rfmotor.setPosition(0) self.rbmotor.setPosition(0) self.lfmotor.setPosition(0) self.lbmotor.setPosition(0) self.lfmotor.reverseSensor(True) self.lbmotor.reverseSensor(True) ''' # changing the encoder output from DISTANCE to RATE (we're dumb) self.lfencoder.setPIDSourceType(wpilib.PIDController.PIDSourceType.kRate) self.lbencoder.setPIDSourceType(wpilib.PIDController.PIDSourceType.kRate) self.rfencoder.setPIDSourceType(wpilib.PIDController.PIDSourceType.kRate) self.rbencoder.setPIDSourceType(wpilib.PIDController.PIDSourceType.kRate) # LiveWindow settings (Encoder) wpilib.LiveWindow.addSensor("Drive Train", "Left Front Encoder", self.lfencoder) wpilib.LiveWindow.addSensor("Drive Train", "Right Front Encoder", self.rfencoder) wpilib.LiveWindow.addSensor("Drive Train", "Left Back Encoder", self.lbencoder) wpilib.LiveWindow.addSensor("Drive Train", "Right Back Encoder", self.rbencoder) ''' ''' # Checking the state of the encoders on the Smart Dashboard wpilib.SmartDashboard.putBoolean("Right Front Encoder Enabled?", self.rfmotor.isSensorPresent) wpilib.SmartDashboard.putBoolean("Right Back Encoder Enabled?", self.rbmotor.isSensorPresent) wpilib.SmartDashboard.putBoolean("Left Front Encoder Enabled?", self.lfmotor.isSensorPresent) wpilib.SmartDashboard.putBoolean("Left Back Encoder Enabled?", self.lbmotor.isSensorPresent) ''' if self.CONTROL_TYPE: # Initializing PID Controls self.pidRightFront = wpilib.PIDController(0.002, 0.8, 0.005, 0, self.rfmotor.feedbackDevice, self.rfmotor, 0.02) self.pidLeftFront = wpilib.PIDController(0.002, 0.8, 0.005, 0, self.lfmotor.feedbackDevice, self.lfmotor, 0.02) self.pidRightBack = wpilib.PIDController(0.002, 0.8, 0.005, 0, self.rbmotor.feedbackDevice, self.rbmotor, 0.02) self.pidLeftBack = wpilib.PIDController(0.002, 0.8, 0.005, 0, self.lbmotor.feedbackDevice, self.lbmotor, 0.02) # PID Absolute Tolerance Settings self.pidRightFront.setAbsoluteTolerance(0.05) self.pidLeftFront.setAbsoluteTolerance(0.05) self.pidRightBack.setAbsoluteTolerance(0.05) self.pidLeftBack.setAbsoluteTolerance(0.05) # PID Output Range Settings self.pidRightFront.setOutputRange(-1, 1) self.pidLeftFront.setOutputRange(-1, 1) self.pidRightBack.setOutputRange(-1, 1) self.pidLeftBack.setOutputRange(-1, 1) # Enable PID #self.enablePIDs() ''' # LiveWindow settings (PID) wpilib.LiveWindow.addActuator("Drive Train Right", "Right Front PID", self.pidRightFront) wpilib.LiveWindow.addActuator("Drive Train Left", "Left Front PID", self.pidLeftFront) wpilib.LiveWindow.addActuator("Drive Train Right", "Right Back PID", self.pidRightBack) wpilib.LiveWindow.addActuator("Drive Train Left", "Left Back PID", self.pidLeftBack) ''' self.dashTimer = Timer() # Timer for SmartDashboard updating self.dashTimer.start() ''' # Adding components to the LiveWindow (testing) wpilib.LiveWindow.addActuator("Drive Train Left", "Left Front Motor", self.lfmotor) wpilib.LiveWindow.addActuator("Drive Train Right", "Right Front Motor", self.rfmotor) wpilib.LiveWindow.addActuator("Drive Train Left", "Left Back Motor", self.lbmotor) wpilib.LiveWindow.addActuator("Drive Train Right", "Right Back Motor", self.rbmotor) ''' def log(self): #The log method puts interesting information to the SmartDashboard. (like velocity information) ''' #no longer implemented because of change of hardware wpilib.SmartDashboard.putNumber("Left Front Speed", self.lfmotor.getEncVelocity()) wpilib.SmartDashboard.putNumber("Right Front Speed", self.rfmotor.getEncVelocity()) wpilib.SmartDashboard.putNumber("Left Back Speed", self.lbmotor.getEncVelocity()) wpilib.SmartDashboard.putNumber("Right Back Speed", self.rbmotor.getEncVelocity()) ''' wpilib.SmartDashboard.putNumber("RF Mag Enc Position", self.rfmotor.getPosition()) wpilib.SmartDashboard.putNumber("RB Mag Enc Position", self.rbmotor.getPosition()) wpilib.SmartDashboard.putNumber("LF Mag Enc Position", self.lfmotor.getPosition()) wpilib.SmartDashboard.putNumber("LB Mag Enc Position", self.lbmotor.getPosition()) ''' wpilib.SmartDashboard.putNumber("Right Front Mag Distance(inches)", self.convertEncoderRaw(self.rfmotor.getPosition()*0.57)) wpilib.SmartDashboard.putNumber("Right Back Mag Distance(inches)", self.convertEncoderRaw(self.rbmotor.getPosition()*0.57)) wpilib.SmartDashboard.putNumber("Left Front Mag Distance(inches)", self.convertEncoderRaw(self.lfmotor.getPosition()*0.57)) wpilib.SmartDashboard.putNumber("Left Back Mag Distance(inches)", self.convertEncoderRaw(self.lbmotor.getPosition()*0.57)) ''' # drive forward function def drive_forward(self, speed) : self.drive.tankDrive(speed, speed, True) # manual drive function for Tank Drive def xboxTankDrive(self, leftSpeed, rightSpeed, leftB, rightB, leftT, rightT): #self.lfmotor.setCloseLoopRampRate(1) #self.lbmotor.setCloseLoopRampRate(1) #self.rfmotor.setCloseLoopRampRate(1) #self.rbmotor.setCloseLoopRampRate(1) if (leftB == True): #Straight Button rightSpeed = leftSpeed if (rightB == True): #Slow Button #leftSpeed = leftSpeed/1.75 #rightSpeed = rightSpeed/1.75 if(not(leftSpeed < -0.5 and rightSpeed > 0.5 or leftSpeed > -0.5 and rightSpeed < 0.5)): #only do t if not turning leftSpeed = leftSpeed/1.75 rightSpeed = rightSpeed/1.75 # Fast button if(rightT == True): #self.lfmotor.setCloseLoopRampRate(24) #self.lbmotor.setCloseLoopRampRate(24) #self.rfmotor.setCloseLoopRampRate(24) #self.rbmotor.setCloseLoopRampRate(24) leftSpeed = leftSpeed*(1.75) rightSpeed = rightSpeed*(1.75) if(leftT == True): leftSpeed = 0.1 rightSpeed = 0.1 # Creating margin for error when using the joysticks, as they're quite sensitive if abs(rightSpeed) < 0.04 : rightSpeed = 0 if abs(leftSpeed) < 0.04 : leftSpeed = 0 if self.CONTROL_TYPE: self.pidRightFront.setSetpoint(rightSpeed) self.pidRightBack.setSetpoint(rightSpeed) self.pidLeftFront.setSetpoint(leftSpeed) self.pidLeftBack.setSetpoint(leftSpeed) else: self.lfmotor.set(leftSpeed*512) self.rfmotor.set(rightSpeed*512) self.lbmotor.set(leftSpeed*512) self.rbmotor.set(rightSpeed*512) #autononmous tank drive (to remove a need for a slow, striaght, or fast button) def autonTankDrive(self, leftSpeed, rightSpeed): self.log() #self.drive.tankDrive(leftSpeed, rightSpeed, True) self.rfmotor.set(rightSpeed) self.rbmotor.set(rightSpeed*(-1)) self.lfmotor.set(leftSpeed) self.lbmotor.set(leftSpeed*(-1)) # stop function def drive_stop(self) : self.drive.tankDrive(0,0) # fucntion to reset the PID's and encoder values def reset(self): self.rfmotor.setPosition(0) self.rbmotor.setPosition(0) self.lfmotor.setPosition(0) self.lbmotor.setPosition(0) if self.CONTROL_TYPE: self.LEFTFRONTCUMULATIVE = 0 self.RIGHTFRONTCUMULATIVE = 0 self.LEFTBACKCUMULATIVE= 0 self.RIGHTBACKCUMULATIVE = 0 self.pidLeftBack.setSetpoint(0) self.pidLeftFront.setSetpoint(0) self.pidRightBack.setSetpoint(0) self.pidRightFront.setSetpoint(0) # def getDistance(self) # return (abs(self.convertEncoderRaw(LEFTFRONTCUMULATIVE) + abs(self.convertEncoderRaw(LEFTBACKCUMULATIVE)) + abs(self.convertEncoderRaw(RIGHTFRONTCUMULATIVE)) + abs(self.convertEncoderRaw(RIGHTBACKCUMULATIVE))) def turn_angle(self, degrees): desired_inches = self.INCHES_PER_DEGREE * degrees if degrees < 0: while (abs(self.lfencoder.getDistance()) + abs(self.rfencoder.getDistance())) <= desired_inches : self.autonTankDrive(0.4, -0.4) elif degrees > 0: while (abs(self.lfencoder.getDistance()) + abs(self.rfencoder.getDistance())) <= desired_inches : self.autonTankDrive(-0.4, 0.4) # Enable PID Controllers def enablePIDs(self): ''' #No longer required because we swapped from analog encoders to magnetic encoders self.pidLeftFront.enable() self.pidLeftBack.enable() self.pidRightFront.enable() self.pidRightBack.enable() ''' # Disable PID Controllers def disablePIDs(self): ''' #see explaination above self.pidLeftFront.disable() self.pidLeftBack.disable() self.pidRightFront.disable() self.pidRightBack.disable() ''' def getAutonDistance(self): return (self.convertEncoderRaw(abs(self.rfmotor.getPosition()*0.57)) + self.convertEncoderRaw(abs(self.rbmotor.getPosition()*0.57)) + self.convertEncoderRaw(abs(self.lfmotor.getPosition()*0.57)) + self.convertEncoderRaw(abs(self.lbmotor.getPosition()*0.57)))/4 #detirmines how many ticks the encoder has processed def getMotorDistance(self, motor, cumulativeDistance): currentRollovers = 0 #number of times the encoder has gone from 1023 to 0 previousValue = cumulativeDistance #variable for comparison currentValue = motor.getEncPosition() #variable for comparison if(previousValue > currentValue): #checks to see if the encoder reset itself from 1023 to 0 currentRollovers += 1 #notes the rollover return currentValue + (currentRollovers * 1024) #adds current value to the number of rollovers, each rollover == 1024 ticks #converts ticks from getMotorDistance into inches def convertEncoderRaw(self, selectedEncoderValue): return selectedEncoderValue * self.INCHES_PER_REV
class Shooter: left_fly = CANTalon right_fly = CANTalon intake_main = CANTalon intake_mecanum = Talon ball_limit = DigitalInput def __init__(self): self.left_fly = CANTalon(motor_map.left_fly_motor) self.right_fly = CANTalon(motor_map.right_fly_motor) self.intake_main = CANTalon(motor_map.intake_main_motor) self.intake_mecanum = Talon(motor_map.intake_mecanum_motor) self.ball_limit = DigitalInput(sensor_map.ball_limit) self.intake_mecanum.setInverted(True) self.left_fly.reverseOutput(True) self.left_fly.enableBrakeMode(False) self.right_fly.enableBrakeMode(False) self.left_fly.setControlMode(CANTalon.ControlMode.Speed) self.right_fly.setControlMode(CANTalon.ControlMode.Speed) self.left_fly.setPID(sensor_map.shoot_P, sensor_map.shoot_I, sensor_map.shoot_D, sensor_map.shoot_F, sensor_map.shoot_Izone, sensor_map.shoot_RR, sensor_map.shoot_Profile) self.right_fly.setPID(sensor_map.shoot_P, sensor_map.shoot_I, sensor_map.shoot_D, sensor_map.shoot_F, sensor_map.shoot_Izone, sensor_map.shoot_RR, sensor_map.shoot_Profile) self.left_fly.setFeedbackDevice(CANTalon.FeedbackDevice.EncRising) self.right_fly.setFeedbackDevice(CANTalon.FeedbackDevice.EncRising) self.left_fly.configEncoderCodesPerRev(sensor_map.shoot_codes_per_rev) self.right_fly.configEncoderCodesPerRev(sensor_map.shoot_codes_per_rev) def warm_up(self): self.set_rpm(2500) # Warm up flywheels to get ready to shoot def low_goal(self): self.set_rpm(500) def set_rpm(self, rpm_l_set, rpm_r_set=None): if rpm_r_set is None: rpm_r_set = rpm_l_set self.left_fly.set(rpm_l_set) self.right_fly.set(rpm_r_set) def get_rpms(self): return self.left_fly.get(), self.right_fly.get() def set_fly_off(self): self.set_rpm(0) def set_intake(self, power): self.intake_mecanum.set(-power) self.intake_main.set(power) def get_intake(self): return self.intake_main.get() def set_intake_off(self): self.set_intake(0) def get_ball_limit(self): return not bool(self.ball_limit.get()) def execute(self): if int(self.left_fly.getOutputCurrent()) > 20 \ or int(self.left_fly.getOutputCurrent()) > 20: self.set_rpm(0, 0)