ftclib/drive.h
author TrcTeam1
Tue Mar 18 23:46:38 2014 -0700 (2014-03-18)
changeset 135 6c58e0fa8374
parent 104 ac4bace3c3fe
permissions -rw-r--r--
Merged
     1 #if 0
     2 /// Copyright (c) Titan Robotics Club. All rights reserved.
     3 ///
     4 /// <module name="drive.h" />
     5 ///
     6 /// <summary>
     7 ///     This module contains the library functions for the drive subsystem.
     8 /// </summary>
     9 ///
    10 /// <remarks>
    11 ///     Environment: RobotC for Lego Mindstorms NXT.
    12 /// </remarks>
    13 #endif
    14 
    15 #ifndef _DRIVE_H
    16 #define _DRIVE_H
    17 
    18 #pragma systemFile
    19 
    20 #ifdef MOD_ID
    21     #undef MOD_ID
    22 #endif
    23 #define MOD_ID                  MOD_DRIVE
    24 
    25 //
    26 // Constants.
    27 //
    28 #define DRIVEF_STATE_MASK       0x00ff
    29 #define DRIVEF_ON               0x0001
    30 #define DRIVEF_STALLED          0x0002
    31 #define DRIVEF_STALL_PROTECT_ON 0x0100
    32 #define DRIVEF_FOUR_MOTORS      0x0200
    33 #define DRIVEF_SWERVE           0x0400
    34 
    35 #define DRIVE_MIN_STALL_POWER   20
    36 #define DRIVE_STALL_TIME        2000    //2 seconds
    37 
    38 #define MAX_NUM_MOTORS          4
    39 #define IDX_FRONT_LEFT          0
    40 #define IDX_FRONT_RIGHT         1
    41 #define IDX_REAR_LEFT           2
    42 #define IDX_REAR_RIGHT          3
    43 
    44 #define CRABDIR_NONE            0
    45 #define CRABDIR_LEFT            1
    46 #define CRABDIR_RIGHT           2
    47 
    48 //
    49 // Macros.
    50 //
    51 
    52 /**
    53  *  This macro checks if the drive subsystem is stalled.
    54  *
    55  *  @param d Points to the DRIVE structure.
    56  *
    57  *  @return Returns true if detected stall condition.
    58  */
    59 #define DriveIsStalled(d)       ((d).flags & DRIVEF_STALLED)
    60 
    61 /**
    62  *  This macro checks if the drive subsystem is using swerve drive.
    63  *
    64  *  @param d Points to the DRIVE structure.
    65  *
    66  *  @return Returns true if it is swerve drive.
    67  */
    68 #define DriveIsSwerve(d)        ((d).flags & DRIVEF_SWERVE)
    69 
    70 /**
    71  *  This macro checks if the drive subsystem is 4-motor drive.
    72  *
    73  *  @param d Points to the DRIVE structure.
    74  *
    75  *  @return Returns true if it is 4-motor drive.
    76  */
    77 #define DriveHas4Motors(d)      ((d).flags & DRIVEF_FOUR_MOTORS)
    78 
    79 /**
    80  *  This macro returns the robot's X position using the motor encoders.
    81  *
    82  *  @param d Points to the DRIVE structure.
    83  *
    84  *  @return Returns the X position in encoder clicks.
    85  */
    86 #define DriveGetXPos(d)         ((d).xPos)
    87 
    88 /**
    89  *  This macro returns the robot's Y position using the motor encoders.
    90  *
    91  *  @param d Points to the DRIVE structure.
    92  *
    93  *  @return Returns the Y position in encoder clicks.
    94  */
    95 #define DriveGetYPos(d)         ((d).yPos)
    96 
    97 /**
    98  *  This macro returns the robot's Rotation position using the motor encoders.
    99  *
   100  *  @param d Points to the DRIVE structure.
   101  *
   102  *  @return Returns the Rotation position in encoder clicks.
   103  */
   104 #define DriveGetRotPos(d)       ((d).rotPos)
   105 
   106 //
   107 // Type definitions.
   108 //
   109 typedef struct
   110 {
   111     int             flags;
   112     tMotor          motorIDs[MAX_NUM_MOTORS];
   113     int             motorPowers[MAX_NUM_MOTORS];
   114     long            motorEncoders[MAX_NUM_MOTORS];
   115     float           motorSpeeds[MAX_NUM_MOTORS];
   116     unsigned long   prevTime;
   117     unsigned long   stallTimer;
   118     float           xPos;
   119     float           yPos;
   120     float           rotPos;
   121 #ifdef _SERVO_H
   122     SERVO          *servos[MAX_NUM_MOTORS];
   123     float           lowAngleLimit;
   124     float           highAngleLimit;
   125 #endif
   126 } DRIVE;
   127 
   128 /**
   129  *  This function resets the drive system.
   130  *
   131  *  @param drive Points to the DRIVE structure to be reset.
   132  */
   133 void
   134 DriveReset(
   135     DRIVE &drive
   136     )
   137 {
   138     TFuncName("DriveReset");
   139     TLevel(API);
   140     TEnter();
   141 
   142     int numMotors = DriveHas4Motors(drive)? 4: 2;
   143 
   144     //
   145     // Stop the motors.
   146     //
   147     for (int i = 0; i < numMotors; i++)
   148     {
   149         drive.motorPowers[i] = 0;
   150         drive.motorEncoders[i] = 0;
   151         drive.motorSpeeds[i] = 0.0;
   152         motor[drive.motorIDs[i]] = 0;
   153         nMotorEncoder[drive.motorIDs[i]] = 0;
   154     }
   155     drive.prevTime = GetMsecTime();
   156     drive.stallTimer = 0;
   157     drive.flags &= ~DRIVEF_STATE_MASK;
   158 
   159     TExit();
   160     return;
   161 }   //DriveReset
   162 
   163 /**
   164  *  This function initializes the drive system for 2-motor drive.
   165  *
   166  *  @param drive Points to the DRIVE structure.
   167  *  @param leftMotor Specifies the left motor.
   168  *  @param rightMotor Specifies the right motor.
   169  */
   170 void
   171 DriveInit(
   172     DRIVE &drive,
   173     tMotor leftMotor,
   174     tMotor rightMotor
   175     )
   176 {
   177     TFuncName("DriveInit");
   178     TLevel(INIT);
   179     TEnter();
   180 
   181     for (int i = 0; i < MAX_NUM_MOTORS; i++)
   182     {
   183         drive.motorIDs[i] = (tMotor)-1;
   184 #ifdef _SERVO_H
   185         drive.servos[i] = NULL;
   186 #endif
   187         drive.motorPowers[i] = 0;
   188         drive.motorEncoders[i] = 0;
   189         drive.motorSpeeds[i] = 0.0;
   190     }
   191     drive.motorIDs[IDX_FRONT_LEFT] = leftMotor;
   192     drive.motorIDs[IDX_FRONT_RIGHT] = rightMotor;
   193     drive.prevTime = GetMsecTime();
   194     drive.stallTimer = 0;
   195     drive.flags = 0;
   196     drive.xPos = 0.0;
   197     drive.yPos = 0.0;
   198     drive.rotPos = 0.0;
   199 
   200     TExit();
   201     return;
   202 }   //DriveInit
   203 
   204 /**
   205  *  This function initializes the drive system for 4-motor drive.
   206  *
   207  *  @param drive Points to the DRIVE structure.
   208  *  @param frontLeftMotor Specifies the front left motor.
   209  *  @param frontRightMotor Specifies the front right motor.
   210  *  @param rearLeftMotor Specifies the rear left motor.
   211  *  @param rearRightMotor Specifies the rear right motor.
   212  */
   213 void
   214 DriveInit(
   215     DRIVE &drive,
   216     tMotor frontLeftMotor,
   217     tMotor frontRightMotor,
   218     tMotor rearLeftMotor,
   219     tMotor rearRightMotor
   220     )
   221 {
   222     TFuncName("DriveInit");
   223     TLevel(INIT);
   224     TEnter();
   225 
   226     DriveInit(drive, frontLeftMotor, frontRightMotor);
   227     drive.motorIDs[IDX_REAR_LEFT] = rearLeftMotor;
   228     drive.motorIDs[IDX_REAR_RIGHT] = rearRightMotor;
   229     drive.flags |= DRIVEF_FOUR_MOTORS;
   230 
   231     TExit();
   232     return;
   233 }   //DriveInit
   234 
   235 #ifdef _SERVO_H
   236 /**
   237  *  This function initializes the drive system for 4-motor and 4-servo drive.
   238  *
   239  *  @param drive Points to the DRIVE structure.
   240  *  @param frontLeftMotor Specifies the front left motor.
   241  *  @param frontRightMotor Specifies the front right motor.
   242  *  @param rearLeftMotor Specifies the rear left motor.
   243  *  @param rearRightMotor Specifies the rear right motor.
   244  *  @param frontLeftServo Specifies the front left servo motor.
   245  *  @param frontRightServo Specifies the front right servo motor.
   246  *  @param rearLeftServo Specifies the rear left servo motor.
   247  *  @param rearRightServo Specifies the rear right servo motor.
   248  *  @param lowAngleLimit Optionally specifies the servo low angle limit.
   249  *  @param highAngleLimit Optionally specifies the servo high angle limit.
   250  */
   251 void
   252 DriveInit(
   253     DRIVE  &drive,
   254     tMotor  frontLeftMotor,
   255     tMotor  frontRightMotor,
   256     tMotor  rearLeftMotor,
   257     tMotor  rearRightMotor,
   258     SERVO  &frontLeftServo,
   259     SERVO  &frontRightServo,
   260     SERVO  &rearLeftServo,
   261     SERVO  &rearRightServo,
   262     float   lowAngleLimit = -90.0,
   263     float   highAngleLimit = 90.0
   264     )
   265 {
   266     TFuncName("DriveInit");
   267     TLevel(INIT);
   268     TEnter();
   269 
   270     DriveInit(drive, frontLeftMotor, frontRightMotor,
   271               rearLeftMotor, rearRightMotor);
   272     drive.servos[IDX_FRONT_LEFT] = &frontLeftServo;
   273     drive.servos[IDX_FRONT_RIGHT] = &frontRightServo;
   274     drive.servos[IDX_REAR_LEFT] = &rearLeftServo;
   275     drive.servos[IDX_REAR_RIGHT] = &rearRightServo;
   276     drive.lowAngleLimit = lowAngleLimit;
   277     drive.highAngleLimit = highAngleLimit;
   278     drive.flags |= DRIVEF_SWERVE;
   279 
   280     TExit();
   281     return;
   282 }   //DriveInit
   283 #endif
   284 
   285 /**
   286  *  This function enables or disables stall protection.
   287  *
   288  *  @param drive Points to the DRIVE structure.
   289  *  @param fOn If true, enables stall protection.
   290  */
   291 void
   292 DriveStallProtect(
   293     DRIVE &drive,
   294     bool fOn
   295     )
   296 {
   297     TFuncName("DriveStallProtect");
   298     TLevel(API);
   299     TEnterMsg(("fOn=%d", (byte)fOn));
   300 
   301     if (fOn)
   302     {
   303         drive.flags |= DRIVEF_STALL_PROTECT_ON;
   304     }
   305     else
   306     {
   307         drive.flags &= ~DRIVEF_STALL_PROTECT_ON;
   308     }
   309 
   310     TExit();
   311     return;
   312 }   //DriveStallProtect
   313 
   314 /**
   315  *  This function sets power of the motors for tank drive.
   316  *
   317  *  @param drive Points to the DRIVE structure.
   318  *  @param leftPower Specifies the left motor power.
   319  *  @param rightPower Specifies the right motor power.
   320  */
   321 void
   322 DriveTank(
   323     DRIVE &drive,
   324     int leftPower,
   325     int rightPower
   326     )
   327 {
   328     TFuncName("DriveTank");
   329     TLevel(API);
   330     TEnterMsg(("Left=%d,Right=%d", leftPower, rightPower));
   331 
   332     drive.motorPowers[IDX_FRONT_LEFT] =
   333         BOUND(leftPower, MOTOR_MIN_VALUE, MOTOR_MAX_VALUE);
   334     drive.motorPowers[IDX_FRONT_RIGHT] =
   335         BOUND(rightPower, MOTOR_MIN_VALUE, MOTOR_MAX_VALUE);
   336     if (DriveHas4Motors(drive))
   337     {
   338         drive.motorPowers[IDX_REAR_LEFT] =
   339             drive.motorPowers[IDX_FRONT_LEFT];
   340         drive.motorPowers[IDX_REAR_RIGHT] =
   341             drive.motorPowers[IDX_FRONT_RIGHT];
   342     }
   343     drive.flags |= DRIVEF_ON;
   344 
   345     TExit();
   346     return;
   347 }   //DriveTank
   348 
   349 /**
   350  *  This function sets power of the motors for arcade drive.
   351  *
   352  *  @param drive Points to the DRIVE structure.
   353  *  @param drivePower Specifies the drive power.
   354  *  @param turnPower Specifies the turn power.
   355  *  @param crabDir Optionally specifies the crabbing direction:
   356  *          CRABDIR_NONE: No crabbing (default).
   357  *          CRABDIR_LEFT: Crab to left.
   358  *          CRABDIR_RIGHT: Crab to right.
   359  */
   360 void
   361 DriveArcade(
   362     DRIVE &drive,
   363     int drivePower,
   364     int turnPower,
   365     int crabDir = CRABDIR_NONE
   366     )
   367 {
   368     TFuncName("DriveArcade");
   369     TLevel(API);
   370     TEnterMsg(("Drive=%d,Turn=%d,Crab=%d", drivePower, turnPower, crabDir));
   371 
   372     int leftPower, rightPower;
   373 
   374     drivePower = BOUND(drivePower, MOTOR_MIN_VALUE, MOTOR_MAX_VALUE);
   375     turnPower = BOUND(turnPower, MOTOR_MIN_VALUE, MOTOR_MAX_VALUE);
   376     if (drivePower + turnPower > MOTOR_MAX_VALUE)
   377     {
   378         //
   379         // Forward right:
   380         //  left = drive + turn - (drive + turn - MOTOR_MAX_VALUE)
   381         //  right = drive - turn - (drive + turn - MOTOR_MAX_VALUE)
   382         //
   383         leftPower = MOTOR_MAX_VALUE;
   384         rightPower = -2*turnPower + MOTOR_MAX_VALUE;
   385     }
   386     else if (drivePower - turnPower > MOTOR_MAX_VALUE)
   387     {
   388         //
   389         // Forward left:
   390         //  left = drive + turn - (drive - turn - MOTOR_MAX_VALUE)
   391         //  right = drive - turn - (drive - turn - MOTOR_MAX_VALUE)
   392         //
   393         leftPower = 2*turnPower + MOTOR_MAX_VALUE;
   394         rightPower = MOTOR_MAX_VALUE;
   395     }
   396     else if (drivePower + turnPower < MOTOR_MIN_VALUE)
   397     {
   398         //
   399         // Backward left:
   400         //  left = drive + turn - (drive + turn - MOTOR_MIN_VALUE)
   401         //  right = drive - turn - (drive + turn - MOTOR_MIN_VALUE)
   402         //
   403         leftPower = MOTOR_MIN_VALUE;
   404         rightPower = -2*turnPower + MOTOR_MIN_VALUE;
   405     }
   406     else if (drivePower - turnPower < MOTOR_MIN_VALUE)
   407     {
   408         //
   409         // Backward right:
   410         //  left = drive + turn - (drive - turn - MOTOR_MIN_VALUE)
   411         //  right = drive - turn - (drive - turn - MOTOR_MIN_VALUE)
   412         //
   413         leftPower = 2*turnPower + MOTOR_MIN_VALUE;
   414         rightPower = MOTOR_MIN_VALUE;
   415     }
   416     else
   417     {
   418         leftPower = drivePower + turnPower;
   419         rightPower = drivePower - turnPower;
   420     }
   421 
   422     if (DriveHas4Motors(drive))
   423     {
   424         if (crabDir == CRABDIR_LEFT)
   425         {
   426             drive.motorPowers[IDX_FRONT_LEFT] = rightPower;
   427             drive.motorPowers[IDX_FRONT_RIGHT] = rightPower;
   428             drive.motorPowers[IDX_REAR_LEFT] = leftPower;
   429             drive.motorPowers[IDX_REAR_RIGHT] = leftPower;
   430         }
   431         else if (crabDir == CRABDIR_RIGHT)
   432         {
   433             drive.motorPowers[IDX_FRONT_LEFT] = leftPower;
   434             drive.motorPowers[IDX_FRONT_RIGHT] = leftPower;
   435             drive.motorPowers[IDX_REAR_LEFT] = rightPower;
   436             drive.motorPowers[IDX_REAR_RIGHT] = rightPower;
   437         }
   438         else
   439         {
   440             drive.motorPowers[IDX_FRONT_LEFT] = leftPower;
   441             drive.motorPowers[IDX_FRONT_RIGHT] = rightPower;
   442             drive.motorPowers[IDX_REAR_LEFT] = leftPower;
   443             drive.motorPowers[IDX_REAR_RIGHT] = rightPower;
   444         }
   445     }
   446     else
   447     {
   448         drive.motorPowers[IDX_FRONT_LEFT] = leftPower;
   449         drive.motorPowers[IDX_FRONT_RIGHT] = rightPower;
   450     }
   451 
   452     drive.flags |= DRIVEF_ON;
   453 
   454     TExit();
   455     return;
   456 }   //DriveArcade
   457 
   458 #ifdef _SERVO_H
   459 /**
   460  *  This function sets the wheel angles for swerve drive.
   461  *
   462  *  @param drive Points to the DRIVE structure.
   463  *  @param frontLeftAngle Specifies the angle of the front left wheel.
   464  *  @param frontRightAngle Specifies the angle of the front right wheel.
   465  *  @param rearLeftAngle Specifies the angle of the rear left wheel.
   466  *  @param rearRightAngle Specifies the angle of the rear right wheel.
   467  */
   468 void
   469 DriveSwerveSetAngles(
   470     DRIVE &drive,
   471     float frontLeftAngle,
   472     float frontRightAngle,
   473     float rearLeftAngle,
   474     float rearRightAngle
   475     )
   476 {
   477     TFuncName("DriveSwerveSetAngles");
   478     TLevel(API);
   479     TEnter();
   480 
   481     if (DriveIsSwerve(drive))
   482     {
   483         frontLeftAngle = BOUND(frontLeftAngle,
   484                                drive.lowAngleLimit, drive.highAngleLimit);
   485         frontRightAngle = BOUND(frontRightAngle,
   486                                 drive.lowAngleLimit, drive.highAngleLimit);
   487         rearLeftAngle = BOUND(rearLeftAngle,
   488                               drive.lowAngleLimit, drive.highAngleLimit);
   489         rearRightAngle = BOUND(rearRightAngle,
   490                                drive.lowAngleLimit, drive.highAngleLimit);
   491         ServoSetAngle(*drive.servos[IDX_FRONT_LEFT], frontLeftAngle);
   492         ServoSetAngle(*drive.servos[IDX_FRONT_RIGHT], frontRightAngle);
   493         ServoSetAngle(*drive.servos[IDX_REAR_LEFT], rearLeftAngle);
   494         ServoSetAngle(*drive.servos[IDX_REAR_RIGHT], rearRightAngle);
   495     }
   496 
   497     TExit();
   498     return;
   499 }   //DriveSwerveSetAngles
   500 
   501 /**
   502  *  This function sets power of the motors for swerve drive in cartesian
   503  *  coordinate system.
   504  *
   505  *  @param drive Points to the DRIVE structure.
   506  *  @param x Specifies the x speed.
   507  *  @param y Specifies the y speed.
   508  *  @param rot Specifies the rotaton speed.
   509  *  @param diffRot Optionally specifies if using left/right differential
   510  *                 to rotate.
   511  *  @param retNeutral Optionally specifies if the wheels will return to
   512  *         neutral angle when we stop driving.
   513  *  @param noDrive Optionally specifies if the drive motors will not move when
   514  *         true. This is useful for turning the servos only so the wheels are
   515  *         in the correct direction before we start driving.
   516  */
   517 void
   518 DriveSwerve(
   519     DRIVE &drive,
   520     int x,
   521     int y,
   522     int rot,
   523     bool diffRot = false,
   524     bool retNeutral = false,
   525     bool noDrive = false
   526     )
   527 {
   528     TFuncName("DriveSwerve");
   529     TLevel(API);
   530     TEnterMsg(("x=%d,y=%d,rot=%d", x, y, rot));
   531 
   532     x = BOUND(x, MOTOR_MIN_VALUE, MOTOR_MAX_VALUE);
   533     y = BOUND(y, MOTOR_MIN_VALUE, MOTOR_MAX_VALUE);
   534     rot = BOUND(rot, MOTOR_MIN_VALUE, MOTOR_MAX_VALUE);
   535 
   536     //
   537     // Calculate the magnitude and scale to a maximum of MAX_MOTOR_VALUE.
   538     //
   539     int mag = BOUND((int)sqrt(x*x + y*y), MOTOR_MIN_VALUE, MOTOR_MAX_VALUE);
   540     float angle;
   541 
   542     if (mag == 0)
   543     {
   544         angle = 0.0;
   545     }
   546     else
   547     {
   548         angle = 90.0 - atan2(y, x)*180.0/PI;
   549         if (angle > 90.0)
   550         {
   551             angle -= 180.0;
   552             mag = -mag;
   553         }
   554     }
   555 
   556     if ((mag == 0) && (rot != 0) && !diffRot)
   557     {
   558         //
   559         // We are doing rotate only. We will set the servo angles to
   560         // a fixed 45-degree diamond formation for in-place rotation.
   561         // We will set the driving wheel power proportional to the
   562         // rotation magnitude. We must apply some power to the wheel
   563         // motors because, first of all, the user is expecting the
   564         // robot to turn. Without wheel power, the robot won't turn.
   565         // Secondly, with the robot not moving, the friction on the
   566         // wheels may be too big to even turn the servos.
   567         //
   568         ServoSetAngle(*drive.servos[IDX_FRONT_LEFT],
   569                       NORMALIZE(45.0,
   570                                 -90.0, 90.0,
   571                                 drive.lowAngleLimit,
   572                                 drive.highAngleLimit));
   573         ServoSetAngle(*drive.servos[IDX_FRONT_RIGHT],
   574                       NORMALIZE(-45.0,
   575                                 -90.0, 90.0,
   576                                 drive.lowAngleLimit,
   577                                 drive.highAngleLimit));
   578         ServoSetAngle(*drive.servos[IDX_REAR_LEFT],
   579                       NORMALIZE(-45.0,
   580                                 -90.0, 90.0,
   581                                 drive.lowAngleLimit,
   582                                 drive.highAngleLimit));
   583         ServoSetAngle(*drive.servos[IDX_REAR_RIGHT],
   584                       NORMALIZE(45.0,
   585                                 -90.0, 90.0,
   586                                 drive.lowAngleLimit,
   587                                 drive.highAngleLimit));
   588 
   589         drive.motorPowers[IDX_FRONT_LEFT] =
   590         drive.motorPowers[IDX_REAR_LEFT] =
   591             noDrive? 0: rot;
   592         drive.motorPowers[IDX_FRONT_RIGHT] =
   593         drive.motorPowers[IDX_REAR_RIGHT] =
   594             noDrive? 0: -rot;
   595     }
   596     else if (diffRot)
   597     {
   598         //
   599         // Do rotation by setting different speed of the left and right
   600         // wheels.
   601         //
   602         float prevAngle = ServoGetAngle(*drive.servos[IDX_FRONT_LEFT]);
   603         if ((x != 0) && (y == 0) && (rot == 0) &&
   604             (abs(angle - prevAngle) == 180.0))
   605         {
   606             //
   607             // Crab only mode. In this scenario, make sure we don't
   608             // turn the wheels 180-degree when changing direction.
   609             // We can just spin the wheels the other way. This will
   610             // prevent the robot from twitching.
   611             //
   612             angle = prevAngle;
   613             mag = -mag;
   614         }
   615 
   616         if ((angle != prevAngle) && ((mag != 0.0) || retNeutral))
   617         {
   618             ServoSetAngle(*drive.servos[IDX_FRONT_LEFT], angle);
   619             ServoSetAngle(*drive.servos[IDX_FRONT_RIGHT], angle);
   620             ServoSetAngle(*drive.servos[IDX_REAR_LEFT], angle);
   621             ServoSetAngle(*drive.servos[IDX_REAR_RIGHT], angle);
   622         }
   623         DriveArcade(drive, mag, rot,
   624                     (angle == -90.0)? CRABDIR_LEFT:
   625                     (angle == 90.0)? CRABDIR_RIGHT: CRABDIR_NONE);
   626     }
   627     else
   628     {
   629         rot = NORMALIZE(rot,
   630                         MOTOR_MIN_VALUE, MOTOR_MAX_VALUE,
   631                         -45.0, 45.0);
   632 
   633         float frontAngle = NORMALIZE(BOUND(angle + rot, -90.0, 90.0),
   634                                      -90.0,
   635                                      90.0,
   636                                      drive.lowAngleLimit,
   637                                      drive.highAngleLimit);
   638         float rearAngle = NORMALIZE(BOUND(angle - rot, -90.0, 90.0),
   639                                     -90.0,
   640                                     90.0,
   641                                     drive.lowAngleLimit,
   642                                     drive.highAngleLimit);
   643 
   644         if ((x != 0) && (y == 0) && (rot == 0) &&
   645             (abs(frontAngle -
   646                  ServoGetAngle(*drive.servos[IDX_FRONT_LEFT])) == 180.0))
   647         {
   648             //
   649             // Crab only mode. In this scenario, make sure we don't
   650             // turn the wheels 180-degree when changing direction.
   651             // We can just spin the wheels the other way. This will
   652             // prevent the robot from twitching.
   653             //
   654             frontAngle = -frontAngle;
   655             rearAngle = -rearAngle;
   656             mag = -mag;
   657         }
   658 
   659         if (mag != 0 || retNeutral)
   660         {
   661             //
   662             // Turn the wheels only if we are moving. This will leave the
   663             // wheels at the previous angles and not turn the wheels to
   664             // forward direction if we let go of the joystick, for example.
   665             // This will eliminate unnecessary twitching of the robot.
   666             //
   667             ServoSetAngle(*drive.servos[IDX_FRONT_LEFT], frontAngle);
   668             ServoSetAngle(*drive.servos[IDX_FRONT_RIGHT], frontAngle);
   669             ServoSetAngle(*drive.servos[IDX_REAR_LEFT], rearAngle);
   670             ServoSetAngle(*drive.servos[IDX_REAR_RIGHT], rearAngle);
   671         }
   672 
   673         for (int i = 0; i < MAX_NUM_MOTORS; i++)
   674         {
   675             drive.motorPowers[i] = noDrive? 0: mag;
   676         }
   677     }
   678 
   679     drive.flags |= DRIVEF_ON;
   680 
   681     TExit();
   682     return;
   683 }   //DriveSwerve
   684 #endif
   685 
   686 /**
   687  *  This function sets power of the motors for mecanum drive in cartesian
   688  *  coordinate system.
   689  *
   690  *  @param drive Points to the DRIVE structure.
   691  *  @param x Specifies the x speed.
   692  *  @param y Specifies the y speed.
   693  *  @param rot Specifies the rotaton speed.
   694  */
   695 void
   696 DriveMecanumCartesian(
   697     DRIVE &drive,
   698     int x,
   699     int y,
   700     int rot
   701     )
   702 {
   703     TFuncName("MecanumCartesian");
   704     TLevel(API);
   705     TEnterMsg(("x=%d,y=%d,rot=%d", x, y, rot));
   706 
   707 #ifdef _SERVO_H
   708     if (drive.flags & DRIVEF_SWERVE)
   709     {
   710         DriveSwerve(drive, x, y, rot);
   711     }
   712     else
   713 #endif
   714     if (DriveHas4Motors(drive))
   715     {
   716         int mag, maxMag, i;
   717 
   718         drive.motorPowers[IDX_FRONT_LEFT] = x + y + rot;
   719         drive.motorPowers[IDX_FRONT_RIGHT] = -x + y - rot;
   720         drive.motorPowers[IDX_REAR_LEFT] = -x + y + rot;
   721         drive.motorPowers[IDX_REAR_RIGHT] = x + y - rot;
   722         //
   723         // Normalize
   724         //
   725         maxMag = abs(drive.motorPowers[0]);
   726         for (i = 1; i < MAX_NUM_MOTORS; i++)
   727         {
   728             mag = abs(drive.motorPowers[i]);
   729             if (mag > maxMag)
   730             {
   731                 maxMag = mag;
   732             }
   733         }
   734 
   735         if (maxMag > MOTOR_MAX_VALUE)
   736         {
   737             for (i = 0; i < MAX_NUM_MOTORS; i++)
   738             {
   739                 drive.motorPowers[i] =
   740                     (drive.motorPowers[i]*MOTOR_MAX_VALUE)/maxMag;
   741             }
   742         }
   743     }
   744     else
   745     {
   746         //
   747         // Mecanum drive is only possible with 4 motors. For 2 motors, we
   748         // do arcade drive instead.
   749         //
   750         DriveArcade(drive, y, rot);
   751     }
   752 
   753     drive.flags |= DRIVEF_ON;
   754 
   755     TExit();
   756     return;
   757 }   //DriveMecanumCartesian
   758 
   759 /**
   760  *  This function sets power of the motors for mecanum drive in polar
   761  *  coordinate system.
   762  *
   763  *  @param drive Points to the DRIVE structure.
   764  *  @param mag Specifies the magnitude.
   765  *  @param dir Specifies the direction.
   766  *  @param rot Specifies the rotaton.
   767  */
   768 void
   769 DriveMecanumPolar(
   770     DRIVE &drive,
   771     int mag,
   772     int dir,
   773     int rot
   774     )
   775 {
   776     TFuncName("MecanumPolar");
   777     TLevel(API);
   778     TEnterMsg(("m=%d,d=%d,r=%d", mag, dir, rot));
   779 
   780     if (DriveHas4Motors(drive))
   781     {
   782         float magnitude = (BOUND(mag, MOTOR_MIN_VALUE, MOTOR_MAX_VALUE)*
   783                            sqrt(2.0))/MOTOR_MAX_VALUE;
   784         float rotation = (float)BOUND(rot, MOTOR_MIN_VALUE, MOTOR_MAX_VALUE)/
   785                          MOTOR_MAX_VALUE;
   786         float dirInRad = (dir + 45.0)*PI/180.0;
   787         float sinD = sin(dirInRad);
   788         float cosD = cos(dirInRad);
   789         float powers[MAX_NUM_MOTORS];
   790 
   791         powers[IDX_FRONT_LEFT] = sinD*magnitude + rotation;
   792         powers[IDX_FRONT_RIGHT] = cosD*magnitude - rotation;
   793         powers[IDX_REAR_LEFT] = cosD*magnitude + rotation;
   794         powers[IDX_REAR_RIGHT] = sinD*magnitude - rotation;
   795         //
   796         // Normalize
   797         //
   798         float maxMag = abs(powers[0]);
   799         float mag;
   800         int i;
   801 
   802         for (i = 1; i < MAX_NUM_MOTORS; i++)
   803         {
   804             mag = abs(powers[i]);
   805             if (mag > maxMag)
   806             {
   807                 maxMag = mag;
   808             }
   809         }
   810 
   811         for (i = 0; i < MAX_NUM_MOTORS; i++)
   812         {
   813             if (maxMag > 1.0)
   814             {
   815                 drive.motorPowers[i] = (int)
   816                     (powers[i]*MOTOR_MAX_VALUE/maxMag);
   817             }
   818             else
   819             {
   820                 drive.motorPowers[i] = (int)(powers[i]*MOTOR_MAX_VALUE);
   821             }
   822         }
   823     }
   824     else
   825     {
   826         //
   827         // Mecanum drive is only possible with 4 motors. For 2 motors, we
   828         // do arcade drive instead.
   829         //
   830         DriveArcade(drive, mag, rot);
   831     }
   832 
   833     drive.flags |= DRIVEF_ON;
   834 
   835     TExit();
   836     return;
   837 }   //DriveMecanumPolar
   838 
   839 /**
   840  *  This function performs the driving task according to the drive state.
   841  *
   842  *  @param drive Points to the DRIVE structure.
   843  */
   844 void
   845 DriveTask(
   846     DRIVE &drive
   847     )
   848 {
   849     TFuncName("DriveTask");
   850     TLevel(TASK);
   851     TEnter();
   852 
   853     if (drive.flags & DRIVEF_ON)
   854     {
   855         unsigned long currTime = GetMsecTime();
   856         float dt = (currTime - drive.prevTime)/1000.0;
   857         long flPos = nMotorEncoder[drive.motorIDs[IDX_FRONT_LEFT]];
   858         long frPos = nMotorEncoder[drive.motorIDs[IDX_FRONT_RIGHT]];
   859         long rlPos = 0;
   860         long rrPos = 0;
   861         long flDelta = flPos - drive.motorEncoders[IDX_FRONT_LEFT];
   862         long frDelta = frPos - drive.motorEncoders[IDX_FRONT_RIGHT];
   863         long rlDelta = 0;
   864         long rrDelta = 0;
   865         float flSpeed = (float)flDelta/dt;
   866         float frSpeed = (float)frDelta/dt;
   867         float rlSpeed = 0.0;
   868         float rrSpeed = 0.0;
   869 
   870         if (abs(flSpeed - drive.motorSpeeds[IDX_FRONT_LEFT]) > 10000.0)
   871         {
   872             //
   873             // Bogus encoder value, ignore it and assume previous speed.
   874             //
   875 #ifdef _DEBUG_DRIVE
   876             TPrintfLine("Bogus flSpeed: %6.1f", flSpeed);
   877 #endif
   878             flSpeed = drive.motorSpeeds[IDX_FRONT_LEFT];
   879             flDelta = (long)(flSpeed*dt);
   880             flPos = drive.motorEncoders[IDX_FRONT_LEFT] + flDelta;
   881         }
   882         else
   883         {
   884             drive.motorSpeeds[IDX_FRONT_LEFT] = flSpeed;
   885         }
   886 
   887         if (abs(frSpeed - drive.motorSpeeds[IDX_FRONT_RIGHT]) > 10000.0)
   888         {
   889             //
   890             // Bogus encoder value, ignore it and assume previous speed.
   891             //
   892 #ifdef _DEBUG_DRIVE
   893             TPrintfLine("Bogus frSpeed: %6.1f", frSpeed);
   894 #endif
   895             frSpeed = drive.motorSpeeds[IDX_FRONT_RIGHT];
   896             frDelta = (long)(frSpeed*dt);
   897             frPos = drive.motorEncoders[IDX_FRONT_RIGHT] + frDelta;
   898         }
   899         else
   900         {
   901             drive.motorSpeeds[IDX_FRONT_RIGHT] = frSpeed;
   902         }
   903 
   904         if (DriveHas4Motors(drive))
   905         {
   906             rlPos = nMotorEncoder[drive.motorIDs[IDX_REAR_LEFT]];
   907             rrPos = nMotorEncoder[drive.motorIDs[IDX_REAR_RIGHT]];
   908             rlDelta = rlPos - drive.motorEncoders[IDX_REAR_LEFT];
   909             rrDelta = rrPos - drive.motorEncoders[IDX_REAR_RIGHT];
   910             rlSpeed = (float)rlDelta/dt;
   911             rrSpeed = (float)rrDelta/dt;
   912 
   913             if (abs(rlSpeed - drive.motorSpeeds[IDX_REAR_LEFT]) > 10000.0)
   914             {
   915                 //
   916                 // Bogus encoder value, ignore it and assume previous speed.
   917                 //
   918 #ifdef _DEBUG_DRIVE
   919                 TPrintfLine("Bogus rlSpeed: %6.1f", rlSpeed);
   920 #endif
   921                 rlSpeed = drive.motorSpeeds[IDX_REAR_LEFT];
   922                 rlDelta = (long)(rlSpeed*dt);
   923                 rlPos = drive.motorEncoders[IDX_REAR_LEFT] + rlDelta;
   924             }
   925             else
   926             {
   927                 drive.motorSpeeds[IDX_REAR_LEFT] = rlSpeed;
   928             }
   929 
   930             if (abs(rrSpeed - drive.motorSpeeds[IDX_REAR_RIGHT]) > 10000.0)
   931             {
   932                 //
   933                 // Bogus encoder value, ignore it and assume previous speed.
   934                 //
   935 #ifdef _DEBUG_DRIVE
   936                 TPrintfLine("Bogus rrSpeed: %6.1f", rrSpeed);
   937 #endif
   938                 rrSpeed = drive.motorSpeeds[IDX_REAR_RIGHT];
   939                 rrDelta = (long)(rrSpeed*dt);
   940                 rrPos = drive.motorEncoders[IDX_REAR_RIGHT] + rrDelta;
   941             }
   942             else
   943             {
   944                 drive.motorSpeeds[IDX_REAR_RIGHT] = rrSpeed;
   945             }
   946 
   947             if (!DriveIsSwerve(drive))
   948             {
   949                 //
   950                 // Macanum drive
   951                 //
   952                 drive.xPos += ((flDelta + rrDelta) - (frDelta + rlDelta))/4.0;
   953                 drive.yPos += (flDelta + frDelta + rlDelta + rrDelta)/4.0;
   954                 drive.rotPos += ((flDelta + rlDelta) - (frDelta + rrDelta))/4.0;
   955             }
   956 #ifdef _SERVO_H
   957             else
   958             {
   959                 //
   960                 // Swerve drive.
   961                 //
   962                 float flAngle = ServoGetAngle(*drive.servos[IDX_FRONT_LEFT]);
   963                 float rlAngle = ServoGetAngle(*drive.servos[IDX_REAR_LEFT]);
   964                 float frAngle = ServoGetAngle(*drive.servos[IDX_FRONT_RIGHT]);
   965                 float rrAngle = ServoGetAngle(*drive.servos[IDX_REAR_RIGHT]);
   966                 bool fTranslating = flAngle == frAngle &&
   967                                     rlAngle == rrAngle &&
   968                                     flAngle == rrAngle;
   969                 bool fInPlaceTurn = !fTranslating &&
   970                                     flAngle == rrAngle &&
   971                                     frAngle == rlAngle;
   972                 float angle = ((flAngle + rlAngle + frAngle + rrAngle)/4.0)*
   973                               PI/180.0;
   974 
   975                 if (fTranslating)
   976                 {
   977                     float delta = (flDelta + frDelta + rlDelta + rrDelta)/4.0;
   978 
   979                     drive.xPos += delta*sin(angle);
   980                     drive.yPos += delta*cos(angle);
   981                 }
   982                 else if (fInPlaceTurn)
   983                 {
   984                     drive.rotPos += ((flDelta + rlDelta) - (frDelta + rrDelta))/
   985                                     4.0;
   986                 }
   987                 else
   988                 {
   989                     //???
   990                     drive.rotPos += (flDelta + frDelta + rlDelta + rrDelta)/4.0;
   991                 }
   992             }
   993 #endif
   994         }
   995         else
   996         {
   997             drive.yPos += (flDelta + frDelta)/2.0;
   998             drive.rotPos += (flDelta - frDelta)/2.0;
   999         }
  1000 
  1001 #ifdef _DEBUG_DRIVE
  1002         TPrintfLine("dT=%d", currTime - drive.prevTime);
  1003         TPrintfLine("flPower=%4d, rlPower=%4d, frPower=%4d, rrPower=%4d",
  1004                     drive.motorPowers[IDX_FRONT_LEFT],
  1005                     drive.motorPowers[IDX_REAR_LEFT],
  1006                     drive.motorPowers[IDX_FRONT_RIGHT],
  1007                     drive.motorPowers[IDX_REAR_RIGHT]);
  1008         TPrintfLine("flPos=%6d, rlPos=%6d, frPos=%6d, rrPos=%6d",
  1009                     flPos, rlPos, frPos, rrPos);
  1010         TPrintfLine("flSpeed=%6.1f, rlSpeed=%6.1f, frSpeed=%6.1f, rrSpeed=%6.1f",
  1011                     drive.motorSpeeds[IDX_FRONT_LEFT],
  1012                     drive.motorSpeeds[IDX_REAR_LEFT],
  1013                     drive.motorSpeeds[IDX_FRONT_RIGHT],
  1014                     drive.motorSpeeds[IDX_REAR_RIGHT]);
  1015         TPrintfLine("xPos=%6.1f, yPos=%6.1f, rotPos=%6.1f\n",
  1016                     drive.xPos, drive.yPos, drive.rotPos);
  1017 #endif
  1018 
  1019         if ((drive.flags & DRIVEF_STALLED) == 0)
  1020         {
  1021             motor[drive.motorIDs[IDX_FRONT_LEFT]] =
  1022                 drive.motorPowers[IDX_FRONT_LEFT];
  1023             motor[drive.motorIDs[IDX_FRONT_RIGHT]] =
  1024                 drive.motorPowers[IDX_FRONT_RIGHT];
  1025             if (DriveHas4Motors(drive))
  1026             {
  1027                 motor[drive.motorIDs[IDX_REAR_LEFT]] =
  1028                     drive.motorPowers[IDX_REAR_LEFT];
  1029                 motor[drive.motorIDs[IDX_REAR_RIGHT]] =
  1030                     drive.motorPowers[IDX_REAR_RIGHT];
  1031             }
  1032 
  1033             if (drive.flags & DRIVEF_STALL_PROTECT_ON)
  1034             {
  1035                 unsigned long currTime = nPgmTime;
  1036                 //
  1037                 // Check for motor stall conditions:
  1038                 // - Stall timer is set AND
  1039                 // - Any motors are powered above MIN_STALL_POWER AND
  1040                 // - All motors have not moved
  1041                 //
  1042                 if ((drive.stallTimer == 0) ||
  1043                     (abs(drive.motorPowers[IDX_FRONT_LEFT]) <=
  1044                      DRIVE_MIN_STALL_POWER) &&
  1045                     (abs(drive.motorPowers[IDX_FRONT_RIGHT]) <=
  1046                      DRIVE_MIN_STALL_POWER) &&
  1047                     (!DriveHas4Motors(drive) ||
  1048                      (abs(drive.motorPowers[IDX_REAR_LEFT]) <=
  1049                       DRIVE_MIN_STALL_POWER) &&
  1050                      (abs(drive.motorPowers[IDX_REAR_RIGHT]) <=
  1051                       DRIVE_MIN_STALL_POWER)) ||
  1052                     (flPos != drive.motorEncoders[IDX_FRONT_LEFT]) ||
  1053                     (frPos != drive.motorEncoders[IDX_FRONT_RIGHT]) ||
  1054                     DriveHas4Motors(drive) &&
  1055                     ((rlPos != drive.motorEncoders[IDX_REAR_LEFT]) ||
  1056                      (rrPos != drive.motorEncoders[IDX_REAR_RIGHT])))
  1057                 {
  1058                     //
  1059                     // We are not in a stalled situation if any of the
  1060                     // following are true:
  1061                     // - motor powers are below min stall power
  1062                     // - motor encoders showed that they have moved
  1063                     //
  1064                     drive.stallTimer = currTime;
  1065                 }
  1066 
  1067                 if (currTime - drive.stallTimer >= DRIVE_STALL_TIME)
  1068                 {
  1069                     //
  1070                     // We have detected a stalled condition for at
  1071                     // DRIVE_STALL_TIME.
  1072                     // Let's kill the power of all the motors.
  1073                     //
  1074                     motor[drive.motorIDs[IDX_FRONT_LEFT]] = 0;
  1075                     motor[drive.motorIDs[IDX_FRONT_RIGHT]] = 0;
  1076                     if (DriveHas4Motors(drive))
  1077                     {
  1078                         motor[drive.motorIDs[IDX_REAR_LEFT]] = 0;
  1079                         motor[drive.motorIDs[IDX_REAR_RIGHT]] = 0;
  1080                     }
  1081                     drive.flags |= DRIVEF_STALLED;
  1082                     PlayImmediateTone(1000, 100);
  1083                 }
  1084             }
  1085         }
  1086 
  1087         drive.prevTime = currTime;
  1088         drive.motorEncoders[IDX_FRONT_LEFT] = flPos;
  1089         drive.motorEncoders[IDX_FRONT_RIGHT] = frPos;
  1090         if (DriveHas4Motors(drive))
  1091         {
  1092             drive.motorEncoders[IDX_REAR_LEFT] = rlPos;
  1093             drive.motorEncoders[IDX_REAR_RIGHT] = rrPos;
  1094         }
  1095     }
  1096     else
  1097     {
  1098         //
  1099         // The motors should be OFF.
  1100         // Let's make sure they are.
  1101         //
  1102         motor[drive.motorIDs[IDX_FRONT_LEFT]] = 0;
  1103         motor[drive.motorIDs[IDX_FRONT_RIGHT]] = 0;
  1104         if (DriveHas4Motors(drive))
  1105         {
  1106             motor[drive.motorIDs[IDX_REAR_LEFT]] = 0;
  1107             motor[drive.motorIDs[IDX_REAR_RIGHT]] = 0;
  1108         }
  1109     }
  1110 
  1111     TExit();
  1112     return;
  1113 }   //DriveTask
  1114 
  1115 #endif  //ifndef _DRIVE_H