001/*----------------------------------------------------------------------------*/ 002/* Copyright (c) FIRST 2008-2012. All Rights Reserved. */ 003/* Open Source Software - may be modified and shared by FRC teams. The code */ 004/* must be accompanied by the FIRST BSD license file in the root directory of */ 005/* the project. */ 006/*----------------------------------------------------------------------------*/ 007package edu.wpi.first.wpilibj; 008 009import edu.wpi.first.wpilibj.communication.FRCNetworkCommunicationsLibrary.tInstances; 010import edu.wpi.first.wpilibj.communication.FRCNetworkCommunicationsLibrary.tResourceType; 011import edu.wpi.first.wpilibj.communication.UsageReporting; 012 013/** 014 * Utility class for handling Robot drive based on a definition of the motor configuration. 015 * The robot drive class handles basic driving for a robot. Currently, 2 and 4 motor tank and 016 * mecanum drive trains are supported. In the future other drive types like swerve might 017 * be implemented. Motor channel numbers are supplied on creation of the class. Those are 018 * used for either the drive function (intended for hand created drive code, such as autonomous) 019 * or with the Tank/Arcade functions intended to be used for Operator Control driving. 020 */ 021public class RobotDrive implements MotorSafety { 022 023 protected MotorSafetyHelper m_safetyHelper; 024 025 /** 026 * The location of a motor on the robot for the purpose of driving 027 */ 028 public static class MotorType { 029 030 /** 031 * The integer value representing this enumeration 032 */ 033 public final int value; 034 static final int kFrontLeft_val = 0; 035 static final int kFrontRight_val = 1; 036 static final int kRearLeft_val = 2; 037 static final int kRearRight_val = 3; 038 /** 039 * motortype: front left 040 */ 041 public static final MotorType kFrontLeft = new MotorType(kFrontLeft_val); 042 /** 043 * motortype: front right 044 */ 045 public static final MotorType kFrontRight = new MotorType(kFrontRight_val); 046 /** 047 * motortype: rear left 048 */ 049 public static final MotorType kRearLeft = new MotorType(kRearLeft_val); 050 /** 051 * motortype: rear right 052 */ 053 public static final MotorType kRearRight = new MotorType(kRearRight_val); 054 055 private MotorType(int value) { 056 this.value = value; 057 } 058 } 059 public static final double kDefaultExpirationTime = 0.1; 060 public static final double kDefaultSensitivity = 0.5; 061 public static final double kDefaultMaxOutput = 1.0; 062 protected static final int kMaxNumberOfMotors = 4; 063 protected final int m_invertedMotors[] = new int[4]; 064 protected double m_sensitivity; 065 protected double m_maxOutput; 066 protected SpeedController m_frontLeftMotor; 067 protected SpeedController m_frontRightMotor; 068 protected SpeedController m_rearLeftMotor; 069 protected SpeedController m_rearRightMotor; 070 protected boolean m_allocatedSpeedControllers; 071 protected byte m_syncGroup = 0; 072 protected static boolean kArcadeRatioCurve_Reported = false; 073 protected static boolean kTank_Reported = false; 074 protected static boolean kArcadeStandard_Reported = false; 075 protected static boolean kMecanumCartesian_Reported = false; 076 protected static boolean kMecanumPolar_Reported = false; 077 078 /** Constructor for RobotDrive with 2 motors specified with channel numbers. 079 * Set up parameters for a two wheel drive system where the 080 * left and right motor pwm channels are specified in the call. 081 * This call assumes Talons for controlling the motors. 082 * @param leftMotorChannel The PWM channel number that drives the left motor. 083 * @param rightMotorChannel The PWM channel number that drives the right motor. 084 */ 085 public RobotDrive(final int leftMotorChannel, final int rightMotorChannel) { 086 m_sensitivity = kDefaultSensitivity; 087 m_maxOutput = kDefaultMaxOutput; 088 m_frontLeftMotor = null; 089 m_rearLeftMotor = new Talon(leftMotorChannel); 090 m_frontRightMotor = null; 091 m_rearRightMotor = new Talon(rightMotorChannel); 092 for (int i = 0; i < kMaxNumberOfMotors; i++) { 093 m_invertedMotors[i] = 1; 094 } 095 m_allocatedSpeedControllers = true; 096 setupMotorSafety(); 097 drive(0, 0); 098 } 099 100 /** 101 * Constructor for RobotDrive with 4 motors specified with channel numbers. 102 * Set up parameters for a four wheel drive system where all four motor 103 * pwm channels are specified in the call. 104 * This call assumes Talons for controlling the motors. 105 * @param frontLeftMotor Front left motor channel number 106 * @param rearLeftMotor Rear Left motor channel number 107 * @param frontRightMotor Front right motor channel number 108 * @param rearRightMotor Rear Right motor channel number 109 */ 110 public RobotDrive(final int frontLeftMotor, final int rearLeftMotor, 111 final int frontRightMotor, final int rearRightMotor) { 112 m_sensitivity = kDefaultSensitivity; 113 m_maxOutput = kDefaultMaxOutput; 114 m_rearLeftMotor = new Talon(rearLeftMotor); 115 m_rearRightMotor = new Talon(rearRightMotor); 116 m_frontLeftMotor = new Talon(frontLeftMotor); 117 m_frontRightMotor = new Talon(frontRightMotor); 118 for (int i = 0; i < kMaxNumberOfMotors; i++) { 119 m_invertedMotors[i] = 1; 120 } 121 m_allocatedSpeedControllers = true; 122 setupMotorSafety(); 123 drive(0, 0); 124 } 125 126 /** 127 * Constructor for RobotDrive with 2 motors specified as SpeedController objects. 128 * The SpeedController version of the constructor enables programs to use the RobotDrive classes with 129 * subclasses of the SpeedController objects, for example, versions with ramping or reshaping of 130 * the curve to suit motor bias or dead-band elimination. 131 * @param leftMotor The left SpeedController object used to drive the robot. 132 * @param rightMotor the right SpeedController object used to drive the robot. 133 */ 134 public RobotDrive(SpeedController leftMotor, SpeedController rightMotor) { 135 if (leftMotor == null || rightMotor == null) { 136 m_rearLeftMotor = m_rearRightMotor = null; 137 throw new NullPointerException("Null motor provided"); 138 } 139 m_frontLeftMotor = null; 140 m_rearLeftMotor = leftMotor; 141 m_frontRightMotor = null; 142 m_rearRightMotor = rightMotor; 143 m_sensitivity = kDefaultSensitivity; 144 m_maxOutput = kDefaultMaxOutput; 145 for (int i = 0; i < kMaxNumberOfMotors; i++) { 146 m_invertedMotors[i] = 1; 147 } 148 m_allocatedSpeedControllers = false; 149 setupMotorSafety(); 150 drive(0, 0); 151 } 152 153 /** 154 * Constructor for RobotDrive with 4 motors specified as SpeedController objects. 155 * Speed controller input version of RobotDrive (see previous comments). 156 * @param rearLeftMotor The back left SpeedController object used to drive the robot. 157 * @param frontLeftMotor The front left SpeedController object used to drive the robot 158 * @param rearRightMotor The back right SpeedController object used to drive the robot. 159 * @param frontRightMotor The front right SpeedController object used to drive the robot. 160 */ 161 public RobotDrive(SpeedController frontLeftMotor, SpeedController rearLeftMotor, 162 SpeedController frontRightMotor, SpeedController rearRightMotor) { 163 if (frontLeftMotor == null || rearLeftMotor == null || frontRightMotor == null || rearRightMotor == null) { 164 m_frontLeftMotor = m_rearLeftMotor = m_frontRightMotor = m_rearRightMotor = null; 165 throw new NullPointerException("Null motor provided"); 166 } 167 m_frontLeftMotor = frontLeftMotor; 168 m_rearLeftMotor = rearLeftMotor; 169 m_frontRightMotor = frontRightMotor; 170 m_rearRightMotor = rearRightMotor; 171 m_sensitivity = kDefaultSensitivity; 172 m_maxOutput = kDefaultMaxOutput; 173 for (int i = 0; i < kMaxNumberOfMotors; i++) { 174 m_invertedMotors[i] = 1; 175 } 176 m_allocatedSpeedControllers = false; 177 setupMotorSafety(); 178 drive(0, 0); 179 } 180 181 /** 182 * Drive the motors at "speed" and "curve". 183 * 184 * The speed and curve are -1.0 to +1.0 values where 0.0 represents stopped and 185 * not turning. The algorithm for adding in the direction attempts to provide a constant 186 * turn radius for differing speeds. 187 * 188 * This function will most likely be used in an autonomous routine. 189 * 190 * @param outputMagnitude The forward component of the output magnitude to send to the motors. 191 * @param curve The rate of turn, constant for different forward speeds. 192 */ 193 public void drive(double outputMagnitude, double curve) { 194 double leftOutput, rightOutput; 195 196 if(!kArcadeRatioCurve_Reported) { 197 UsageReporting.report(tResourceType.kResourceType_RobotDrive, getNumMotors(), tInstances.kRobotDrive_ArcadeRatioCurve); 198 kArcadeRatioCurve_Reported = true; 199 } 200 if (curve < 0) { 201 double value = Math.log(-curve); 202 double ratio = (value - m_sensitivity) / (value + m_sensitivity); 203 if (ratio == 0) { 204 ratio = .0000000001; 205 } 206 leftOutput = outputMagnitude / ratio; 207 rightOutput = outputMagnitude; 208 } else if (curve > 0) { 209 double value = Math.log(curve); 210 double ratio = (value - m_sensitivity) / (value + m_sensitivity); 211 if (ratio == 0) { 212 ratio = .0000000001; 213 } 214 leftOutput = outputMagnitude; 215 rightOutput = outputMagnitude / ratio; 216 } else { 217 leftOutput = outputMagnitude; 218 rightOutput = outputMagnitude; 219 } 220 setLeftRightMotorOutputs(leftOutput, rightOutput); 221 } 222 223 /** 224 * Provide tank steering using the stored robot configuration. 225 * drive the robot using two joystick inputs. The Y-axis will be selected from 226 * each Joystick object. 227 * @param leftStick The joystick to control the left side of the robot. 228 * @param rightStick The joystick to control the right side of the robot. 229 */ 230 public void tankDrive(GenericHID leftStick, GenericHID rightStick) { 231 if (leftStick == null || rightStick == null) { 232 throw new NullPointerException("Null HID provided"); 233 } 234 tankDrive(leftStick.getY(), rightStick.getY(), true); 235 } 236 237 /** 238 * Provide tank steering using the stored robot configuration. 239 * drive the robot using two joystick inputs. The Y-axis will be selected from 240 * each Joystick object. 241 * @param leftStick The joystick to control the left side of the robot. 242 * @param rightStick The joystick to control the right side of the robot. 243 * @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds 244 */ 245 public void tankDrive(GenericHID leftStick, GenericHID rightStick, boolean squaredInputs) { 246 if (leftStick == null || rightStick == null) { 247 throw new NullPointerException("Null HID provided"); 248 } 249 tankDrive(leftStick.getY(), rightStick.getY(), squaredInputs); 250 } 251 252 /** 253 * Provide tank steering using the stored robot configuration. 254 * This function lets you pick the axis to be used on each Joystick object for the left 255 * and right sides of the robot. 256 * @param leftStick The Joystick object to use for the left side of the robot. 257 * @param leftAxis The axis to select on the left side Joystick object. 258 * @param rightStick The Joystick object to use for the right side of the robot. 259 * @param rightAxis The axis to select on the right side Joystick object. 260 */ 261 public void tankDrive(GenericHID leftStick, final int leftAxis, 262 GenericHID rightStick, final int rightAxis) { 263 if (leftStick == null || rightStick == null) { 264 throw new NullPointerException("Null HID provided"); 265 } 266 tankDrive(leftStick.getRawAxis(leftAxis), rightStick.getRawAxis(rightAxis), true); 267 } 268 269 /** 270 * Provide tank steering using the stored robot configuration. 271 * This function lets you pick the axis to be used on each Joystick object for the left 272 * and right sides of the robot. 273 * @param leftStick The Joystick object to use for the left side of the robot. 274 * @param leftAxis The axis to select on the left side Joystick object. 275 * @param rightStick The Joystick object to use for the right side of the robot. 276 * @param rightAxis The axis to select on the right side Joystick object. 277 * @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds 278 */ 279 public void tankDrive(GenericHID leftStick, final int leftAxis, 280 GenericHID rightStick, final int rightAxis, boolean squaredInputs) { 281 if (leftStick == null || rightStick == null) { 282 throw new NullPointerException("Null HID provided"); 283 } 284 tankDrive(leftStick.getRawAxis(leftAxis), rightStick.getRawAxis(rightAxis), squaredInputs); 285 } 286 287 /** 288 * Provide tank steering using the stored robot configuration. 289 * This function lets you directly provide joystick values from any source. 290 * @param leftValue The value of the left stick. 291 * @param rightValue The value of the right stick. 292 * @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds 293 */ 294 public void tankDrive(double leftValue, double rightValue, boolean squaredInputs) { 295 296 if(!kTank_Reported) { 297 UsageReporting.report(tResourceType.kResourceType_RobotDrive, getNumMotors(), tInstances.kRobotDrive_Tank); 298 kTank_Reported = true; 299 } 300 301 // square the inputs (while preserving the sign) to increase fine control while permitting full power 302 leftValue = limit(leftValue); 303 rightValue = limit(rightValue); 304 if(squaredInputs) { 305 if (leftValue >= 0.0) { 306 leftValue = (leftValue * leftValue); 307 } else { 308 leftValue = -(leftValue * leftValue); 309 } 310 if (rightValue >= 0.0) { 311 rightValue = (rightValue * rightValue); 312 } else { 313 rightValue = -(rightValue * rightValue); 314 } 315 } 316 setLeftRightMotorOutputs(leftValue, rightValue); 317 } 318 319 /** 320 * Provide tank steering using the stored robot configuration. 321 * This function lets you directly provide joystick values from any source. 322 * @param leftValue The value of the left stick. 323 * @param rightValue The value of the right stick. 324 */ 325 public void tankDrive(double leftValue, double rightValue) { 326 tankDrive(leftValue, rightValue, true); 327 } 328 329 /** 330 * Arcade drive implements single stick driving. 331 * Given a single Joystick, the class assumes the Y axis for the move value and the X axis 332 * for the rotate value. 333 * (Should add more information here regarding the way that arcade drive works.) 334 * @param stick The joystick to use for Arcade single-stick driving. The Y-axis will be selected 335 * for forwards/backwards and the X-axis will be selected for rotation rate. 336 * @param squaredInputs If true, the sensitivity will be decreased for small values 337 */ 338 public void arcadeDrive(GenericHID stick, boolean squaredInputs) { 339 // simply call the full-featured arcadeDrive with the appropriate values 340 arcadeDrive(stick.getY(), stick.getX(), squaredInputs); 341 } 342 343 /** 344 * Arcade drive implements single stick driving. 345 * Given a single Joystick, the class assumes the Y axis for the move value and the X axis 346 * for the rotate value. 347 * (Should add more information here regarding the way that arcade drive works.) 348 * @param stick The joystick to use for Arcade single-stick driving. The Y-axis will be selected 349 * for forwards/backwards and the X-axis will be selected for rotation rate. 350 */ 351 public void arcadeDrive(GenericHID stick) { 352 this.arcadeDrive(stick, true); 353 } 354 355 /** 356 * Arcade drive implements single stick driving. 357 * Given two joystick instances and two axis, compute the values to send to either two 358 * or four motors. 359 * @param moveStick The Joystick object that represents the forward/backward direction 360 * @param moveAxis The axis on the moveStick object to use for forwards/backwards (typically Y_AXIS) 361 * @param rotateStick The Joystick object that represents the rotation value 362 * @param rotateAxis The axis on the rotation object to use for the rotate right/left (typically X_AXIS) 363 * @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds 364 */ 365 public void arcadeDrive(GenericHID moveStick, final int moveAxis, 366 GenericHID rotateStick, final int rotateAxis, 367 boolean squaredInputs) { 368 double moveValue = moveStick.getRawAxis(moveAxis); 369 double rotateValue = rotateStick.getRawAxis(rotateAxis); 370 371 arcadeDrive(moveValue, rotateValue, squaredInputs); 372 } 373 374 /** 375 * Arcade drive implements single stick driving. 376 * Given two joystick instances and two axis, compute the values to send to either two 377 * or four motors. 378 * @param moveStick The Joystick object that represents the forward/backward direction 379 * @param moveAxis The axis on the moveStick object to use for forwards/backwards (typically Y_AXIS) 380 * @param rotateStick The Joystick object that represents the rotation value 381 * @param rotateAxis The axis on the rotation object to use for the rotate right/left (typically X_AXIS) 382 */ 383 public void arcadeDrive(GenericHID moveStick, final int moveAxis, 384 GenericHID rotateStick, final int rotateAxis) { 385 this.arcadeDrive(moveStick, moveAxis, rotateStick, rotateAxis, true); 386 } 387 388 /** 389 * Arcade drive implements single stick driving. 390 * This function lets you directly provide joystick values from any source. 391 * @param moveValue The value to use for forwards/backwards 392 * @param rotateValue The value to use for the rotate right/left 393 * @param squaredInputs If set, decreases the sensitivity at low speeds 394 */ 395 public void arcadeDrive(double moveValue, double rotateValue, boolean squaredInputs) { 396 // local variables to hold the computed PWM values for the motors 397 if(!kArcadeStandard_Reported) { 398 UsageReporting.report(tResourceType.kResourceType_RobotDrive, getNumMotors(), tInstances.kRobotDrive_ArcadeStandard); 399 kArcadeStandard_Reported = true; 400 } 401 402 double leftMotorSpeed; 403 double rightMotorSpeed; 404 405 moveValue = limit(moveValue); 406 rotateValue = limit(rotateValue); 407 408 if (squaredInputs) { 409 // square the inputs (while preserving the sign) to increase fine control while permitting full power 410 if (moveValue >= 0.0) { 411 moveValue = (moveValue * moveValue); 412 } else { 413 moveValue = -(moveValue * moveValue); 414 } 415 if (rotateValue >= 0.0) { 416 rotateValue = (rotateValue * rotateValue); 417 } else { 418 rotateValue = -(rotateValue * rotateValue); 419 } 420 } 421 422 if (moveValue > 0.0) { 423 if (rotateValue > 0.0) { 424 leftMotorSpeed = moveValue - rotateValue; 425 rightMotorSpeed = Math.max(moveValue, rotateValue); 426 } else { 427 leftMotorSpeed = Math.max(moveValue, -rotateValue); 428 rightMotorSpeed = moveValue + rotateValue; 429 } 430 } else { 431 if (rotateValue > 0.0) { 432 leftMotorSpeed = -Math.max(-moveValue, rotateValue); 433 rightMotorSpeed = moveValue + rotateValue; 434 } else { 435 leftMotorSpeed = moveValue - rotateValue; 436 rightMotorSpeed = -Math.max(-moveValue, -rotateValue); 437 } 438 } 439 440 setLeftRightMotorOutputs(leftMotorSpeed, rightMotorSpeed); 441 } 442 443 /** 444 * Arcade drive implements single stick driving. 445 * This function lets you directly provide joystick values from any source. 446 * @param moveValue The value to use for fowards/backwards 447 * @param rotateValue The value to use for the rotate right/left 448 */ 449 public void arcadeDrive(double moveValue, double rotateValue) { 450 this.arcadeDrive(moveValue, rotateValue, true); 451 } 452 453 /** 454 * Drive method for Mecanum wheeled robots. 455 * 456 * A method for driving with Mecanum wheeled robots. There are 4 wheels 457 * on the robot, arranged so that the front and back wheels are toed in 45 degrees. 458 * When looking at the wheels from the top, the roller axles should form an X across the robot. 459 * 460 * This is designed to be directly driven by joystick axes. 461 * 462 * @param x The speed that the robot should drive in the X direction. [-1.0..1.0] 463 * @param y The speed that the robot should drive in the Y direction. 464 * This input is inverted to match the forward == -1.0 that joysticks produce. [-1.0..1.0] 465 * @param rotation The rate of rotation for the robot that is completely independent of 466 * the translation. [-1.0..1.0] 467 * @param gyroAngle The current angle reading from the gyro. Use this to implement field-oriented controls. 468 */ 469 public void mecanumDrive_Cartesian(double x, double y, double rotation, double gyroAngle) { 470 if(!kMecanumCartesian_Reported) { 471 UsageReporting.report(tResourceType.kResourceType_RobotDrive, getNumMotors(), tInstances.kRobotDrive_MecanumCartesian); 472 kMecanumCartesian_Reported = true; 473 } 474 double xIn = x; 475 double yIn = y; 476 // Negate y for the joystick. 477 yIn = -yIn; 478 // Compenstate for gyro angle. 479 double rotated[] = rotateVector(xIn, yIn, gyroAngle); 480 xIn = rotated[0]; 481 yIn = rotated[1]; 482 483 double wheelSpeeds[] = new double[kMaxNumberOfMotors]; 484 wheelSpeeds[MotorType.kFrontLeft_val] = xIn + yIn + rotation; 485 wheelSpeeds[MotorType.kFrontRight_val] = -xIn + yIn - rotation; 486 wheelSpeeds[MotorType.kRearLeft_val] = -xIn + yIn + rotation; 487 wheelSpeeds[MotorType.kRearRight_val] = xIn + yIn - rotation; 488 489 normalize(wheelSpeeds); 490 m_frontLeftMotor.set(wheelSpeeds[MotorType.kFrontLeft_val] * m_invertedMotors[MotorType.kFrontLeft_val] * m_maxOutput, m_syncGroup); 491 m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight_val] * m_invertedMotors[MotorType.kFrontRight_val] * m_maxOutput, m_syncGroup); 492 m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft_val] * m_invertedMotors[MotorType.kRearLeft_val] * m_maxOutput, m_syncGroup); 493 m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight_val] * m_invertedMotors[MotorType.kRearRight_val] * m_maxOutput, m_syncGroup); 494 495 if (m_syncGroup != 0) { 496 CANJaguar.updateSyncGroup(m_syncGroup); 497 } 498 499 if (m_safetyHelper != null) m_safetyHelper.feed(); 500 } 501 502 /** 503 * Drive method for Mecanum wheeled robots. 504 * 505 * A method for driving with Mecanum wheeled robots. There are 4 wheels 506 * on the robot, arranged so that the front and back wheels are toed in 45 degrees. 507 * When looking at the wheels from the top, the roller axles should form an X across the robot. 508 * 509 * @param magnitude The speed that the robot should drive in a given direction. 510 * @param direction The direction the robot should drive in degrees. The direction and maginitute are 511 * independent of the rotation rate. 512 * @param rotation The rate of rotation for the robot that is completely independent of 513 * the magnitute or direction. [-1.0..1.0] 514 */ 515 public void mecanumDrive_Polar(double magnitude, double direction, double rotation) { 516 if(!kMecanumPolar_Reported) { 517 UsageReporting.report(tResourceType.kResourceType_RobotDrive, getNumMotors(), tInstances.kRobotDrive_MecanumPolar); 518 kMecanumPolar_Reported = true; 519 } 520 double frontLeftSpeed, rearLeftSpeed, frontRightSpeed, rearRightSpeed; 521 // Normalized for full power along the Cartesian axes. 522 magnitude = limit(magnitude) * Math.sqrt(2.0); 523 // The rollers are at 45 degree angles. 524 double dirInRad = (direction + 45.0) * 3.14159 / 180.0; 525 double cosD = Math.cos(dirInRad); 526 double sinD = Math.sin(dirInRad); 527 528 double wheelSpeeds[] = new double[kMaxNumberOfMotors]; 529 wheelSpeeds[MotorType.kFrontLeft_val] = (sinD * magnitude + rotation); 530 wheelSpeeds[MotorType.kFrontRight_val] = (cosD * magnitude - rotation); 531 wheelSpeeds[MotorType.kRearLeft_val] = (cosD * magnitude + rotation); 532 wheelSpeeds[MotorType.kRearRight_val] = (sinD * magnitude - rotation); 533 534 normalize(wheelSpeeds); 535 536 m_frontLeftMotor.set(wheelSpeeds[MotorType.kFrontLeft_val] * m_invertedMotors[MotorType.kFrontLeft_val] * m_maxOutput, m_syncGroup); 537 m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight_val] * m_invertedMotors[MotorType.kFrontRight_val] * m_maxOutput, m_syncGroup); 538 m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft_val] * m_invertedMotors[MotorType.kRearLeft_val] * m_maxOutput, m_syncGroup); 539 m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight_val] * m_invertedMotors[MotorType.kRearRight_val] * m_maxOutput, m_syncGroup); 540 541 if (this.m_syncGroup != 0) { 542 CANJaguar.updateSyncGroup(m_syncGroup); 543 } 544 545 if (m_safetyHelper != null) m_safetyHelper.feed(); 546 } 547 548 /** 549 * Holonomic Drive method for Mecanum wheeled robots. 550 * 551 * This is an alias to mecanumDrive_Polar() for backward compatability 552 * 553 * @param magnitude The speed that the robot should drive in a given direction. [-1.0..1.0] 554 * @param direction The direction the robot should drive. The direction and maginitute are 555 * independent of the rotation rate. 556 * @param rotation The rate of rotation for the robot that is completely independent of 557 * the magnitute or direction. [-1.0..1.0] 558 */ 559 void holonomicDrive(float magnitude, float direction, float rotation) { 560 mecanumDrive_Polar(magnitude, direction, rotation); 561 } 562 563 /** Set the speed of the right and left motors. 564 * This is used once an appropriate drive setup function is called such as 565 * twoWheelDrive(). The motors are set to "leftSpeed" and "rightSpeed" 566 * and includes flipping the direction of one side for opposing motors. 567 * @param leftOutput The speed to send to the left side of the robot. 568 * @param rightOutput The speed to send to the right side of the robot. 569 */ 570 public void setLeftRightMotorOutputs(double leftOutput, double rightOutput) { 571 if (m_rearLeftMotor == null || m_rearRightMotor == null) { 572 throw new NullPointerException("Null motor provided"); 573 } 574 575 if (m_frontLeftMotor != null) { 576 m_frontLeftMotor.set(limit(leftOutput) * m_invertedMotors[MotorType.kFrontLeft_val] * m_maxOutput, m_syncGroup); 577 } 578 m_rearLeftMotor.set(limit(leftOutput) * m_invertedMotors[MotorType.kRearLeft_val] * m_maxOutput, m_syncGroup); 579 580 if (m_frontRightMotor != null) { 581 m_frontRightMotor.set(-limit(rightOutput) * m_invertedMotors[MotorType.kFrontRight_val] * m_maxOutput, m_syncGroup); 582 } 583 m_rearRightMotor.set(-limit(rightOutput) * m_invertedMotors[MotorType.kRearRight_val] * m_maxOutput, m_syncGroup); 584 585 if (this.m_syncGroup != 0) { 586 CANJaguar.updateSyncGroup(m_syncGroup); 587 } 588 589 if (m_safetyHelper != null) m_safetyHelper.feed(); 590 } 591 592 /** 593 * Limit motor values to the -1.0 to +1.0 range. 594 */ 595 protected static double limit(double num) { 596 if (num > 1.0) { 597 return 1.0; 598 } 599 if (num < -1.0) { 600 return -1.0; 601 } 602 return num; 603 } 604 605 /** 606 * Normalize all wheel speeds if the magnitude of any wheel is greater than 1.0. 607 */ 608 protected static void normalize(double wheelSpeeds[]) { 609 double maxMagnitude = Math.abs(wheelSpeeds[0]); 610 int i; 611 for (i=1; i<kMaxNumberOfMotors; i++) { 612 double temp = Math.abs(wheelSpeeds[i]); 613 if (maxMagnitude < temp) maxMagnitude = temp; 614 } 615 if (maxMagnitude > 1.0) { 616 for (i=0; i<kMaxNumberOfMotors; i++) { 617 wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude; 618 } 619 } 620 } 621 622 /** 623 * Rotate a vector in Cartesian space. 624 */ 625 protected static double[] rotateVector(double x, double y, double angle) { 626 double cosA = Math.cos(angle * (3.14159 / 180.0)); 627 double sinA = Math.sin(angle * (3.14159 / 180.0)); 628 double out[] = new double[2]; 629 out[0] = x * cosA - y * sinA; 630 out[1] = x * sinA + y * cosA; 631 return out; 632 } 633 634 /** 635 * Invert a motor direction. 636 * This is used when a motor should run in the opposite direction as the drive 637 * code would normally run it. Motors that are direct drive would be inverted, the 638 * drive code assumes that the motors are geared with one reversal. 639 * @param motor The motor index to invert. 640 * @param isInverted True if the motor should be inverted when operated. 641 */ 642 public void setInvertedMotor(MotorType motor, boolean isInverted) { 643 m_invertedMotors[motor.value] = isInverted ? -1 : 1; 644 } 645 646 /** 647 * Set the turning sensitivity. 648 * 649 * This only impacts the drive() entry-point. 650 * @param sensitivity Effectively sets the turning sensitivity (or turn radius for a given value) 651 */ 652 public void setSensitivity(double sensitivity) { 653 m_sensitivity = sensitivity; 654 } 655 656 /** 657 * Configure the scaling factor for using RobotDrive with motor controllers in a mode other than PercentVbus. 658 * @param maxOutput Multiplied with the output percentage computed by the drive functions. 659 */ 660 public void setMaxOutput(double maxOutput) { 661 m_maxOutput = maxOutput; 662 } 663 664 /** 665 * Set the number of the sync group for the motor controllers. If the motor controllers are {@link CANJaguar}s, 666 * then they will all be added to this sync group, causing them to update their values at the same time. 667 * 668 * @param syncGroup the update group to add the motor controllers to 669 */ 670 public void setCANJaguarSyncGroup(byte syncGroup) { 671 m_syncGroup = syncGroup; 672 } 673 674 /** 675 * Free the speed controllers if they were allocated locally 676 */ 677 public void free() { 678 if (m_allocatedSpeedControllers) { 679 if (m_frontLeftMotor != null) { 680 ((PWM) m_frontLeftMotor).free(); 681 } 682 if (m_frontRightMotor != null) { 683 ((PWM) m_frontRightMotor).free(); 684 } 685 if (m_rearLeftMotor != null) { 686 ((PWM) m_rearLeftMotor).free(); 687 } 688 if (m_rearRightMotor != null) { 689 ((PWM) m_rearRightMotor).free(); 690 } 691 } 692 } 693 694 public void setExpiration(double timeout) { 695 m_safetyHelper.setExpiration(timeout); 696 } 697 698 public double getExpiration() { 699 return m_safetyHelper.getExpiration(); 700 } 701 702 public boolean isAlive() { 703 return m_safetyHelper.isAlive(); 704 } 705 706 public boolean isSafetyEnabled() { 707 return m_safetyHelper.isSafetyEnabled(); 708 } 709 710 public void setSafetyEnabled(boolean enabled) { 711 m_safetyHelper.setSafetyEnabled(enabled); 712 } 713 714 public String getDescription() { 715 return "Robot Drive"; 716 } 717 718 public void stopMotor() { 719 if (m_frontLeftMotor != null) { 720 m_frontLeftMotor.set(0.0); 721 } 722 if (m_frontRightMotor != null) { 723 m_frontRightMotor.set(0.0); 724 } 725 if (m_rearLeftMotor != null) { 726 m_rearLeftMotor.set(0.0); 727 } 728 if (m_rearRightMotor != null) { 729 m_rearRightMotor.set(0.0); 730 } 731 if (m_safetyHelper != null) m_safetyHelper.feed(); 732 } 733 734 private void setupMotorSafety() { 735 m_safetyHelper = new MotorSafetyHelper(this); 736 m_safetyHelper.setExpiration(kDefaultExpirationTime); 737 m_safetyHelper.setSafetyEnabled(true); 738 } 739 740 protected int getNumMotors() { 741 int motors = 0; 742 if (m_frontLeftMotor != null) motors++; 743 if (m_frontRightMotor != null) motors++; 744 if (m_rearLeftMotor != null) motors++; 745 if (m_rearRightMotor != null) motors++; 746 return motors; 747 } 748}