Diff: motorControl.cpp

Revision:

0:17d76ace37b6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motorControl.cpp	Fri Mar 06 17:20:00 2015 +0000
@@ -0,0 +1,137 @@
+// motorControl.cpp -----------------------------------------
+#include "mbed.h"
+#include "motorControl.h"
+
+PwmOut leftSideMotor(p21);
+PwmOut rightSideMotor(p22);
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+
+float startingSpeed = 0.45;
+float topSpeed = 0.20;
+int acceleration = 0;
+
+int motorCounts = 0;
+int assigner = 0;
+float averageOfMotorCounts = 0;
+float averageOfMotorCountsForOneStep[3];
+
+void stopMotors (void)
+{
+    leftSideMotor = STOPSPEED;
+    rightSideMotor = STOPSPEED;
+}
+
+int warmupStart(void)
+{
+    leftSideMotor = WARMUPSPEED;
+    rightSideMotor = WARMUPSPEED;
+    return 1;
+}
+
+void recordMotorCounts(int *ptrMotorCounts){
+    for(int a = 0; a < 4; a++) motorCounts = motorCounts + *(ptrMotorCounts + a);
+    averageOfMotorCounts = (motorCounts/4);
+    assignAverage(averageOfMotorCounts);
+}
+
+void assignAverage(float average){
+    averageOfMotorCountsForOneStep[assigner] = average;
+    assigner++;
+}
+
+void exponentialAccAndDecCurves(void){
+    
+    float sumOfCounts = 0;
+    float averageCounts = 0;
+    float percentOf1Rev = 0;
+    float ftMovedIn1Step = 0;
+    float speedDifference = 0;
+    float stepMult = SIZEOFINCREMENT;
+    float totalStepMult = SIZEOFINCREMENT; 
+    float totalOfAllStepMult = 0;
+    float firstExpStep = 0;
+    float multipliers[NUMOFEXPINCREMENTS];
+    multipliers[0] = 1;
+    float expSpeedIncrements[NUMOFEXPINCREMENTS];
+    float expPWMIncrements[NUMOFEXPINCREMENTS];
+    float expCompleteWave[NUMOFEXPINCCOMPLETEWAVE];
+    float j = WARMUPSPEED;
+    
+    averageOfMotorCountsForOneStep[2] = (averageOfMotorCountsForOneStep[2] - averageOfMotorCountsForOneStep[1]);
+    averageOfMotorCountsForOneStep[1] = (averageOfMotorCountsForOneStep[1] - averageOfMotorCountsForOneStep[0]);
+    for(int b = 0; b < 3; b++) sumOfCounts = sumOfCounts + averageOfMotorCountsForOneStep[b];
+    averageCounts = (sumOfCounts/4);
+    percentOf1Rev = (averageCounts/COUNTSOF1REV1CH);
+    ftMovedIn1Step = (FTOF1REV * percentOf1Rev);  //ftMovedIn1Step is the speed in ft/s of one step, a step being one of the three
+    
+    speedDifference = (MAXSPEED/2);
+    for(int c = 0; c < NUMOFEXPINCREMENTS; c++){ 
+        totalStepMult = SIZEOFINCREMENT;
+        for(int d = 2; d <= c; d++){  
+            totalStepMult = (totalStepMult * stepMult);
+            multipliers[c] = totalStepMult; 
+        }
+        if(c == 0)totalOfAllStepMult++; 
+        else if(c == 1){
+            multipliers[c] = SIZEOFINCREMENT;
+            totalOfAllStepMult = totalOfAllStepMult + totalStepMult;
+        }
+        else totalOfAllStepMult = totalOfAllStepMult + totalStepMult;
+    }
+    firstExpStep = (speedDifference/totalOfAllStepMult);
+    expSpeedIncrements[0] = firstExpStep;
+    
+    for(int e = 1; e < NUMOFEXPINCREMENTS; e++) expSpeedIncrements[e] = (firstExpStep * multipliers[e]);
+    
+    for(int f = 0; f < NUMOFEXPINCREMENTS; f++){
+        float mult = 0;
+        mult = (expSpeedIncrements[f]/ftMovedIn1Step);
+        expPWMIncrements[f] = (mult * INITIALPWMINCREMENT);   //this should produce an array filled with 10 exponential PWM increments
+    }
+    
+    for(int g = 0; g < NUMOFEXPINCREMENTS; g++) expCompleteWave[g] = expPWMIncrements[g];
+    int m = 1;
+    for(int h = 10; h < NUMOFEXPINCCOMPLETEWAVE; h++){
+        expCompleteWave[h] = expPWMIncrements[(h-m)];
+        m = m + 2;
+    }
+    
+    for(int i = 0; i < NUMOFEXPINCCOMPLETEWAVE; i++){
+        j = j - expCompleteWave[i];
+        leftSideMotor = j;
+        rightSideMotor = j;
+        led1 = 1;
+        wait(0.15);
+        led1 = 0;
+        wait(0.15);
+    }
+}
+  
+void stopMotors1 (void)
+{
+    leftSideMotor = STOPSPEED;
+    rightSideMotor = STOPSPEED;
+    wait(10);
+    float a;
+    
+    
+    for(a = 0.6; a >= 0.31;){
+        leftSideMotor = a;
+        rightSideMotor = a;
+        a = a - 0.005;
+        wait(0.1);
+    }
+    leftSideMotor = a;
+    rightSideMotor = a;
+    wait(5);
+}
+void testingFunction1 (void)
+{
+        for(float a = 0.8; a >= 0.20;){  
+            leftSideMotor = a;
+            rightSideMotor = a;
+            a = a - 0.001;
+            wait(0.015);
+        }  
+}