Fernon Eijkhoudt / Mbed 2 deprecated RoboBirdV1

Alles in 1

Dependencies:   Encoder HIDScope MODSERIAL QEI mbed

Fork of RoboBird3 by Fernon Eijkhoudt

main.cpp@19:b8d959e02e5d, 2015-10-07 (annotated)

Committer:
Fernon
Date:
Wed Oct 07 20:22:21 2015 +0000
Revision:
19:b8d959e02e5d
Parent:
18:0f7f57228901
Child:
20:473735947e52
Working PID controller, na 5 seconden gaat hij terug naar home

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 1:bb11e38dda43 6 DigitalOut Direction(D4); //1 = CCW - 0 = CW
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 16:b0ec8e6a8ad4 16 Timer timer;
yc238 13:c60942a1da8e 17 HIDScope scope(3);
Fernon 0:5a5f417fa1b2 18
yc238 15:f7e2b20f4dca 19
Fernon 10:e210675cbe71 20 double z=0; //initiele waarde potmeter
Fernon 8:a2b725b502d8 21 const double twopi = 6.2831853071795;
Fernon 8:a2b725b502d8 22 int Pulses;
Fernon 10:e210675cbe71 23 double Rotatie; //aantal graden dat de motor is gedraaid
yc238 14:b9c925a8176a 24 double Rotatieup=0; //aantal graden dat de motor is gedraaid in een bereik van n*pi
Fernon 11:a9a23042fc9e 25 double Goal = 0; //initele waarde goal waar de motor naar toe moet, dit wordt gedaan
Fernon 8:a2b725b502d8 26 double Error = 0;
Fernon 18:0f7f57228901 27 double Errord = 0;
Fernon 18:0f7f57228901 28 double Errori = 0;
Fernon 18:0f7f57228901 29 double Errorp = 0;
Fernon 18:0f7f57228901 30 const double Kp = 0.2; //Moet berekend worden aan de hand van Control concept slides
Fernon 18:0f7f57228901 31 const double Kd = 10;
Fernon 19:b8d959e02e5d 32 const double Ki = 0.2;
Fernon 10:e210675cbe71 33 double v = 0; //snelheid van de motor (0-0.1
Fernon 10:e210675cbe71 34 double upperlimit; //max aantal rotaties
Fernon 11:a9a23042fc9e 35 bool Excecute = false;
Fernon 17:c5eea26e171d 36 bool Excecute2 = false;
Fernon 18:0f7f57228901 37 const double Fs=100;
Fernon 10:e210675cbe71 38
yc238 15:f7e2b20f4dca 39 double n = 3;
Fernon 2:f0e9ffc5df09 40
yc238 14:b9c925a8176a 41 void MotorSet()
Fernon 2:f0e9ffc5df09 42 {
Fernon 18:0f7f57228901 43 v=Kp*Error + Kd*Errord + Ki*Errori;
Fernon 18:0f7f57228901 44 if (Error>=0) {
yc238 14:b9c925a8176a 45 Direction=1;
yc238 14:b9c925a8176a 46 } else {
yc238 14:b9c925a8176a 47 Direction=0;
yc238 14:b9c925a8176a 48 }
yc238 14:b9c925a8176a 49 PowerMotor.write(fabs(v));
Fernon 2:f0e9ffc5df09 50 }
yc238 15:f7e2b20f4dca 51 void Send()
yc238 15:f7e2b20f4dca 52 {
yc238 15:f7e2b20f4dca 53 scope.set(0,Goal);
yc238 15:f7e2b20f4dca 54 scope.set(1,Rotatieup);
yc238 15:f7e2b20f4dca 55 scope.set(2,Error);
Fernon 16:b0ec8e6a8ad4 56 scope.send();
Fernon 16:b0ec8e6a8ad4 57 }
Fernon 0:5a5f417fa1b2 58 int main()
Fernon 0:5a5f417fa1b2 59 {
Fernon 11:a9a23042fc9e 60 upperlimit = n*twopi;
Fernon 2:f0e9ffc5df09 61 pc.baud(115200);
Fernon 2:f0e9ffc5df09 62 PowerMotor.write(0);
Fernon 16:b0ec8e6a8ad4 63 Sender.attach(Send,0.5/Fs);
yc238 15:f7e2b20f4dca 64 MotorWrite.attach(MotorSet,1/Fs); // Deze ticker moet de waarde uitlezen van de PotMeter Fs keer per seconde
Fernon 0:5a5f417fa1b2 65 while (true) {
Fernon 17:c5eea26e171d 66 Encoder.reset();
Fernon 11:a9a23042fc9e 67 if (Button == 0) {
Fernon 11:a9a23042fc9e 68 Excecute =! Excecute;
Fernon 17:c5eea26e171d 69 }
yc238 12:dcf90cafb2a5 70 while (Excecute ) {
Fernon 16:b0ec8e6a8ad4 71 Pulses = Encoder.getPulses();
yc238 12:dcf90cafb2a5 72 Rotatie = (Pulses*twopi)/4192; // Aantal graden draaien in radialen
yc238 12:dcf90cafb2a5 73 Rotatieup = fmod(Rotatie,upperlimit); //Aantal graden draaien in radialen binnen het bereik van upperlimit
yc238 12:dcf90cafb2a5 74 pc.printf ("Rotatie = %f [radialen] \n", Rotatieup);
Fernon 19:b8d959e02e5d 75 Goal = 0.5*twopi; //PotMeter.read()*upperlimit; // Het doel waar hij naar toe moet
yc238 12:dcf90cafb2a5 76 Error = Goal-Rotatieup; // De error die het motortje maakt ten opzichte van je Goal
Fernon 18:0f7f57228901 77 Errord = (Error-Errorp)/Fs;
Fernon 18:0f7f57228901 78 Errorp = Error;
Fernon 18:0f7f57228901 79 if (fabs(Error) <= 0.5) {
Fernon 18:0f7f57228901 80 Errori = Errori + Error*1/Fs;
Fernon 18:0f7f57228901 81 } else {
Fernon 18:0f7f57228901 82 Errori = 0;
Fernon 18:0f7f57228901 83 }
yc238 14:b9c925a8176a 84 pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
Fernon 18:0f7f57228901 85 if (fabs(Error)<=0.0015) {
Fernon 16:b0ec8e6a8ad4 86 timer.start();
Fernon 16:b0ec8e6a8ad4 87 } else {
Fernon 16:b0ec8e6a8ad4 88 timer.stop();
Fernon 16:b0ec8e6a8ad4 89 timer.reset();
Fernon 16:b0ec8e6a8ad4 90 }
Fernon 16:b0ec8e6a8ad4 91 if (timer.read() >= 5) {
yc238 14:b9c925a8176a 92 Excecute=false;
Fernon 17:c5eea26e171d 93 Excecute2 = true;
Fernon 17:c5eea26e171d 94 Error = 0;
Fernon 18:0f7f57228901 95 Errori = 0;
Fernon 18:0f7f57228901 96 Errord = 0;
Fernon 17:c5eea26e171d 97 }
Fernon 17:c5eea26e171d 98 }
Fernon 17:c5eea26e171d 99 while (Excecute2) {
Fernon 17:c5eea26e171d 100 Pulses = Encoder.getPulses();
Fernon 17:c5eea26e171d 101 Rotatie = (Pulses*twopi)/4192; // Aantal graden draaien in radialen
Fernon 17:c5eea26e171d 102 Rotatieup = fmod(Rotatie,upperlimit); //Aantal graden draaien in radialen binnen het bereik van upperlimit
Fernon 17:c5eea26e171d 103 pc.printf ("Rotatie = %f [radialen] \n", Rotatieup);
Fernon 17:c5eea26e171d 104 Goal = 0; // Het doel waar hij naar toe moet
Fernon 17:c5eea26e171d 105 Error = Goal-Rotatieup; // De error die het motortje maakt ten opzichte van je Goal
Fernon 19:b8d959e02e5d 106 Errord = (Error-Errorp)/Fs;
Fernon 19:b8d959e02e5d 107 Errorp = Error;
Fernon 19:b8d959e02e5d 108 if (fabs(Error) <= 0.5) {
Fernon 19:b8d959e02e5d 109 Errori = Errori + Error*1/Fs;
Fernon 19:b8d959e02e5d 110 } else {
Fernon 19:b8d959e02e5d 111 Errori = 0;
Fernon 19:b8d959e02e5d 112 }
Fernon 17:c5eea26e171d 113 pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
Fernon 18:0f7f57228901 114 if (fabs(Error)<=0.0015) {
Fernon 17:c5eea26e171d 115 timer.start();
Fernon 17:c5eea26e171d 116 } else {
Fernon 17:c5eea26e171d 117 timer.stop();
Fernon 17:c5eea26e171d 118 timer.reset();
Fernon 17:c5eea26e171d 119 }
Fernon 17:c5eea26e171d 120 if (timer.read() >= 5) {
Fernon 19:b8d959e02e5d 121 Excecute=false;
Fernon 19:b8d959e02e5d 122 Excecute2 = false;
Fernon 17:c5eea26e171d 123 Error = 0;
Fernon 18:0f7f57228901 124 Errori = 0;
Fernon 18:0f7f57228901 125 Errord = 0;
Fernon 11:a9a23042fc9e 126 }
Fernon 8:a2b725b502d8 127 }
Fernon 0:5a5f417fa1b2 128 }
Fernon 17:c5eea26e171d 129 }