Owen Roseborough / Mbed 2 deprecated BeverageRoverV1

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

motorControl.cpp@0:17d76ace37b6, 2015-03-06 (annotated)

Committer:
embedMasterForever
Date:
Fri Mar 06 17:20:00 2015 +0000
Revision:
0:17d76ace37b6
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.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
embedMasterForever 0:17d76ace37b6 1 // motorControl.cpp -----------------------------------------
embedMasterForever 0:17d76ace37b6 2 #include "mbed.h"
embedMasterForever 0:17d76ace37b6 3 #include "motorControl.h"
embedMasterForever 0:17d76ace37b6 4
embedMasterForever 0:17d76ace37b6 5 PwmOut leftSideMotor(p21);
embedMasterForever 0:17d76ace37b6 6 PwmOut rightSideMotor(p22);
embedMasterForever 0:17d76ace37b6 7 DigitalOut led1(LED1);
embedMasterForever 0:17d76ace37b6 8 DigitalOut led2(LED2);
embedMasterForever 0:17d76ace37b6 9
embedMasterForever 0:17d76ace37b6 10 float startingSpeed = 0.45;
embedMasterForever 0:17d76ace37b6 11 float topSpeed = 0.20;
embedMasterForever 0:17d76ace37b6 12 int acceleration = 0;
embedMasterForever 0:17d76ace37b6 13
embedMasterForever 0:17d76ace37b6 14 int motorCounts = 0;
embedMasterForever 0:17d76ace37b6 15 int assigner = 0;
embedMasterForever 0:17d76ace37b6 16 float averageOfMotorCounts = 0;
embedMasterForever 0:17d76ace37b6 17 float averageOfMotorCountsForOneStep[3];
embedMasterForever 0:17d76ace37b6 18
embedMasterForever 0:17d76ace37b6 19 void stopMotors (void)
embedMasterForever 0:17d76ace37b6 20 {
embedMasterForever 0:17d76ace37b6 21 leftSideMotor = STOPSPEED;
embedMasterForever 0:17d76ace37b6 22 rightSideMotor = STOPSPEED;
embedMasterForever 0:17d76ace37b6 23 }
embedMasterForever 0:17d76ace37b6 24
embedMasterForever 0:17d76ace37b6 25 int warmupStart(void)
embedMasterForever 0:17d76ace37b6 26 {
embedMasterForever 0:17d76ace37b6 27 leftSideMotor = WARMUPSPEED;
embedMasterForever 0:17d76ace37b6 28 rightSideMotor = WARMUPSPEED;
embedMasterForever 0:17d76ace37b6 29 return 1;
embedMasterForever 0:17d76ace37b6 30 }
embedMasterForever 0:17d76ace37b6 31
embedMasterForever 0:17d76ace37b6 32 void recordMotorCounts(int *ptrMotorCounts){
embedMasterForever 0:17d76ace37b6 33 for(int a = 0; a < 4; a++) motorCounts = motorCounts + *(ptrMotorCounts + a);
embedMasterForever 0:17d76ace37b6 34 averageOfMotorCounts = (motorCounts/4);
embedMasterForever 0:17d76ace37b6 35 assignAverage(averageOfMotorCounts);
embedMasterForever 0:17d76ace37b6 36 }
embedMasterForever 0:17d76ace37b6 37
embedMasterForever 0:17d76ace37b6 38 void assignAverage(float average){
embedMasterForever 0:17d76ace37b6 39 averageOfMotorCountsForOneStep[assigner] = average;
embedMasterForever 0:17d76ace37b6 40 assigner++;
embedMasterForever 0:17d76ace37b6 41 }
embedMasterForever 0:17d76ace37b6 42
embedMasterForever 0:17d76ace37b6 43 void exponentialAccAndDecCurves(void){
embedMasterForever 0:17d76ace37b6 44
embedMasterForever 0:17d76ace37b6 45 float sumOfCounts = 0;
embedMasterForever 0:17d76ace37b6 46 float averageCounts = 0;
embedMasterForever 0:17d76ace37b6 47 float percentOf1Rev = 0;
embedMasterForever 0:17d76ace37b6 48 float ftMovedIn1Step = 0;
embedMasterForever 0:17d76ace37b6 49 float speedDifference = 0;
embedMasterForever 0:17d76ace37b6 50 float stepMult = SIZEOFINCREMENT;
embedMasterForever 0:17d76ace37b6 51 float totalStepMult = SIZEOFINCREMENT;
embedMasterForever 0:17d76ace37b6 52 float totalOfAllStepMult = 0;
embedMasterForever 0:17d76ace37b6 53 float firstExpStep = 0;
embedMasterForever 0:17d76ace37b6 54 float multipliers[NUMOFEXPINCREMENTS];
embedMasterForever 0:17d76ace37b6 55 multipliers[0] = 1;
embedMasterForever 0:17d76ace37b6 56 float expSpeedIncrements[NUMOFEXPINCREMENTS];
embedMasterForever 0:17d76ace37b6 57 float expPWMIncrements[NUMOFEXPINCREMENTS];
embedMasterForever 0:17d76ace37b6 58 float expCompleteWave[NUMOFEXPINCCOMPLETEWAVE];
embedMasterForever 0:17d76ace37b6 59 float j = WARMUPSPEED;
embedMasterForever 0:17d76ace37b6 60
embedMasterForever 0:17d76ace37b6 61 averageOfMotorCountsForOneStep[2] = (averageOfMotorCountsForOneStep[2] - averageOfMotorCountsForOneStep[1]);
embedMasterForever 0:17d76ace37b6 62 averageOfMotorCountsForOneStep[1] = (averageOfMotorCountsForOneStep[1] - averageOfMotorCountsForOneStep[0]);
embedMasterForever 0:17d76ace37b6 63 for(int b = 0; b < 3; b++) sumOfCounts = sumOfCounts + averageOfMotorCountsForOneStep[b];
embedMasterForever 0:17d76ace37b6 64 averageCounts = (sumOfCounts/4);
embedMasterForever 0:17d76ace37b6 65 percentOf1Rev = (averageCounts/COUNTSOF1REV1CH);
embedMasterForever 0:17d76ace37b6 66 ftMovedIn1Step = (FTOF1REV * percentOf1Rev); //ftMovedIn1Step is the speed in ft/s of one step, a step being one of the three
embedMasterForever 0:17d76ace37b6 67
embedMasterForever 0:17d76ace37b6 68 speedDifference = (MAXSPEED/2);
embedMasterForever 0:17d76ace37b6 69 for(int c = 0; c < NUMOFEXPINCREMENTS; c++){
embedMasterForever 0:17d76ace37b6 70 totalStepMult = SIZEOFINCREMENT;
embedMasterForever 0:17d76ace37b6 71 for(int d = 2; d <= c; d++){
embedMasterForever 0:17d76ace37b6 72 totalStepMult = (totalStepMult * stepMult);
embedMasterForever 0:17d76ace37b6 73 multipliers[c] = totalStepMult;
embedMasterForever 0:17d76ace37b6 74 }
embedMasterForever 0:17d76ace37b6 75 if(c == 0)totalOfAllStepMult++;
embedMasterForever 0:17d76ace37b6 76 else if(c == 1){
embedMasterForever 0:17d76ace37b6 77 multipliers[c] = SIZEOFINCREMENT;
embedMasterForever 0:17d76ace37b6 78 totalOfAllStepMult = totalOfAllStepMult + totalStepMult;
embedMasterForever 0:17d76ace37b6 79 }
embedMasterForever 0:17d76ace37b6 80 else totalOfAllStepMult = totalOfAllStepMult + totalStepMult;
embedMasterForever 0:17d76ace37b6 81 }
embedMasterForever 0:17d76ace37b6 82 firstExpStep = (speedDifference/totalOfAllStepMult);
embedMasterForever 0:17d76ace37b6 83 expSpeedIncrements[0] = firstExpStep;
embedMasterForever 0:17d76ace37b6 84
embedMasterForever 0:17d76ace37b6 85 for(int e = 1; e < NUMOFEXPINCREMENTS; e++) expSpeedIncrements[e] = (firstExpStep * multipliers[e]);
embedMasterForever 0:17d76ace37b6 86
embedMasterForever 0:17d76ace37b6 87 for(int f = 0; f < NUMOFEXPINCREMENTS; f++){
embedMasterForever 0:17d76ace37b6 88 float mult = 0;
embedMasterForever 0:17d76ace37b6 89 mult = (expSpeedIncrements[f]/ftMovedIn1Step);
embedMasterForever 0:17d76ace37b6 90 expPWMIncrements[f] = (mult * INITIALPWMINCREMENT); //this should produce an array filled with 10 exponential PWM increments
embedMasterForever 0:17d76ace37b6 91 }
embedMasterForever 0:17d76ace37b6 92
embedMasterForever 0:17d76ace37b6 93 for(int g = 0; g < NUMOFEXPINCREMENTS; g++) expCompleteWave[g] = expPWMIncrements[g];
embedMasterForever 0:17d76ace37b6 94 int m = 1;
embedMasterForever 0:17d76ace37b6 95 for(int h = 10; h < NUMOFEXPINCCOMPLETEWAVE; h++){
embedMasterForever 0:17d76ace37b6 96 expCompleteWave[h] = expPWMIncrements[(h-m)];
embedMasterForever 0:17d76ace37b6 97 m = m + 2;
embedMasterForever 0:17d76ace37b6 98 }
embedMasterForever 0:17d76ace37b6 99
embedMasterForever 0:17d76ace37b6 100 for(int i = 0; i < NUMOFEXPINCCOMPLETEWAVE; i++){
embedMasterForever 0:17d76ace37b6 101 j = j - expCompleteWave[i];
embedMasterForever 0:17d76ace37b6 102 leftSideMotor = j;
embedMasterForever 0:17d76ace37b6 103 rightSideMotor = j;
embedMasterForever 0:17d76ace37b6 104 led1 = 1;
embedMasterForever 0:17d76ace37b6 105 wait(0.15);
embedMasterForever 0:17d76ace37b6 106 led1 = 0;
embedMasterForever 0:17d76ace37b6 107 wait(0.15);
embedMasterForever 0:17d76ace37b6 108 }
embedMasterForever 0:17d76ace37b6 109 }
embedMasterForever 0:17d76ace37b6 110
embedMasterForever 0:17d76ace37b6 111 void stopMotors1 (void)
embedMasterForever 0:17d76ace37b6 112 {
embedMasterForever 0:17d76ace37b6 113 leftSideMotor = STOPSPEED;
embedMasterForever 0:17d76ace37b6 114 rightSideMotor = STOPSPEED;
embedMasterForever 0:17d76ace37b6 115 wait(10);
embedMasterForever 0:17d76ace37b6 116 float a;
embedMasterForever 0:17d76ace37b6 117
embedMasterForever 0:17d76ace37b6 118
embedMasterForever 0:17d76ace37b6 119 for(a = 0.6; a >= 0.31;){
embedMasterForever 0:17d76ace37b6 120 leftSideMotor = a;
embedMasterForever 0:17d76ace37b6 121 rightSideMotor = a;
embedMasterForever 0:17d76ace37b6 122 a = a - 0.005;
embedMasterForever 0:17d76ace37b6 123 wait(0.1);
embedMasterForever 0:17d76ace37b6 124 }
embedMasterForever 0:17d76ace37b6 125 leftSideMotor = a;
embedMasterForever 0:17d76ace37b6 126 rightSideMotor = a;
embedMasterForever 0:17d76ace37b6 127 wait(5);
embedMasterForever 0:17d76ace37b6 128 }
embedMasterForever 0:17d76ace37b6 129 void testingFunction1 (void)
embedMasterForever 0:17d76ace37b6 130 {
embedMasterForever 0:17d76ace37b6 131 for(float a = 0.8; a >= 0.20;){
embedMasterForever 0:17d76ace37b6 132 leftSideMotor = a;
embedMasterForever 0:17d76ace37b6 133 rightSideMotor = a;
embedMasterForever 0:17d76ace37b6 134 a = a - 0.001;
embedMasterForever 0:17d76ace37b6 135 wait(0.015);
embedMasterForever 0:17d76ace37b6 136 }
embedMasterForever 0:17d76ace37b6 137 }