Owen Roseborough / Mbed 2 deprecated BeverageRoverV1

Dependencies:   mbed

motorControl.cpp

Committer:
embedMasterForever
Date:
2015-03-06
Revision:
0:17d76ace37b6

File content as of revision 0:17d76ace37b6:

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