TRC Library for FTC (2011) 2011
TRC Library Code Project

C:/Users/Michael/Ftc/2011/code/trclib/drive.h

Go to the documentation of this file.
00001 #if 0
00002 
00003 
00004 
00005 
00006 
00007 
00008 
00009 
00010 
00011 
00012 
00013 #endif
00014 
00015 #ifndef _DRIVE_H
00016 #define _DRIVE_H
00017 
00018 #pragma systemFile
00019 
00020 #ifdef MOD_ID
00021     #undef MOD_ID
00022 #endif
00023 #define MOD_ID                  MOD_DRIVE
00024 
00025 //
00026 // Constants.
00027 //
00028 #define DRIVEF_ON               0x0100
00029 #define DRIVEF_STALL_PROTECT_ON 0x0200
00030 #define DRIVEF_STALLED          0x0400
00031 
00032 #define MOTOR_MIN_VALUE         -100
00033 #define MOTOR_MAX_VALUE         100
00034 
00035 #define MIN_STALL_POWER         20
00036 #define STALL_TIME              2000    //2 seconds
00037 
00038 //
00039 // Macros.
00040 //
00041 #define NORMALIZE_DRIVE(x,m,n)  NORMALIZE(x, m, n, MOTOR_MIN_VALUE, MOTOR_MAX_VALUE)
00042 
00043 //
00044 // Type definitions.
00045 //
00046 typedef struct
00047 {
00048     int   leftMotor;
00049     int   rightMotor;
00050     float distPerClick;
00051     float degreesPerClick;
00052     int   driveFlags;
00053     int   leftPower;
00054     int   rightPower;
00055     int   leftEncoder;
00056     int   rightEncoder;
00057     long  stallTimer;
00058 } DRIVE;
00059 
00065 void
00066 DriveStop(
00067     __out DRIVE &drive
00068     )
00069 {
00070     TFuncName("DriveStop");
00071     TLevel(API);
00072     TEnter();
00073 
00074     drive.driveFlags &= ~DRIVEF_ON;
00075     drive.leftPower = 0;
00076     drive.rightPower = 0;
00077     //
00078     // Stop the motors.
00079     //
00080     motor[drive.leftMotor] = 0;
00081     motor[drive.rightMotor] = 0;
00082     drive.leftEncoder = 0;
00083     drive.rightEncoder = 0;
00084     drive.stallTimer = 0;
00085 
00086     TExit();
00087     return;
00088 }   //DriveStop
00089 
00095 void
00096 DriveReset(
00097     __out DRIVE &drive
00098     )
00099 {
00100     TFuncName("DriveReset");
00101     TLevel(API);
00102     TEnter();
00103 
00104     DriveStop(drive);
00105 //    nMotorEncoder[drive.leftMotor] = 0;
00106 //    nMotorEncoder[drive.rightMotor] = 0;
00107     drive.driveFlags &= ~DRIVEF_STALLED;
00108 
00109     TExit();
00110     return;
00111 }   //DriveReset
00112 
00120 void
00121 DriveInit(
00122     __out DRIVE &drive,
00123     __in  int leftMotor,
00124     __in  int rightMotor,
00125     __in  float distPerClick,
00126     __in  float degreesPerClick
00127     )
00128 {
00129     TFuncName("DriveInit");
00130     TLevel(INIT);
00131     TEnter();
00132 
00133     drive.leftMotor = leftMotor;
00134     drive.rightMotor = rightMotor;
00135     drive.distPerClick = distPerClick;
00136     drive.degreesPerClick = degreesPerClick;
00137     drive.driveFlags = 0;
00138     DriveReset(drive);
00139 
00140     TExit();
00141     return;
00142 }   //DriveInit
00143 
00150 void
00151 DriveStallProtect(
00152     __inout DRIVE &drive,
00153     __in    bool fOn
00154     )
00155 {
00156     TFuncName("DriveStallProtect");
00157     TLevel(API);
00158     TEnterMsg(("fOn=%d", (byte)fOn));
00159 
00160     if (fOn)
00161     {
00162         drive.driveFlags |= DRIVEF_STALL_PROTECT_ON;
00163     }
00164     else
00165     {
00166         drive.driveFlags &= ~DRIVEF_STALL_PROTECT_ON;
00167     }
00168 
00169     TExit();
00170     return;
00171 }   //DriveStallProtect
00172 
00180 void
00181 DriveTank(
00182     __out DRIVE &drive,
00183     __in  int leftPower,
00184     __in  int rightPower
00185     )
00186 {
00187     TFuncName("DriveTank");
00188     TLevel(API);
00189     TEnterMsg(("Left=%d,Right=%d", leftPower, rightPower));
00190 
00191     drive.leftPower = BOUND(leftPower, MOTOR_MIN_VALUE, MOTOR_MAX_VALUE);
00192     drive.rightPower = BOUND(rightPower, MOTOR_MIN_VALUE, MOTOR_MAX_VALUE);
00193     drive.driveFlags |= DRIVEF_ON;
00194 
00195     TExit();
00196     return;
00197 }   //DriveTank
00198 
00206 void
00207 DriveArcade(
00208     __out DRIVE &drive,
00209     __in  int drivePower,
00210     __in  int turnPower
00211     )
00212 {
00213     TFuncName("DriveArcade");
00214     TLevel(API);
00215     TEnterMsg(("Drive=%d,Turn=%d", drivePower, turnPower));
00216 
00217     drivePower = BOUND(drivePower, MOTOR_MIN_VALUE, MOTOR_MAX_VALUE);
00218     turnPower = BOUND(turnPower, MOTOR_MIN_VALUE, MOTOR_MAX_VALUE);
00219     if (drivePower + turnPower > MOTOR_MAX_VALUE)
00220     {
00221         //
00222         // Forward right:
00223         //  left = drive + turn - (drive + turn - MOTOR_MAX_VALUE)
00224         //  right = drive - turn - (drive + turn - MOTOR_MAX_VALUE)
00225         //
00226         drive.leftPower = MOTOR_MAX_VALUE;
00227         drive.rightPower = -2*turnPower + MOTOR_MAX_VALUE;
00228     }
00229     else if (drivePower - turnPower > MOTOR_MAX_VALUE)
00230     {
00231         //
00232         // Forward left:
00233         //  left = drive + turn - (drive - turn - MOTOR_MAX_VALUE)
00234         //  right = drive - turn - (drive - turn - MOTOR_MAX_VALUE)
00235         //
00236         drive.leftPower = 2*turnPower + MOTOR_MAX_VALUE;
00237         drive.rightPower = MOTOR_MAX_VALUE;
00238     }
00239     else if (drivePower + turnPower < MOTOR_MIN_VALUE)
00240     {
00241         //
00242         // Backward left:
00243         //  left = drive + turn - (drive + turn - MOTOR_MIN_VALUE)
00244         //  right = drive - turn - (drive + turn - MOTOR_MIN_VALUE)
00245         //
00246         drive.leftPower = MOTOR_MIN_VALUE;
00247         drive.rightPower = -2*turnPower + MOTOR_MIN_VALUE;
00248     }
00249     else if (drivePower - turnPower < MOTOR_MIN_VALUE)
00250     {
00251         //
00252         // Backward right:
00253         //  left = drive + turn - (drive - turn - MOTOR_MIN_VALUE)
00254         //  right = drive - turn - (drive - turn - MOTOR_MIN_VALUE)
00255         //
00256         drive.leftPower = 2*turnPower + MOTOR_MIN_VALUE;
00257         drive.rightPower = MOTOR_MIN_VALUE;
00258     }
00259     else
00260     {
00261         drive.leftPower = drivePower + turnPower;
00262         drive.rightPower = drivePower - turnPower;
00263     }
00264     drive.driveFlags |= DRIVEF_ON;
00265 
00266     TExit();
00267     return;
00268 }   //DriveArcade
00269 
00275 void
00276 DriveTask(
00277     __inout DRIVE &drive
00278     )
00279 {
00280     TFuncName("DriveTask");
00281     TLevel(TASK);
00282     TEnter();
00283 
00284     if (drive.driveFlags & DRIVEF_ON)
00285     {
00286         if ((drive.driveFlags & DRIVEF_STALLED) == 0)
00287         {
00288             motor[drive.leftMotor] = drive.leftPower;
00289             motor[drive.rightMotor] = drive.rightPower;
00290             if (drive.driveFlags & DRIVEF_STALL_PROTECT_ON)
00291             {
00292                 long currTime = nPgmTime;
00293                 if ((drive.stallTimer == 0) ||
00294                     (abs(drive.leftPower) <= MIN_STALL_POWER) &&
00295                     (abs(drive.rightPower) <= MIN_STALL_POWER) ||
00296                     (nMotorEncoder[drive.leftMotor] != drive.leftEncoder) ||
00297                     (nMotorEncoder[drive.rightMotor] != drive.rightEncoder))
00298                 {
00299                     drive.leftEncoder = nMotorEncoder[drive.leftMotor];
00300                     drive.rightEncoder = nMotorEncoder[drive.rightMotor];
00301                     drive.stallTimer = currTime;
00302                 }
00303 
00304                 if (currTime - drive.stallTimer >= STALL_TIME)
00305                 {
00306                     motor[drive.leftMotor] = 0;
00307                     motor[drive.rightMotor] = 0;
00308                     drive.driveFlags |= DRIVEF_STALLED;
00309                     PlayImmediateTone(1000, 100);
00310                 }
00311             }
00312         }
00313     }
00314     else
00315     {
00316         motor[drive.leftMotor] = 0;
00317         motor[drive.rightMotor] = 0;
00318     }
00319 
00320     TExit();
00321     return;
00322 }   //DriveTask
00323 
00324 #endif  //ifndef _DRIVE_H
 All Data Structures Files Functions Variables Defines