Marijke Abma / Mbed 2 deprecated motor1aansturing201014

Programma om motor 1 aan te sturen

Dependencies:   Encoder MODSERIAL mbed-dsp mbed

Fork of motor1aansturing by BMT M9 Groep01

main.cpp@25:9ab3123c2b15, 2014-10-17 (annotated)

Committer:
Jolein
Date:
Fri Oct 17 07:09:41 2014 +0000
Revision:
25:9ab3123c2b15
Parent:
24:543c8ee9bca1
Child:
26:02170c78ac2f
publishing

Who changed what in which revision?

UserRevisionLine numberNew 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 25:9ab3123c2b15 44 while(1)
Jolein 25:9ab3123c2b15 45 {
Jolein 0:eb00992c1597 46 float setpoint;
Jolein 0:eb00992c1597 47 float new_pwm;
Jolein 0:eb00992c1597 48 while(!looptimerflag);
Jolein 2:ef0fa691e77e 49 looptimerflag = false;
Jolein 16:bf4c5affd3e9 50
Jolein 21:6412e24cef74 51 if (motor1.getPosition()<= ANGLEMIN && toestand != SLAAN)
Jolein 16:bf4c5affd3e9 52 {
Jolein 22:93d32c57bac8 53 toestand = WACHTEN;
Jolein 22:93d32c57bac8 54 }
Jolein 22:93d32c57bac8 55
Jolein 25:9ab3123c2b15 56 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 57 {
Jolein 22:93d32c57bac8 58 toestand = SLAAN;
Jolein 25:9ab3123c2b15 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 25:9ab3123c2b15 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 25:9ab3123c2b15 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 24:543c8ee9bca1 79 //new_pwm = getal, opletten dat deze tijdens het slaan niet kan veranderen!
Jolein 24:543c8ee9bca1 80 //kan niet want hij gaat pas naar terugkeren als ANGLEMAX is overschreden
Jolein 24:543c8ee9bca1 81 //en hij kan niet bij veranderingen van setpoints komen als de toestand = SLAAN
Jolein 22:93d32c57bac8 82 if (toestand != SLAAN)//is dit de beste manier en plaats voor dit?
Jolein 22:93d32c57bac8 83 {
Jolein 23:29428dd42769 84 setpoint = 0;
Jolein 22:93d32c57bac8 85 }
Jolein 0:eb00992c1597 86 new_pwm = pid(setpoint, motor1.getPosition());
Jolein 0:eb00992c1597 87 clamp(&new_pwm, -1,1);
Jolein 17:0116e2f17d0d 88 //-------------------------------------------------------------------------------------output motor richting-------------------------------------
Jolein 0:eb00992c1597 89 if(new_pwm > 0)
Jolein 0:eb00992c1597 90 motordir = 0;
Jolein 0:eb00992c1597 91 else
Jolein 0:eb00992c1597 92 motordir = 1;
Jolein 22:93d32c57bac8 93 pwm_motor.write(abs(new_pwm));
Jolein 25:9ab3123c2b15 94 if(motor1.getPosition()>= ANGLEMAX && toestand==SLAAN)
Jolein 14:6ec84bfda0f3 95 {
Jolein 15:3a6074f3ceaf 96 toestand = TERUGKEREN;
Jolein 22:93d32c57bac8 97 //setpoint = 0 //misschien -5 oid... (hij moet 0 wel echt berijken)
Jolein 18:1188cdb7ccbe 98 // pc.printf ("Toestand = terugkeren!");
Jolein 14:6ec84bfda0f3 99 }
Jolein 25:9ab3123c2b15 100 } // eind while (1)
Jolein 25:9ab3123c2b15 101 } //eind int main
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 }