Revision 0:17d76ace37b6, committed 2015-03-06

Comitter:

embedMasterForever

Date:

Fri Mar 06 17:20:00 2015 +0000

Commit message:

This is research and development code for programming exponential acceleration for a Rover 5 chassis by Pololu Electronics, controlled by Sabertooth 2x5 Motor Drivers by Dimension Engineering, and an Mbed LPC1768.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Mar 06 17:20:00 2015 +0000
@@ -0,0 +1,113 @@
+// main.cpp -----------------------------------------
+#include "mbed.h"
+#include "motorControl.h"
+
+DigitalIn Motor1A(p10);
+DigitalIn Motor1B(p25);
+
+DigitalIn Motor2A(p13);
+DigitalIn Motor2B(p23);
+
+DigitalIn Motor3A(p12);
+DigitalIn Motor3B(p24);
+
+DigitalIn Motor4A(p11);
+DigitalIn Motor4B(p26);
+
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+#define MTR1A 0
+#define MTR1B 1
+#define MTR2A 2
+#define MTR2B 3
+#define MTR3A 4
+#define MTR3B 5
+#define MTR4A 6
+#define MTR4B 7
+
+int mtrCounts[4];
+
+int flagMotor1A = 0;
+int flagMotor2A = 0;
+int flagMotor3A = 0;
+int flagMotor4A = 0;
+
+int warmUpNeeded = 0;
+int repeatFlag = 1;
+
+Timer timer1;
+Timer timer2;
+
+int main() 
+{   /*
+    stopMotors();
+    wait(10);
+    */
+    while(1){
+    stopMotors1();
+    }/*
+        if(warmUpNeeded < 4 && repeatFlag == 1){
+            warmUpNeeded = warmUpNeeded + warmupStart();
+            timer1.start();
+            repeatFlag = 0;
+        }
+        if(0.95 < timer1.read() && timer1.read() < 1.05){
+            stopMotors();
+            wait(1);
+            timer1.reset();
+            timer1.start();
+            recordMotorCounts(&mtrCounts[0]);
+            repeatFlag = 1;
+        }
+        if(warmUpNeeded == 4 && repeatFlag == 1){
+            exponentialAccAndDecCurves(); 
+            warmUpNeeded = 10000;
+        }
+    
+        if(Motor1A == 0 && flagMotor1A == 1){
+            mtrCounts[0]++;
+            flagMotor1A = 0;
+        }
+        if(Motor1A == 1 && flagMotor1A == 0){ 
+            mtrCounts[0]++;
+            flagMotor1A = 1;
+        }
+    
+        if(Motor2A == 0 && flagMotor2A == 1){
+            mtrCounts[1]++;
+            flagMotor2A = 0;
+        }
+        if(Motor2A == 1 && flagMotor2A == 0){ 
+            mtrCounts[1]++;
+            flagMotor2A = 1;
+        }
+    
+        if(Motor3A == 0 && flagMotor3A == 1){
+            mtrCounts[2]++;
+            flagMotor3A = 0;
+        }
+        if(Motor3A == 1 && flagMotor3A == 0){ 
+            mtrCounts[2]++;
+            flagMotor3A = 1;
+        }
+    
+        if(Motor4A == 0 && flagMotor4A == 1){
+            mtrCounts[3]++;
+            flagMotor4A = 0;
+        }
+        if(Motor4A == 1 && flagMotor4A == 0){ 
+            mtrCounts[3]++;
+            flagMotor4A = 1;
+        }
+    } */
+}
+        
+
+
+
+
+
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Mar 06 17:20:00 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/031413cf7a89
\ No newline at end of file

--- /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);
+        }  
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motorControl.h	Fri Mar 06 17:20:00 2015 +0000
@@ -0,0 +1,28 @@
+// motorControl.h -----------------------------------------
+
+#define STOPSPEED 0.85
+#define WARMUPSPEED 0.70
+#define MAX_VOLTAGE 1.2
+#define COUNTSOF1REV1CH  166.67
+#define FTOF1REV    0.64444
+#define MAXSPEED    1.3 
+#define INITIALPWMINCREMENT 0.15
+#define NUMOFEXPINCREMENTS  10
+#define NUMOFEXPINCCOMPLETEWAVE  20
+#define SIZEOFINCREMENT 1.1
+#define TIMEINCREMENTS 0.5
+
+extern PwmOut leftSideMotor;
+extern PwmOut rightSideMotor;
+
+void stopMotors (void);
+int warmupStart(void);
+void recordMotorCounts(int *ptrMotorCounts);
+void assignAverage(float average);
+void exponentialAccAndDecCurves(void);
+void stopMotors1 (void);
+void accelerateMotors (void);
+void decelerateMotors (void);
+void motorsFullSpeedForeward (void);
+void testingFunction(void);
+void testingFunction1(void);
\ No newline at end of file