Mechatronics Robotics / Mbed 2 deprecated BrobotV1

These are the core files for the Robot at Team conception.

Dependencies:   mbed UniServ

SpeedControl.cpp@15:4efc66de795a, 2017-06-05 (annotated)

Committer:
obrie829
Date:
Mon Jun 05 07:56:08 2017 +0000
Revision:
15:4efc66de795a
Parent:
8:351b0b7b05b2
Child:
17:ec52258b9472
blah

Who changed what in which revision?

UserRevisionLine numberNew contents of line
obrie829 8:351b0b7b05b2 1
obrie829 8:351b0b7b05b2 2 #include "mbed.h"
obrie829 8:351b0b7b05b2 3 #include "SpeedControl.h"
obrie829 8:351b0b7b05b2 4 #include "EncoderCounter.h"
obrie829 8:351b0b7b05b2 5 #include "LowpassFilter.h"
obrie829 8:351b0b7b05b2 6
obrie829 15:4efc66de795a 7 long integralErrorRight=0;
obrie829 15:4efc66de795a 8 long integralErrorLeft=0;
obrie829 15:4efc66de795a 9 short integralErrorMax=1000;
obrie829 15:4efc66de795a 10
obrie829 15:4efc66de795a 11
obrie829 15:4efc66de795a 12 //Constructor
obrie829 8:351b0b7b05b2 13 //directly from mainSpeedControl int main()
obrie829 8:351b0b7b05b2 14 SpeedControl::SpeedControl(PwmOut* pwmLeft, PwmOut* pwmRight, EncoderCounter* counterLeft, EncoderCounter* counterRight)
obrie829 8:351b0b7b05b2 15 {
obrie829 8:351b0b7b05b2 16 this->pwmLeft = pwmLeft;
obrie829 8:351b0b7b05b2 17 this->pwmRight = pwmRight;
obrie829 8:351b0b7b05b2 18
obrie829 15:4efc66de795a 19 // Initialisieren der PWM Ausgaenge pwmLeft.period(0.00005f); // PWM Periode von 50 us
obrie829 8:351b0b7b05b2 20 this->pwmLeft->period(0.00005f); // Setzt die Periode auf 50 μs
obrie829 8:351b0b7b05b2 21 this->pwmRight->period(0.00005f);
obrie829 8:351b0b7b05b2 22 this->pwmLeft->write( 0.5f); // Duty-Cycle von 50% pwmRight.period(0.00005f); // PWM Periode von 50 us
obrie829 8:351b0b7b05b2 23 this->pwmRight->write( 0.5f); // Duty-Cycle von 50%
obrie829 8:351b0b7b05b2 24
obrie829 8:351b0b7b05b2 25 // Initialisieren von lokalen Variabeln
obrie829 8:351b0b7b05b2 26 previousValueCounterLeft = counterLeft->read();
obrie829 8:351b0b7b05b2 27 previousValueCounterRight = counterRight->read();
obrie829 8:351b0b7b05b2 28 speedLeftFilter.setPeriod(PERIOD);
obrie829 8:351b0b7b05b2 29 speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
obrie829 8:351b0b7b05b2 30 speedRightFilter.setPeriod(PERIOD);
obrie829 8:351b0b7b05b2 31 speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
obrie829 8:351b0b7b05b2 32
obrie829 8:351b0b7b05b2 33 desiredSpeedLeft = 0.0f;
obrie829 8:351b0b7b05b2 34 desiredSpeedRight = 0.0f;
obrie829 8:351b0b7b05b2 35 actualSpeedLeft = 0.0f;
obrie829 8:351b0b7b05b2 36 actualSpeedRight = 0.0f;
obrie829 8:351b0b7b05b2 37
obrie829 15:4efc66de795a 38 t2.attach( callback(this, &SpeedControl::speedCtrl), 0.05f); // SpeedControl:: allows you to access funtion within the class file
obrie829 8:351b0b7b05b2 39
obrie829 8:351b0b7b05b2 40 }
obrie829 8:351b0b7b05b2 41
obrie829 8:351b0b7b05b2 42 //Destructor
obrie829 8:351b0b7b05b2 43 SpeedControl::~SpeedControl()
obrie829 8:351b0b7b05b2 44 {
obrie829 8:351b0b7b05b2 45 }
obrie829 8:351b0b7b05b2 46
obrie829 8:351b0b7b05b2 47 void SpeedControl::speedCtrl() //EncoderCounter* counterLeft, EncoderCounter* counterRight
obrie829 8:351b0b7b05b2 48 {
obrie829 15:4efc66de795a 49 //Calculate Effective speeds of the motor [rpm]
obrie829 8:351b0b7b05b2 50 short valueCounterLeft = counterLeft->read();
obrie829 8:351b0b7b05b2 51 short valueCounterRight = counterRight->read();
obrie829 8:351b0b7b05b2 52 short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
obrie829 8:351b0b7b05b2 53 short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
obrie829 8:351b0b7b05b2 54
obrie829 8:351b0b7b05b2 55 previousValueCounterLeft = valueCounterLeft;
obrie829 8:351b0b7b05b2 56 previousValueCounterRight = valueCounterRight;
obrie829 8:351b0b7b05b2 57 actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft /COUNTS_PER_TURN/PERIOD*60.0f);
obrie829 8:351b0b7b05b2 58 actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight /COUNTS_PER_TURN/PERIOD*60.0f);
obrie829 8:351b0b7b05b2 59
obrie829 15:4efc66de795a 60
obrie829 8:351b0b7b05b2 61 //Error of each wheel from Pauls I code
obrie829 15:4efc66de795a 62
obrie829 8:351b0b7b05b2 63 short eL = desiredSpeedLeft-actualSpeedLeft;
obrie829 8:351b0b7b05b2 64 short eR = desiredSpeedRight-actualSpeedRight;
obrie829 8:351b0b7b05b2 65
obrie829 15:4efc66de795a 66 /*
obrie829 8:351b0b7b05b2 67 // Increment the error integral for each wheel
obrie829 8:351b0b7b05b2 68 integralErrorRight += eR;
obrie829 8:351b0b7b05b2 69 integralErrorLeft += eL;
obrie829 8:351b0b7b05b2 70 // Limit the integral term
obrie829 8:351b0b7b05b2 71 if (integralErrorRight > integralErrorMax)
obrie829 8:351b0b7b05b2 72 integralErrorRight = integralErrorMax;
obrie829 8:351b0b7b05b2 73 if (integralErrorRight < -integralErrorMax)
obrie829 8:351b0b7b05b2 74 integralErrorRight = -integralErrorMax;
obrie829 8:351b0b7b05b2 75 if (integralErrorLeft > integralErrorMax)
obrie829 8:351b0b7b05b2 76 integralErrorLeft = integralErrorMax;
obrie829 8:351b0b7b05b2 77 if (integralErrorLeft < -integralErrorMax)
obrie829 8:351b0b7b05b2 78 integralErrorLeft = -integralErrorMax;
obrie829 8:351b0b7b05b2 79 //end of Paul's I control
obrie829 15:4efc66de795a 80 */
obrie829 15:4efc66de795a 81 // + KI*integralErrorLeft and KI*integralErrorRight + were removed from below
obrie829 8:351b0b7b05b2 82 // P + I Control!
obrie829 15:4efc66de795a 83 float voltageLeft = KP*eL + desiredSpeedLeft/KN;
obrie829 15:4efc66de795a 84 float voltageRight = KP*eR + desiredSpeedRight/KN;
obrie829 15:4efc66de795a 85
obrie829 15:4efc66de795a 86 //printf("the voltage on the L and R sides are %f and %f \n\r", voltageLeft, voltageRight);
obrie829 15:4efc66de795a 87
obrie829 8:351b0b7b05b2 88 // Berechnen, Limitieren und Setzen der Duty-Cycle
obrie829 15:4efc66de795a 89 float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE; // Max_voltage is 12.0f
obrie829 8:351b0b7b05b2 90 if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
obrie829 8:351b0b7b05b2 91 else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
obrie829 8:351b0b7b05b2 92
obrie829 8:351b0b7b05b2 93 *pwmLeft = dutyCycleLeft;
obrie829 8:351b0b7b05b2 94
obrie829 8:351b0b7b05b2 95 float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
obrie829 8:351b0b7b05b2 96 if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
obrie829 8:351b0b7b05b2 97 else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
obrie829 8:351b0b7b05b2 98
obrie829 8:351b0b7b05b2 99 *pwmRight = dutyCycleRight;
obrie829 15:4efc66de795a 100
obrie829 15:4efc66de795a 101 // printf("the pwm L and R speeds should be %f and %f \n\r", dutyCycleLeft, dutyCycleRight);
obrie829 15:4efc66de795a 102
obrie829 8:351b0b7b05b2 103 }
obrie829 8:351b0b7b05b2 104
obrie829 15:4efc66de795a 105 //set desired wheel speed in RPM
obrie829 8:351b0b7b05b2 106 void SpeedControl::setDesiredSpeed( float L, float R)
obrie829 8:351b0b7b05b2 107 {
obrie829 15:4efc66de795a 108 desiredSpeedLeft = L * 1.1f ;
obrie829 8:351b0b7b05b2 109 desiredSpeedRight = R;
obrie829 8:351b0b7b05b2 110 }
obrie829 8:351b0b7b05b2 111