Mechatronics Robotics / Mbed 2 deprecated BrobotV1

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

Dependencies:   mbed UniServ

SpeedControl.cpp@17:ec52258b9472, 2017-06-07 (annotated)

Committer:
obrie829
Date:
Wed Jun 07 11:35:59 2017 +0000
Revision:
17:ec52258b9472
Parent:
15:4efc66de795a 
v18

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 17:ec52258b9472 38 t2.attach( callback(this, &SpeedControl::speedCtrl), PERIOD); // SpeedControl:: allows you to access funtion within the class file
obrie829 8:351b0b7b05b2 39 }
obrie829 8:351b0b7b05b2 40
obrie829 8:351b0b7b05b2 41 //Destructor
obrie829 8:351b0b7b05b2 42 SpeedControl::~SpeedControl()
obrie829 8:351b0b7b05b2 43 {
obrie829 8:351b0b7b05b2 44 }
obrie829 8:351b0b7b05b2 45
obrie829 8:351b0b7b05b2 46 void SpeedControl::speedCtrl() //EncoderCounter* counterLeft, EncoderCounter* counterRight
obrie829 8:351b0b7b05b2 47 {
obrie829 15:4efc66de795a 48 //Calculate Effective speeds of the motor [rpm]
obrie829 8:351b0b7b05b2 49 short valueCounterLeft = counterLeft->read();
obrie829 8:351b0b7b05b2 50 short valueCounterRight = counterRight->read();
obrie829 8:351b0b7b05b2 51 short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
obrie829 8:351b0b7b05b2 52 short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
obrie829 17:ec52258b9472 53
obrie829 8:351b0b7b05b2 54 previousValueCounterLeft = valueCounterLeft;
obrie829 8:351b0b7b05b2 55 previousValueCounterRight = valueCounterRight;
obrie829 8:351b0b7b05b2 56 actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft /COUNTS_PER_TURN/PERIOD*60.0f);
obrie829 8:351b0b7b05b2 57 actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight /COUNTS_PER_TURN/PERIOD*60.0f);
obrie829 8:351b0b7b05b2 58
obrie829 17:ec52258b9472 59 // Berechnen der Motorspannungen Uout
obrie829 17:ec52258b9472 60 float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
obrie829 17:ec52258b9472 61 float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN;
obrie829 8:351b0b7b05b2 62
obrie829 8:351b0b7b05b2 63 // Berechnen, Limitieren und Setzen der Duty-Cycle
obrie829 17:ec52258b9472 64 float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
obrie829 8:351b0b7b05b2 65 if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
obrie829 8:351b0b7b05b2 66 else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
obrie829 8:351b0b7b05b2 67
obrie829 8:351b0b7b05b2 68 *pwmLeft = dutyCycleLeft;
obrie829 8:351b0b7b05b2 69 float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
obrie829 8:351b0b7b05b2 70 if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
obrie829 8:351b0b7b05b2 71 else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
obrie829 8:351b0b7b05b2 72
obrie829 8:351b0b7b05b2 73 *pwmRight = dutyCycleRight;
obrie829 15:4efc66de795a 74
obrie829 15:4efc66de795a 75 // printf("the pwm L and R speeds should be %f and %f \n\r", dutyCycleLeft, dutyCycleRight);
obrie829 15:4efc66de795a 76
obrie829 8:351b0b7b05b2 77 }
obrie829 8:351b0b7b05b2 78
obrie829 15:4efc66de795a 79 //set desired wheel speed in RPM
obrie829 8:351b0b7b05b2 80 void SpeedControl::setDesiredSpeed( float L, float R)
obrie829 8:351b0b7b05b2 81 {
obrie829 17:ec52258b9472 82 desiredSpeedLeft = L * 1.18f ;
obrie829 8:351b0b7b05b2 83 desiredSpeedRight = R;
obrie829 8:351b0b7b05b2 84 }
obrie829 8:351b0b7b05b2 85