motorControl.cpp

00001 // motorControl.cpp -----------------------------------------
00002 #include "mbed.h"
00003 #include "motorControl.h"
00004 
00005 PwmOut leftSideMotor(p21);
00006 PwmOut rightSideMotor(p22);
00007 DigitalOut led1(LED1);
00008 DigitalOut led2(LED2);
00009 
00010 float startingSpeed = 0.45;
00011 float topSpeed = 0.20;
00012 int acceleration = 0;
00013 
00014 int motorCounts = 0;
00015 int assigner = 0;
00016 float averageOfMotorCounts = 0;
00017 float averageOfMotorCountsForOneStep[3];
00018 
00019 void stopMotors (void)
00020 {
00021     leftSideMotor = STOPSPEED;
00022     rightSideMotor = STOPSPEED;
00023 }
00024 
00025 int warmupStart(void)
00026 {
00027     leftSideMotor = WARMUPSPEED;
00028     rightSideMotor = WARMUPSPEED;
00029     return 1;
00030 }
00031 
00032 void recordMotorCounts(int *ptrMotorCounts){
00033     for(int a = 0; a < 4; a++) motorCounts = motorCounts + *(ptrMotorCounts + a);
00034     averageOfMotorCounts = (motorCounts/4);
00035     assignAverage(averageOfMotorCounts);
00036 }
00037 
00038 void assignAverage(float average){
00039     averageOfMotorCountsForOneStep[assigner] = average;
00040     assigner++;
00041 }
00042 
00043 void exponentialAccAndDecCurves(void){
00044     
00045     float sumOfCounts = 0;
00046     float averageCounts = 0;
00047     float percentOf1Rev = 0;
00048     float ftMovedIn1Step = 0;
00049     float speedDifference = 0;
00050     float stepMult = SIZEOFINCREMENT;
00051     float totalStepMult = SIZEOFINCREMENT; 
00052     float totalOfAllStepMult = 0;
00053     float firstExpStep = 0;
00054     float multipliers[NUMOFEXPINCREMENTS];
00055     multipliers[0] = 1;
00056     float expSpeedIncrements[NUMOFEXPINCREMENTS];
00057     float expPWMIncrements[NUMOFEXPINCREMENTS];
00058     float expCompleteWave[NUMOFEXPINCCOMPLETEWAVE];
00059     float j = WARMUPSPEED;
00060     
00061     averageOfMotorCountsForOneStep[2] = (averageOfMotorCountsForOneStep[2] - averageOfMotorCountsForOneStep[1]);
00062     averageOfMotorCountsForOneStep[1] = (averageOfMotorCountsForOneStep[1] - averageOfMotorCountsForOneStep[0]);
00063     for(int b = 0; b < 3; b++) sumOfCounts = sumOfCounts + averageOfMotorCountsForOneStep[b];
00064     averageCounts = (sumOfCounts/4);
00065     percentOf1Rev = (averageCounts/COUNTSOF1REV1CH);
00066     ftMovedIn1Step = (FTOF1REV * percentOf1Rev);  //ftMovedIn1Step is the speed in ft/s of one step, a step being one of the three
00067     
00068     speedDifference = (MAXSPEED/2);
00069     for(int c = 0; c < NUMOFEXPINCREMENTS; c++){ 
00070         totalStepMult = SIZEOFINCREMENT;
00071         for(int d = 2; d <= c; d++){  
00072             totalStepMult = (totalStepMult * stepMult);
00073             multipliers[c] = totalStepMult; 
00074         }
00075         if(c == 0)totalOfAllStepMult++; 
00076         else if(c == 1){
00077             multipliers[c] = SIZEOFINCREMENT;
00078             totalOfAllStepMult = totalOfAllStepMult + totalStepMult;
00079         }
00080         else totalOfAllStepMult = totalOfAllStepMult + totalStepMult;
00081     }
00082     firstExpStep = (speedDifference/totalOfAllStepMult);
00083     expSpeedIncrements[0] = firstExpStep;
00084     
00085     for(int e = 1; e < NUMOFEXPINCREMENTS; e++) expSpeedIncrements[e] = (firstExpStep * multipliers[e]);
00086     
00087     for(int f = 0; f < NUMOFEXPINCREMENTS; f++){
00088         float mult = 0;
00089         mult = (expSpeedIncrements[f]/ftMovedIn1Step);
00090         expPWMIncrements[f] = (mult * INITIALPWMINCREMENT);   //this should produce an array filled with 10 exponential PWM increments
00091     }
00092     
00093     for(int g = 0; g < NUMOFEXPINCREMENTS; g++) expCompleteWave[g] = expPWMIncrements[g];
00094     int m = 1;
00095     for(int h = 10; h < NUMOFEXPINCCOMPLETEWAVE; h++){
00096         expCompleteWave[h] = expPWMIncrements[(h-m)];
00097         m = m + 2;
00098     }
00099     
00100     for(int i = 0; i < NUMOFEXPINCCOMPLETEWAVE; i++){
00101         j = j - expCompleteWave[i];
00102         leftSideMotor = j;
00103         rightSideMotor = j;
00104         led1 = 1;
00105         wait(0.15);
00106         led1 = 0;
00107         wait(0.15);
00108     }
00109 }
00110   
00111 void stopMotors1 (void)
00112 {
00113     leftSideMotor = STOPSPEED;
00114     rightSideMotor = STOPSPEED;
00115     wait(10);
00116     float a;
00117     
00118     
00119     for(a = 0.6; a >= 0.31;){
00120         leftSideMotor = a;
00121         rightSideMotor = a;
00122         a = a - 0.005;
00123         wait(0.1);
00124     }
00125     leftSideMotor = a;
00126     rightSideMotor = a;
00127     wait(5);
00128 }
00129 void testingFunction1 (void)
00130 {
00131         for(float a = 0.8; a >= 0.20;){  
00132             leftSideMotor = a;
00133             rightSideMotor = a;
00134             a = a - 0.001;
00135             wait(0.015);
00136         }  
00137 }