class Drive(Subsystem): def __init__(self): super().__init__('Drive') # These can be used individually, but you should use RobotDrive to controll these self.LB = Motor(ids.LB_motor) self.LF = Motor(ids.LF_motor) self.RB = Motor(ids.RB_motor) self.RF = Motor(ids.RF_motor) self.drive_train = RobotDrive(self.LF, self.LB, self.RF, self.RB) self.drive_train.setInvertedMotor(0, True) self.drive_train.setInvertedMotor(1, True) self.drive_train.setExpiration(0.1) ## Uncomment to fail #import time #time.sleep(5) # doesn't work bc HAL Resource error #self.pcm0 = Solenoid(ids.PCM_pin0) #self.pcm1 = Solenoid(ids.PCM_pin1) def tank(self, L_speed, R_speed): self.drive_train.tankDrive(L_speed, R_speed, squaredInputs=False) def mecanum_cartesian(self, X_speed, Y_speed, R_speed=0, gyroHeading=0): self.drive_train.mecanumDrive_Cartesian(X_speed, Y_speed, R_speed, gyroHeading) def mecanum_polar(self, _speed, _direction, R_speed=0): self.drive_train.mecanumDrive_Polar(_speed, _direction, R_speed)
def __init__(self): super().__init__('Drive') # These can be used individually, but you should use RobotDrive to controll these self.LB = Motor(ids.LB_motor) self.LF = Motor(ids.LF_motor) self.RB = Motor(ids.RB_motor) self.RF = Motor(ids.RF_motor) self.drive_train = RobotDrive(self.LF, self.LB, self.RF, self.RB) self.drive_train.setInvertedMotor(0, True) self.drive_train.setInvertedMotor(1, True) self.drive_train.setExpiration(0.1)
def mecanumDrive_Cartesian(self, x, y, rotation, gyroAngle): """Drive method for Mecanum wheeled robots. A method for driving with Mecanum wheeled robots. There are 4 wheels on the robot, arranged so that the front and back wheels are toed in 45 degrees. When looking at the wheels from the top, the roller axles should form an X across the robot. This is designed to be directly driven by joystick axes. :param x: The speed that the robot should drive in the X direction. [-1.0..1.0] :param y: The speed that the robot should drive in the Y direction. This input is inverted to match the forward == -1.0 that joysticks produce. [-1.0..1.0] :param rotation: The rate of rotation for the robot that is completely independent of the translation. [-1.0..1.0] :param gyroAngle: The current angle reading from the gyro. Use this to implement field-oriented controls. """ if not wpilib.RobotDrive.kMecanumCartesian_Reported: hal.HALReport(hal.HALUsageReporting.kResourceType_RobotDrive, self.getNumMotors(), hal.HALUsageReporting.kRobotDrive_MecanumCartesian) RobotDrive.kMecanumCartesian_Reported = True xIn = x yIn = y # Negate y for the joystick. yIn = -yIn # Compenstate for gyro angle. xIn, yIn = RobotDrive.rotateVector(xIn, yIn, gyroAngle) wheelSpeeds = [0]*self.kMaxNumberOfMotors wheelSpeeds[self.MotorType.kFrontLeft] = xIn + yIn + rotation wheelSpeeds[self.MotorType.kFrontRight] = -xIn + yIn - rotation wheelSpeeds[self.MotorType.kRearLeft] = -xIn + yIn + ( rotation * self.weight_multiplier ) wheelSpeeds[self.MotorType.kRearRight] = xIn + yIn - ( rotation * self.weight_multiplier ) RobotDrive.normalize(wheelSpeeds) self.frontLeftMotor.set(wheelSpeeds[self.MotorType.kFrontLeft] * self.invertedMotors[self.MotorType.kFrontLeft] * self.maxOutput, self.syncGroup) self.frontRightMotor.set(wheelSpeeds[self.MotorType.kFrontRight] * self.invertedMotors[self.MotorType.kFrontRight] * self.maxOutput, self.syncGroup) self.rearLeftMotor.set(wheelSpeeds[self.MotorType.kRearLeft] * self.invertedMotors[self.MotorType.kRearLeft] * self.maxOutput, self.syncGroup) self.rearRightMotor.set(wheelSpeeds[self.MotorType.kRearRight] * self.invertedMotors[self.MotorType.kRearRight] * self.maxOutput, self.syncGroup) if self.syncGroup != 0: wpilib.CANJaguar.updateSyncGroup(self.syncGroup) self.feed()
def __init__(self): super().__init__() """ Motor objects init Reason for recall is because MagicRobot is looking for the CANTalon Object instance before init """ self.left_motor_one = CANTalon(motor_map.drive_base_left_one_motor) self.left_motor_two = CANTalon(motor_map.drive_base_left_two_motor) self.right_motor_one = CANTalon(motor_map.drive_base_right_one_motor) self.right_motor_two = CANTalon(motor_map.drive_base_right_two_motor) self.left_encoder = Encoder(sensor_map.left_drive_encoder_one, sensor_map.left_drive_encoder_two, False, Encoder.EncodingType.k4X) self.right_encoder = Encoder(sensor_map.right_drive_encoder_one, sensor_map.right_drive_encoder_two, False, Encoder.EncodingType.k4X) self.navx = AHRS(SPI.Port.kMXP) self.left_motor_one.enableBrakeMode(True) self.left_motor_two.enableBrakeMode(True) self.right_motor_one.enableBrakeMode(True) self.right_motor_two.enableBrakeMode(True) self.base = RobotDrive(self.left_motor_one, self.left_motor_two, self.right_motor_one, self.right_motor_two) self.dpp = sensor_map.wheel_diameter * math.pi / 360 self.left_encoder.setDistancePerPulse(self.dpp) self.right_encoder.setDistancePerPulse(self.dpp) self.left_encoder.setSamplesToAverage(sensor_map.samples_average) self.right_encoder.setSamplesToAverage(sensor_map.samples_average) self.left_encoder.setMinRate(sensor_map.min_enc_rate) self.right_encoder.setMinRate(sensor_map.min_enc_rate) self.auto_pid_out = AutoPIDOut() self.pid_d_controller = PIDController(sensor_map.drive_P, sensor_map.drive_I, sensor_map.drive_D, sensor_map.drive_F, self.navx, self.auto_pid_out, 0.005) self.type_flag = ("DRIVE", "TURN") self.current_flag = self.type_flag[0] self.auto_pid_out.drive_base = self self.auto_pid_out.current_flag = self.current_flag
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")
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 DriveBase: left_motor_one = CANTalon left_motor_two = CANTalon right_motor_one = CANTalon right_motor_two = CANTalon left_encoder = Encoder right_encoder = Encoder navx = AHRS def __init__(self): super().__init__() """ Motor objects init Reason for recall is because MagicRobot is looking for the CANTalon Object instance before init """ self.left_motor_one = CANTalon(motor_map.drive_base_left_one_motor) self.left_motor_two = CANTalon(motor_map.drive_base_left_two_motor) self.right_motor_one = CANTalon(motor_map.drive_base_right_one_motor) self.right_motor_two = CANTalon(motor_map.drive_base_right_two_motor) self.left_encoder = Encoder(sensor_map.left_drive_encoder_one, sensor_map.left_drive_encoder_two, False, Encoder.EncodingType.k4X) self.right_encoder = Encoder(sensor_map.right_drive_encoder_one, sensor_map.right_drive_encoder_two, False, Encoder.EncodingType.k4X) self.navx = AHRS(SPI.Port.kMXP) self.left_motor_one.enableBrakeMode(True) self.left_motor_two.enableBrakeMode(True) self.right_motor_one.enableBrakeMode(True) self.right_motor_two.enableBrakeMode(True) self.base = RobotDrive(self.left_motor_one, self.left_motor_two, self.right_motor_one, self.right_motor_two) self.dpp = sensor_map.wheel_diameter * math.pi / 360 self.left_encoder.setDistancePerPulse(self.dpp) self.right_encoder.setDistancePerPulse(self.dpp) self.left_encoder.setSamplesToAverage(sensor_map.samples_average) self.right_encoder.setSamplesToAverage(sensor_map.samples_average) self.left_encoder.setMinRate(sensor_map.min_enc_rate) self.right_encoder.setMinRate(sensor_map.min_enc_rate) self.auto_pid_out = AutoPIDOut() self.pid_d_controller = PIDController(sensor_map.drive_P, sensor_map.drive_I, sensor_map.drive_D, sensor_map.drive_F, self.navx, self.auto_pid_out, 0.005) self.type_flag = ("DRIVE", "TURN") self.current_flag = self.type_flag[0] self.auto_pid_out.drive_base = self self.auto_pid_out.current_flag = self.current_flag def drive(self, left_power, right_power): self.base.tankDrive(left_power, right_power) def execute(self): if int(self.base.getNumMotors()) < 3: self.base.drive(0, 0) def get_drive_distance(self): return -float(self.left_encoder.getDistance()), float(self.right_encoder.getDistance()) def rest_base(self): self.left_encoder.reset() self.right_encoder.reset() def pid_drive(self, speed, distance, to_angle=None): self.navx.isCalibrating() self.pid_d_controller.reset() self.pid_d_controller.setPID(sensor_map.drive_P, sensor_map.drive_I, sensor_map.drive_D, sensor_map.drive_F) self.pid_d_controller.setOutputRange(speed - distance, speed + distance) if to_angle is None: set_angle = self.navx.getYaw() else: set_angle = to_angle self.pid_d_controller.setSetpoint(float(set_angle)) self.drive(speed + 0.03, speed) self.pid_d_controller.enable() self.current_flag = self.type_flag[0] self.auto_pid_out.current_flag = self.current_flag def pid_turn(self, angle): self.pid_d_controller.reset() self.pid_d_controller.setPID(sensor_map.turn_P, sensor_map.turn_I, sensor_map.turn_D, sensor_map.turn_F) self.pid_d_controller.setOutputRange(sensor_map.output_range_min, sensor_map.output_range_max) self.pid_d_controller.setSetpoint(float(angle)) self.pid_d_controller.enable() self.current_flag = self.type_flag[1] self.auto_pid_out.current_flag = self.current_flag def stop_pid(self): self.pid_d_controller.disable() self.pid_d_controller.reset()