kings / Mbed 2 deprecated Bewegungen_Encoder

Encoder

Dependencies:   mbed

Fork of Bewegungen by kings

main.cpp@1:e454e6f5d81a, 2017-05-10 (annotated)

Committer:
EHess
Date:
Wed May 10 09:15:42 2017 +0000
Revision:
1:e454e6f5d81a
Parent:
0:96f88638114b 
wd

Who changed what in which revision?

UserRevisionLine numberNew contents of line
EHess 0:96f88638114b 1 #include "mbed.h"
EHess 1:e454e6f5d81a 2 #include "MotorEncoder.h"
EHess 1:e454e6f5d81a 3 #include "LowpassFilter.h"
EHess 0:96f88638114b 4
EHess 1:e454e6f5d81a 5 const float PERIOD = 0.002f; // period of control task, given in [s]
EHess 1:e454e6f5d81a 6 const float COUNTS_PER_TURN = 1200.0f; // resolution of encoder counter
EHess 1:e454e6f5d81a 7 const float LOWPASS_FILTER_FREQUENCY = 300.0f; // frequency of lowpass filter for actual speed values, given in [rad/s]
EHess 1:e454e6f5d81a 8 const float KN = 40.0f; // speed constant of motor, given in [rpm/V]
EHess 1:e454e6f5d81a 9 const float KP = 0.2f; // speed controller gain, given in [V/rpm]
EHess 1:e454e6f5d81a 10 const float MAX_VOLTAGE = 12.0f; // supply voltage for power stage in [V]
EHess 1:e454e6f5d81a 11 const float MIN_DUTY_CYCLE = 0.02f; // minimum allowed value for duty cycle (2%)
EHess 1:e454e6f5d81a 12 const float MAX_DUTY_CYCLE = 0.98f; // maximum allowed value for duty cycle (98%)
EHess 0:96f88638114b 13
EHess 1:e454e6f5d81a 14 MotorEncoder counterLeft(PB_6, PB_7);
EHess 1:e454e6f5d81a 15 MotorEncoder counterRight(PA_6, PC_7);
EHess 1:e454e6f5d81a 16
EHess 1:e454e6f5d81a 17 LowpassFilter speedLeftFilter;
EHess 1:e454e6f5d81a 18 LowpassFilter speedRightFilter;
EHess 1:e454e6f5d81a 19
EHess 1:e454e6f5d81a 20 DigitalOut enableMotorDriver(PB_2);
EHess 1:e454e6f5d81a 21 PwmOut pwmLeft(PA_8);
EHess 1:e454e6f5d81a 22 PwmOut pwmRight(PA_9);
EHess 1:e454e6f5d81a 23
EHess 1:e454e6f5d81a 24 DigitalOut my_led(LED1);
EHess 1:e454e6f5d81a 25
EHess 1:e454e6f5d81a 26 short previousValueCounterRight = 0;
EHess 1:e454e6f5d81a 27 short previousValueCounterLeft = 0;
EHess 1:e454e6f5d81a 28
EHess 1:e454e6f5d81a 29 float desiredSpeedLeft;
EHess 1:e454e6f5d81a 30 float desiredSpeedRight;
EHess 0:96f88638114b 31
EHess 1:e454e6f5d81a 32 float actualSpeedLeft;
EHess 1:e454e6f5d81a 33 float actualSpeedRight;
EHess 0:96f88638114b 34
EHess 1:e454e6f5d81a 35 void speedCtrl()
EHess 1:e454e6f5d81a 36 {
EHess 1:e454e6f5d81a 37 // Berechnen die effektiven Drehzahlen der Motoren in [rpm]
EHess 1:e454e6f5d81a 38 short valueCounterLeft = counterLeft.read();
EHess 1:e454e6f5d81a 39 short valueCounterRight = counterRight.read();
EHess 1:e454e6f5d81a 40 short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
EHess 1:e454e6f5d81a 41 short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
EHess 1:e454e6f5d81a 42
EHess 1:e454e6f5d81a 43 previousValueCounterLeft = valueCounterLeft;
EHess 1:e454e6f5d81a 44 previousValueCounterRight = valueCounterRight;
EHess 1:e454e6f5d81a 45 actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft /COUNTS_PER_TURN/PERIOD*60.0f);
EHess 1:e454e6f5d81a 46 actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight /COUNTS_PER_TURN/PERIOD*60.0f);
EHess 0:96f88638114b 47
EHess 1:e454e6f5d81a 48 // Berechnen der Motorspannungen Uout
EHess 1:e454e6f5d81a 49 float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
EHess 1:e454e6f5d81a 50 float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN;
EHess 0:96f88638114b 51
EHess 1:e454e6f5d81a 52 // Berechnen, Limitieren und Setzen der Duty-Cycle
EHess 1:e454e6f5d81a 53 float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
EHess 1:e454e6f5d81a 54 if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
EHess 1:e454e6f5d81a 55 else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
EHess 1:e454e6f5d81a 56
EHess 1:e454e6f5d81a 57 pwmLeft = dutyCycleLeft;
EHess 1:e454e6f5d81a 58 float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
EHess 1:e454e6f5d81a 59 if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
EHess 1:e454e6f5d81a 60 else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
EHess 1:e454e6f5d81a 61
EHess 1:e454e6f5d81a 62 pwmRight = dutyCycleRight;
EHess 0:96f88638114b 63 }
EHess 0:96f88638114b 64
EHess 1:e454e6f5d81a 65 int main(){
EHess 1:e454e6f5d81a 66
EHess 1:e454e6f5d81a 67 // Initialisieren der PWM Ausgaenge pwmLeft.period(0.00005f); // PWM Periode von 50 us
EHess 1:e454e6f5d81a 68 pwmLeft.period(0.00005f); // Setzt die Periode auf 50 μs
EHess 1:e454e6f5d81a 69 pwmRight.period(0.00005f);
EHess 1:e454e6f5d81a 70 pwmLeft = 0.5f; // Duty-Cycle von 50% pwmRight.period(0.00005f); // PWM Periode von 50 us
EHess 1:e454e6f5d81a 71 pwmRight = 0.5f; // Duty-Cycle von 50%
EHess 0:96f88638114b 72
EHess 1:e454e6f5d81a 73 // Initialisieren von lokalen Variabeln
EHess 1:e454e6f5d81a 74 previousValueCounterLeft = counterLeft.read();
EHess 1:e454e6f5d81a 75 previousValueCounterRight = counterRight.read();
EHess 1:e454e6f5d81a 76 speedLeftFilter.setPeriod(PERIOD);
EHess 1:e454e6f5d81a 77 speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
EHess 1:e454e6f5d81a 78 speedRightFilter.setPeriod(PERIOD);
EHess 1:e454e6f5d81a 79 speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
EHess 0:96f88638114b 80
EHess 1:e454e6f5d81a 81 desiredSpeedLeft = 0.0f;
EHess 1:e454e6f5d81a 82 desiredSpeedRight = 0.0f;
EHess 1:e454e6f5d81a 83 actualSpeedLeft = 0.0f;
EHess 1:e454e6f5d81a 84 actualSpeedRight = 0.0f;
EHess 1:e454e6f5d81a 85
EHess 1:e454e6f5d81a 86 Ticker t1;
EHess 1:e454e6f5d81a 87 t1.attach( &speedCtrl, PERIOD);
EHess 1:e454e6f5d81a 88
EHess 1:e454e6f5d81a 89 desiredSpeedLeft = 10.0f; //50 RPM
EHess 1:e454e6f5d81a 90 desiredSpeedRight = 10.0f; //50 RPM
EHess 0:96f88638114b 91 enableMotorDriver = 1;
EHess 0:96f88638114b 92
EHess 0:96f88638114b 93 while(1) {
EHess 1:e454e6f5d81a 94 my_led = !my_led;
EHess 1:e454e6f5d81a 95 wait(0.5);
EHess 1:e454e6f5d81a 96
EHess 1:e454e6f5d81a 97
EHess 1:e454e6f5d81a 98
EHess 1:e454e6f5d81a 99
EHess 1:e454e6f5d81a 100
EHess 1:e454e6f5d81a 101 // printf( "enc A: %d, enc B:%d \n\r", counterRight.read(), counterLeft.read());
EHess 1:e454e6f5d81a 102
EHess 1:e454e6f5d81a 103 // if(i%10 == 0){
EHess 1:e454e6f5d81a 104 // desiredSpeedLeft -= 1.0f;
EHess 1:e454e6f5d81a 105 // desiredSpeedRight += 1.0f;
EHess 1:e454e6f5d81a 106 // }
EHess 0:96f88638114b 107 }
EHess 1:e454e6f5d81a 108 }