zhaw_st16b_pes2_10 / Mbed 2 deprecated SpeedCtrl

a

Dependencies:   mbed

main.cpp@0:dfea4e0e064b, 2017-05-22 (annotated)

Committer:
beacon
Date:
Mon May 22 10:45:28 2017 +0000
Revision:
0:dfea4e0e064b
k

Who changed what in which revision?

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