Programma om motor 1 aan te sturen
Dependencies: Encoder MODSERIAL mbed-dsp mbed
Fork of motor1aansturing by
main.cpp@23:29428dd42769, 2014-10-10 (annotated)
- Committer:
- Jolein
- Date:
- Fri Oct 10 12:40:41 2014 +0000
- Revision:
- 23:29428dd42769
- Parent:
- 22:93d32c57bac8
- Child:
- 24:543c8ee9bca1
haakjes klopte niet na aanpassing if's, goed gemaakt
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Jolein | 0:eb00992c1597 | 1 | #include "mbed.h" |
Jolein | 0:eb00992c1597 | 2 | #include "encoder.h" |
Jolein | 15:3a6074f3ceaf | 3 | #include "MODSERIAL.h" |
Jolein | 15:3a6074f3ceaf | 4 | |
Jolein | 0:eb00992c1597 | 5 | #define TSAMP 0.01 |
Jolein | 21:6412e24cef74 | 6 | #define K_P (0.01) //aanpassen Ki Kp en Kd vóór motortest! |
Jolein | 0:eb00992c1597 | 7 | #define K_I (0 *TSAMP) |
Jolein | 2:ef0fa691e77e | 8 | #define K_D (0 /TSAMP) |
Jolein | 0:eb00992c1597 | 9 | #define I_LIMIT 1. |
Jolein | 0:eb00992c1597 | 10 | #define POT_AVG 50 |
Jolein | 16:bf4c5affd3e9 | 11 | #define WACHTEN 1 |
Jolein | 15:3a6074f3ceaf | 12 | #define SLAAN 2 |
Jolein | 15:3a6074f3ceaf | 13 | #define TERUGKEREN 3 |
Jolein | 18:1188cdb7ccbe | 14 | #define ANGLEMAX 244 |
Jolein | 18:1188cdb7ccbe | 15 | #define ANGLEMIN 0 |
Jolein | 0:eb00992c1597 | 16 | |
Jolein | 15:3a6074f3ceaf | 17 | MODSERIAL pc(USBTX,USBRX); |
Jolein | 0:eb00992c1597 | 18 | void clamp(float * in, float min, float max); |
Jolein | 0:eb00992c1597 | 19 | float pid(float setpoint, float measurement); |
Jolein | 0:eb00992c1597 | 20 | AnalogIn potmeter(PTC2); |
Jolein | 0:eb00992c1597 | 21 | volatile bool looptimerflag; |
Jolein | 0:eb00992c1597 | 22 | float potsamples[POT_AVG]; |
Jolein | 21:6412e24cef74 | 23 | int EMG(float biceps, float triceps); // waarschijnlijk niet float omdat input functies zijn |
Jolein | 0:eb00992c1597 | 24 | |
Jolein | 0:eb00992c1597 | 25 | void setlooptimerflag(void) |
Jolein | 0:eb00992c1597 | 26 | { |
Jolein | 0:eb00992c1597 | 27 | looptimerflag = true; |
Jolein | 0:eb00992c1597 | 28 | } |
Jolein | 0:eb00992c1597 | 29 | |
Jolein | 0:eb00992c1597 | 30 | int main() |
Jolein | 0:eb00992c1597 | 31 | { |
Jolein | 19:9ea206bb1b05 | 32 | float v3=0.2,v2=0.1,v1=0.05, readbiceps=0, readtriceps=0; |
Jolein | 18:1188cdb7ccbe | 33 | // zie schrift voor berekeningen ANGLEMAX. Is float ANGLEMAX nog nodig als ANGLEMAX = defined |
Jolein | 0:eb00992c1597 | 34 | Encoder motor1(PTD0,PTC9); |
Jolein | 0:eb00992c1597 | 35 | PwmOut pwm_motor(PTA5); |
Jolein | 0:eb00992c1597 | 36 | pwm_motor.period_us(100); |
Jolein | 0:eb00992c1597 | 37 | DigitalOut motordir(PTD1); |
Jolein | 16:bf4c5affd3e9 | 38 | |
Jolein | 16:bf4c5affd3e9 | 39 | int toestand = TERUGKEREN; |
Jolein | 21:6412e24cef74 | 40 | //moet hier wel int voor staan? want terugkeren is al gedifined alszijnde 3 (waarschijnlijk wel want toestand is hier voor het eerst benoemd...) |
Jolein | 16:bf4c5affd3e9 | 41 | |
Jolein | 0:eb00992c1597 | 42 | Ticker looptimer; |
Jolein | 0:eb00992c1597 | 43 | looptimer.attach(setlooptimerflag,TSAMP); |
Jolein | 0:eb00992c1597 | 44 | while(1) { |
Jolein | 0:eb00992c1597 | 45 | float setpoint; |
Jolein | 0:eb00992c1597 | 46 | float new_pwm; |
Jolein | 0:eb00992c1597 | 47 | while(!looptimerflag); |
Jolein | 2:ef0fa691e77e | 48 | looptimerflag = false; |
Jolein | 16:bf4c5affd3e9 | 49 | |
Jolein | 21:6412e24cef74 | 50 | if (motor1.getPosition()<= ANGLEMIN && toestand != SLAAN) |
Jolein | 16:bf4c5affd3e9 | 51 | { |
Jolein | 22:93d32c57bac8 | 52 | toestand = WACHTEN; |
Jolein | 22:93d32c57bac8 | 53 | } |
Jolein | 22:93d32c57bac8 | 54 | |
Jolein | 22:93d32c57bac8 | 55 | if (EMG(readbiceps, readtriceps) != 0 && toestand = WACHTEN) //door de ifs uit elkaar te halen en && condities toe te voegen kan het minder snel fout gaan! |
Jolein | 22:93d32c57bac8 | 56 | { |
Jolein | 22:93d32c57bac8 | 57 | toestand = SLAAN; |
Jolein | 17:0116e2f17d0d | 58 | // potmeter.read() wordt waarschijnlijk output EMG functie (0,1,2,3) |
Jolein | 22:93d32c57bac8 | 59 | if ( EMG(readbiceps, readtriceps)=3) |
Jolein | 22:93d32c57bac8 | 60 | { |
Jolein | 22:93d32c57bac8 | 61 | setpoint = v3; //experimenteel bepaald, snelheid(?)>>>=pulsewidthmodulation! geclampt tussen -1/1. om bovenste goal te raken |
Jolein | 22:93d32c57bac8 | 62 | } |
Jolein | 22:93d32c57bac8 | 63 | else if ( EMG(readbiceps, readtriceps)=2) |
Jolein | 22:93d32c57bac8 | 64 | { |
Jolein | 22:93d32c57bac8 | 65 | setpoint = v2; //experimenteel bepaald, snelheid om middelste goal te raken |
Jolein | 22:93d32c57bac8 | 66 | } |
Jolein | 22:93d32c57bac8 | 67 | else if (EMG(readbiceps, readtriceps)=1) |
Jolein | 22:93d32c57bac8 | 68 | { |
Jolein | 22:93d32c57bac8 | 69 | setpoint = v1; //experimenteel bepaald, snelheid om bovenste goal te raken |
Jolein | 22:93d32c57bac8 | 70 | } |
Jolein | 22:93d32c57bac8 | 71 | else //waarschijnlijk niet nodig want hij kan hier niet komen als readEMG nul is |
Jolein | 22:93d32c57bac8 | 72 | { |
Jolein | 23:29428dd42769 | 73 | toestand = TERUGKEREN; |
Jolein | 17:0116e2f17d0d | 74 | } |
Jolein | 16:bf4c5affd3e9 | 75 | } |
Jolein | 16:bf4c5affd3e9 | 76 | |
Jolein | 17:0116e2f17d0d | 77 | // De toestand moet vanaf wacht naar slaan gaan bij aanspanning van spier(1) Hier zit waarschijnlijk een !!!delay!!! in |
Jolein | 16:bf4c5affd3e9 | 78 | // vanwege het middelen over de tijd van het signaal. van MK en Tanja stukje script met functie met return (0/1/2 of 3). |
Jolein | 18:1188cdb7ccbe | 79 | // angle = ???? Encoder =motor1.getPosition() |
Jolein | 18:1188cdb7ccbe | 80 | //----------------------------------------------------------------------new_pwm = getal, opletten dat deze tijdens het slaan niet kan veranderen! |
Jolein | 22:93d32c57bac8 | 81 | if (toestand != SLAAN)//is dit de beste manier en plaats voor dit? |
Jolein | 22:93d32c57bac8 | 82 | { |
Jolein | 23:29428dd42769 | 83 | setpoint = 0; |
Jolein | 22:93d32c57bac8 | 84 | } |
Jolein | 0:eb00992c1597 | 85 | new_pwm = pid(setpoint, motor1.getPosition()); |
Jolein | 0:eb00992c1597 | 86 | clamp(&new_pwm, -1,1); |
Jolein | 17:0116e2f17d0d | 87 | //-------------------------------------------------------------------------------------output motor richting------------------------------------- |
Jolein | 0:eb00992c1597 | 88 | if(new_pwm > 0) |
Jolein | 0:eb00992c1597 | 89 | motordir = 0; |
Jolein | 0:eb00992c1597 | 90 | else |
Jolein | 0:eb00992c1597 | 91 | motordir = 1; |
Jolein | 22:93d32c57bac8 | 92 | pwm_motor.write(abs(new_pwm)); |
Jolein | 21:6412e24cef74 | 93 | if(motor1.getPosition()>= ANGLEMAX && toestand=SLAAN) |
Jolein | 14:6ec84bfda0f3 | 94 | { |
Jolein | 15:3a6074f3ceaf | 95 | toestand = TERUGKEREN; |
Jolein | 22:93d32c57bac8 | 96 | //setpoint = 0 //misschien -5 oid... (hij moet 0 wel echt berijken) |
Jolein | 18:1188cdb7ccbe | 97 | // pc.printf ("Toestand = terugkeren!"); |
Jolein | 14:6ec84bfda0f3 | 98 | } |
Jolein | 0:eb00992c1597 | 99 | } |
Jolein | 14:6ec84bfda0f3 | 100 | |
Jolein | 0:eb00992c1597 | 101 | } |
Jolein | 0:eb00992c1597 | 102 | |
Jolein | 0:eb00992c1597 | 103 | void clamp(float * in, float min, float max) |
Jolein | 0:eb00992c1597 | 104 | { |
Jolein | 0:eb00992c1597 | 105 | *in > min ? *in < max? : *in = max: *in = min; |
Jolein | 0:eb00992c1597 | 106 | } |
Jolein | 0:eb00992c1597 | 107 | |
Jolein | 0:eb00992c1597 | 108 | |
Jolein | 0:eb00992c1597 | 109 | float pid(float setpoint, float measurement) |
Jolein | 0:eb00992c1597 | 110 | { |
Jolein | 0:eb00992c1597 | 111 | float error; |
Jolein | 0:eb00992c1597 | 112 | static float prev_error = 0; |
Jolein | 0:eb00992c1597 | 113 | float out_p = 0; |
Jolein | 0:eb00992c1597 | 114 | static float out_i = 0; |
Jolein | 0:eb00992c1597 | 115 | float out_d = 0; |
Jolein | 0:eb00992c1597 | 116 | error = setpoint-measurement; |
Jolein | 0:eb00992c1597 | 117 | out_p = error*K_P; |
Jolein | 0:eb00992c1597 | 118 | out_i += error*K_I; |
Jolein | 0:eb00992c1597 | 119 | out_d = (error-prev_error)*K_D; |
Jolein | 0:eb00992c1597 | 120 | clamp(&out_i,-I_LIMIT,I_LIMIT); |
Jolein | 0:eb00992c1597 | 121 | prev_error = error; |
Jolein | 0:eb00992c1597 | 122 | return out_p + out_i + out_d; |
Jolein | 0:eb00992c1597 | 123 | } |