Fernon Eijkhoudt / Mbed 2 deprecated RoboBirdV1

Alles in 1

Dependencies:   Encoder HIDScope MODSERIAL QEI mbed

Fork of RoboBird3 by Fernon Eijkhoudt

main.cpp@20:473735947e52, 2015-10-09 (annotated)

Committer:
Fernon
Date:
Fri Oct 09 10:20:15 2015 +0000
Revision:
20:473735947e52
Parent:
19:b8d959e02e5d
Child:
21:d0156eadcbfe
PID controller werkend, nu toegespits op groep. Ik heb een motor 1 en motor 2 toegevoegd, en dat als hij boven een bepaalde waarde uit komt gaat hij weer terug naar veilige waarde. EMG moet nog goed geimplementeerd worden

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Fernon 0:5a5f417fa1b2 1 #include "mbed.h"
Fernon 16:b0ec8e6a8ad4 2 #include "QEI.h"
Fernon 8:a2b725b502d8 3 #include "math.h"
yc238 12:dcf90cafb2a5 4 #include "HIDScope.h"
Fernon 0:5a5f417fa1b2 5
Fernon 20:473735947e52 6 DigitalOut Direction(D4); //1 = CCW - 0 = CW, moet nog omgezet worden naar up en down
Fernon 1:bb11e38dda43 7 PwmOut PowerMotor(D5); //van 0 tot 1
yc238 15:f7e2b20f4dca 8 PwmOut PowerServo(D7);
Fernon 16:b0ec8e6a8ad4 9 DigitalIn Button(PTC6);
Fernon 18:0f7f57228901 10 DigitalIn Button2(PTC4);
Fernon 1:bb11e38dda43 11 AnalogIn PotMeter(A1);
Fernon 16:b0ec8e6a8ad4 12 QEI Encoder(D13,D12,NC,32,QEI::X2_ENCODING); //Encoder
Fernon 1:bb11e38dda43 13 Serial pc(USBTX, USBRX);
yc238 14:b9c925a8176a 14 Ticker MotorWrite;
yc238 15:f7e2b20f4dca 15 Ticker Sender;
Fernon 20:473735947e52 16 Ticker EMG;
Fernon 16:b0ec8e6a8ad4 17 Timer timer;
yc238 13:c60942a1da8e 18 HIDScope scope(3);
Fernon 0:5a5f417fa1b2 19
yc238 15:f7e2b20f4dca 20
Fernon 10:e210675cbe71 21 double z=0; //initiele waarde potmeter
Fernon 8:a2b725b502d8 22 const double twopi = 6.2831853071795;
Fernon 8:a2b725b502d8 23 int Pulses;
Fernon 20:473735947e52 24 const double downlimit = 0.3
Fernon 20:473735947e52 25 const double margin = 0.3
Fernon 20:473735947e52 26 double Rotatie; //aantal graden dat de motor is gedraaid
yc238 14:b9c925a8176a 27 double Rotatieup=0; //aantal graden dat de motor is gedraaid in een bereik van n*pi
Fernon 11:a9a23042fc9e 28 double Goal = 0; //initele waarde goal waar de motor naar toe moet, dit wordt gedaan
Fernon 8:a2b725b502d8 29 double Error = 0;
Fernon 18:0f7f57228901 30 double Errord = 0;
Fernon 18:0f7f57228901 31 double Errori = 0;
Fernon 18:0f7f57228901 32 double Errorp = 0;
Fernon 18:0f7f57228901 33 const double Kp = 0.2; //Moet berekend worden aan de hand van Control concept slides
Fernon 18:0f7f57228901 34 const double Kd = 10;
Fernon 19:b8d959e02e5d 35 const double Ki = 0.2;
Fernon 10:e210675cbe71 36 double v = 0; //snelheid van de motor (0-0.1
Fernon 10:e210675cbe71 37 double upperlimit; //max aantal rotaties
Fernon 11:a9a23042fc9e 38 bool Excecute = false;
Fernon 17:c5eea26e171d 39 bool Excecute2 = false;
Fernon 18:0f7f57228901 40 const double Fs=100;
Fernon 10:e210675cbe71 41
Fernon 20:473735947e52 42 double n = 4.43546; // Aantal rondjes dat ons apparaat maximaal mag draaien
Fernon 2:f0e9ffc5df09 43
yc238 14:b9c925a8176a 44 void MotorSet()
Fernon 2:f0e9ffc5df09 45 {
Fernon 18:0f7f57228901 46 v=Kp*Error + Kd*Errord + Ki*Errori;
Fernon 18:0f7f57228901 47 if (Error>=0) {
yc238 14:b9c925a8176a 48 Direction=1;
yc238 14:b9c925a8176a 49 } else {
yc238 14:b9c925a8176a 50 Direction=0;
yc238 14:b9c925a8176a 51 }
yc238 14:b9c925a8176a 52 PowerMotor.write(fabs(v));
Fernon 2:f0e9ffc5df09 53 }
yc238 15:f7e2b20f4dca 54 void Send()
yc238 15:f7e2b20f4dca 55 {
yc238 15:f7e2b20f4dca 56 scope.set(0,Goal);
yc238 15:f7e2b20f4dca 57 scope.set(1,Rotatieup);
yc238 15:f7e2b20f4dca 58 scope.set(2,Error);
Fernon 16:b0ec8e6a8ad4 59 scope.send();
Fernon 16:b0ec8e6a8ad4 60 }
Fernon 20:473735947e52 61 void EMGsignal()
Fernon 20:473735947e52 62 {
Fernon 20:473735947e52 63 // Lees het EMG signaal uit
Fernon 20:473735947e52 64 //if (Treshold => x) {
Fernon 20:473735947e52 65 // v = 1
Fernon 20:473735947e52 66 // } else {
Fernon 20:473735947e52 67 // v = 0
Fernon 20:473735947e52 68 // }
Fernon 20:473735947e52 69 // PowerMotor.write(v)
Fernon 20:473735947e52 70 }
Fernon 20:473735947e52 71
Fernon 20:473735947e52 72
Fernon 20:473735947e52 73
Fernon 20:473735947e52 74
Fernon 0:5a5f417fa1b2 75 int main()
Fernon 0:5a5f417fa1b2 76 {
Fernon 11:a9a23042fc9e 77 upperlimit = n*twopi;
Fernon 2:f0e9ffc5df09 78 pc.baud(115200);
Fernon 2:f0e9ffc5df09 79 PowerMotor.write(0);
Fernon 16:b0ec8e6a8ad4 80 Sender.attach(Send,0.5/Fs);
yc238 15:f7e2b20f4dca 81 MotorWrite.attach(MotorSet,1/Fs); // Deze ticker moet de waarde uitlezen van de PotMeter Fs keer per seconde
Fernon 20:473735947e52 82 EMG.attach(EMGsignal,1/Fs);
Fernon 0:5a5f417fa1b2 83 while (true) {
Fernon 17:c5eea26e171d 84 Encoder.reset();
Fernon 11:a9a23042fc9e 85 if (Button == 0) {
Fernon 11:a9a23042fc9e 86 Excecute =! Excecute;
Fernon 17:c5eea26e171d 87 }
Fernon 20:473735947e52 88 while (Excecute ) { //Dit wordt gebruikt voor motor 1
Fernon 16:b0ec8e6a8ad4 89 Pulses = Encoder.getPulses();
Fernon 20:473735947e52 90 Rotatie = (Pulses*twopi)/4200; // Aantal radialen draaien
Fernon 20:473735947e52 91 Rotatieup = fmod(Rotatie,upperlimit); //Aantal radialen draaien binnen het bereik van upperlimit
yc238 12:dcf90cafb2a5 92 pc.printf ("Rotatie = %f [radialen] \n", Rotatieup);
Fernon 20:473735947e52 93 if (Rotatie >= upperlimit) {
Fernon 20:473735947e52 94 Goal = upperlimit - margin;
Fernon 20:473735947e52 95 Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal
Fernon 20:473735947e52 96 Errord = (Error-Errorp)/Fs;
Fernon 20:473735947e52 97 Errorp = Error;
Fernon 20:473735947e52 98 if (fabs(Error) <= 0.5) {
Fernon 20:473735947e52 99 Errori = Errori + Error*1/Fs;
Fernon 20:473735947e52 100 } else {
Fernon 20:473735947e52 101 Errori = 0;
Fernon 20:473735947e52 102 }
Fernon 20:473735947e52 103 pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
Fernon 18:0f7f57228901 104 }
Fernon 20:473735947e52 105 if (Rotatie <= downlimit) {
Fernon 20:473735947e52 106 Goal = 0 + margin;
Fernon 20:473735947e52 107 Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal
Fernon 20:473735947e52 108 Errord = (Error-Errorp)/Fs;
Fernon 20:473735947e52 109 Errorp = Error;
Fernon 20:473735947e52 110 if (fabs(Error) <= 0.5) {
Fernon 20:473735947e52 111 Errori = Errori + Error*1/Fs;
Fernon 20:473735947e52 112 } else {
Fernon 20:473735947e52 113 Errori = 0;
Fernon 20:473735947e52 114 }
Fernon 20:473735947e52 115 pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
Fernon 16:b0ec8e6a8ad4 116 }
Fernon 20:473735947e52 117
Fernon 20:473735947e52 118 else {
Fernon 20:473735947e52 119 // PowerMotor.write(EMG), hier moet dus de output van het EMG signaal gebruikt worden
Fernon 20:473735947e52 120 Goal = z*upperlimit;
Fernon 20:473735947e52 121 Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal
Fernon 20:473735947e52 122 Errord = (Error-Errorp)/Fs;
Fernon 20:473735947e52 123 Errorp = Error;
Fernon 20:473735947e52 124 if (fabs(Error) <= 0.5) {
Fernon 20:473735947e52 125 Errori = Errori + Error*1/Fs;
Fernon 20:473735947e52 126 } else {
Fernon 20:473735947e52 127 Errori = 0;
Fernon 20:473735947e52 128 }
Fernon 20:473735947e52 129 pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
Fernon 17:c5eea26e171d 130 }
Fernon 17:c5eea26e171d 131 }
Fernon 20:473735947e52 132 while (Excecute) { // Dit is voor motor 2, dus naar boven en naar beneden
Fernon 20:473735947e52 133 Pulses = Encoder.getPulses();
Fernon 20:473735947e52 134 Rotatie = (Pulses*twopi)/4200; // Aantal radialen draaien
Fernon 20:473735947e52 135 Rotatieup = fmod(Rotatie,upperlimit); //Aantal radialen draaien binnen het bereik van upperlimit
Fernon 20:473735947e52 136 pc.printf ("Rotatie = %f [radialen] \n", Rotatieup);
Fernon 20:473735947e52 137 if (Rotatie >= upperlimit) {
Fernon 20:473735947e52 138 Goal = upperlimit - margin;
Fernon 20:473735947e52 139 Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal
Fernon 20:473735947e52 140 Errord = (Error-Errorp)/Fs;
Fernon 20:473735947e52 141 Errorp = Error;
Fernon 20:473735947e52 142 if (fabs(Error) <= 0.5) {
Fernon 20:473735947e52 143 Errori = Errori + Error*1/Fs;
Fernon 20:473735947e52 144 } else {
Fernon 20:473735947e52 145 Errori = 0;
Fernon 20:473735947e52 146 }
Fernon 20:473735947e52 147 pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
Fernon 20:473735947e52 148 }
Fernon 20:473735947e52 149 if (Rotatie <= downlimit) {
Fernon 20:473735947e52 150 Goal = 0 + margin;
Fernon 20:473735947e52 151 Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal
Fernon 20:473735947e52 152 Errord = (Error-Errorp)/Fs;
Fernon 20:473735947e52 153 Errorp = Error;
Fernon 20:473735947e52 154 if (fabs(Error) <= 0.5) {
Fernon 20:473735947e52 155 Errori = Errori + Error*1/Fs;
Fernon 20:473735947e52 156 } else {
Fernon 20:473735947e52 157 Errori = 0;
Fernon 20:473735947e52 158 }
Fernon 20:473735947e52 159 pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
Fernon 20:473735947e52 160 }
Fernon 20:473735947e52 161
Fernon 20:473735947e52 162 else {
Fernon 20:473735947e52 163 // PowerMotor.write(EMG), hier moet dus de output van het EMG signaal gebruikt worden
Fernon 20:473735947e52 164 Goal = z*upperlimit;
Fernon 20:473735947e52 165 Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal
Fernon 20:473735947e52 166 Errord = (Error-Errorp)/Fs;
Fernon 20:473735947e52 167 Errorp = Error;
Fernon 20:473735947e52 168 if (fabs(Error) <= 0.5) {
Fernon 20:473735947e52 169 Errori = Errori + Error*1/Fs;
Fernon 20:473735947e52 170 } else {
Fernon 20:473735947e52 171 Errori = 0;
Fernon 20:473735947e52 172 }
Fernon 20:473735947e52 173 pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
Fernon 20:473735947e52 174 }
Fernon 20:473735947e52 175 }
Fernon 20:473735947e52 176
Fernon 20:473735947e52 177
Fernon 20:473735947e52 178
Fernon 20:473735947e52 179 // Dit gedeelte moet vervangen worden if fired then Excecut2 = true after waiting 1 second
Fernon 20:473735947e52 180 // if (fabs(Error)<=0.0015) {
Fernon 20:473735947e52 181 // timer.start();
Fernon 20:473735947e52 182 // } else {
Fernon 20:473735947e52 183 // timer.stop();
Fernon 20:473735947e52 184 // timer.reset();
Fernon 20:473735947e52 185 // }
Fernon 20:473735947e52 186 // if (timer.read() >= 5) {
Fernon 20:473735947e52 187 // Excecute=false;
Fernon 20:473735947e52 188 // Excecute2 = true;
Fernon 20:473735947e52 189 // Errori = 0;
Fernon 20:473735947e52 190 // Errord = 0;
Fernon 20:473735947e52 191 // }
Fernon 20:473735947e52 192
Fernon 20:473735947e52 193 // }
Fernon 17:c5eea26e171d 194 while (Excecute2) {
Fernon 17:c5eea26e171d 195 Pulses = Encoder.getPulses();
Fernon 17:c5eea26e171d 196 Rotatie = (Pulses*twopi)/4192; // Aantal graden draaien in radialen
Fernon 17:c5eea26e171d 197 Rotatieup = fmod(Rotatie,upperlimit); //Aantal graden draaien in radialen binnen het bereik van upperlimit
Fernon 17:c5eea26e171d 198 pc.printf ("Rotatie = %f [radialen] \n", Rotatieup);
Fernon 20:473735947e52 199 Goal = 0.3; // Het doel waar hij naar toe moet
Fernon 17:c5eea26e171d 200 Error = Goal-Rotatieup; // De error die het motortje maakt ten opzichte van je Goal
Fernon 19:b8d959e02e5d 201 Errord = (Error-Errorp)/Fs;
Fernon 19:b8d959e02e5d 202 Errorp = Error;
Fernon 19:b8d959e02e5d 203 if (fabs(Error) <= 0.5) {
Fernon 19:b8d959e02e5d 204 Errori = Errori + Error*1/Fs;
Fernon 19:b8d959e02e5d 205 } else {
Fernon 19:b8d959e02e5d 206 Errori = 0;
Fernon 19:b8d959e02e5d 207 }
Fernon 17:c5eea26e171d 208 pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
Fernon 18:0f7f57228901 209 if (fabs(Error)<=0.0015) {
Fernon 17:c5eea26e171d 210 timer.start();
Fernon 17:c5eea26e171d 211 } else {
Fernon 17:c5eea26e171d 212 timer.stop();
Fernon 17:c5eea26e171d 213 timer.reset();
Fernon 17:c5eea26e171d 214 }
Fernon 17:c5eea26e171d 215 if (timer.read() >= 5) {
Fernon 19:b8d959e02e5d 216 Excecute=false;
Fernon 19:b8d959e02e5d 217 Excecute2 = false;
Fernon 18:0f7f57228901 218 Errori = 0;
Fernon 18:0f7f57228901 219 Errord = 0;
Fernon 11:a9a23042fc9e 220 }
Fernon 8:a2b725b502d8 221 }
Fernon 0:5a5f417fa1b2 222 }
Fernon 17:c5eea26e171d 223 }