Fernon Eijkhoudt / Mbed 2 deprecated RoboBird3

almost perfect

Dependencies:   Encoder HIDScope MODSERIAL QEI mbed

Fork of Robobird2 by Fernon Eijkhoudt

main.cpp@17:c5eea26e171d, 2015-10-07 (annotated)

Committer:
Fernon
Date:
Wed Oct 07 18:25:02 2015 +0000
Revision:
17:c5eea26e171d
Parent:
16:b0ec8e6a8ad4
Child:
18:0f7f57228901
Working P controller with return after 5 seconds to 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 16:b0ec8e6a8ad4 10 //DigitalIn Button2(D2);
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;
yc238 13:c60942a1da8e 27 double Kp = 0.2; //Moet berekend worden aan de hand van Control concept slides
Fernon 10:e210675cbe71 28 double v = 0; //snelheid van de motor (0-0.1
Fernon 10:e210675cbe71 29 double upperlimit; //max aantal rotaties
Fernon 11:a9a23042fc9e 30 bool Excecute = false;
Fernon 17:c5eea26e171d 31 bool Excecute2 = false;
yc238 14:b9c925a8176a 32 double Fs=100;
Fernon 10:e210675cbe71 33
yc238 15:f7e2b20f4dca 34 double n = 3;
Fernon 2:f0e9ffc5df09 35
yc238 14:b9c925a8176a 36 void MotorSet()
Fernon 2:f0e9ffc5df09 37 {
yc238 14:b9c925a8176a 38 v=Kp*Error;
yc238 14:b9c925a8176a 39 if (v>=0) {
yc238 14:b9c925a8176a 40 Direction=1;
yc238 14:b9c925a8176a 41 } else {
yc238 14:b9c925a8176a 42 Direction=0;
yc238 14:b9c925a8176a 43 }
yc238 14:b9c925a8176a 44 PowerMotor.write(fabs(v));
Fernon 2:f0e9ffc5df09 45 }
yc238 15:f7e2b20f4dca 46 void Send()
yc238 15:f7e2b20f4dca 47 {
yc238 15:f7e2b20f4dca 48 scope.set(0,Goal);
yc238 15:f7e2b20f4dca 49 scope.set(1,Rotatieup);
yc238 15:f7e2b20f4dca 50 scope.set(2,Error);
Fernon 16:b0ec8e6a8ad4 51 scope.send();
Fernon 16:b0ec8e6a8ad4 52 }
Fernon 0:5a5f417fa1b2 53 int main()
Fernon 0:5a5f417fa1b2 54 {
Fernon 11:a9a23042fc9e 55 upperlimit = n*twopi;
Fernon 2:f0e9ffc5df09 56 pc.baud(115200);
Fernon 2:f0e9ffc5df09 57 PowerMotor.write(0);
Fernon 16:b0ec8e6a8ad4 58 Sender.attach(Send,0.5/Fs);
yc238 15:f7e2b20f4dca 59 MotorWrite.attach(MotorSet,1/Fs); // Deze ticker moet de waarde uitlezen van de PotMeter Fs keer per seconde
Fernon 0:5a5f417fa1b2 60 while (true) {
Fernon 17:c5eea26e171d 61 Encoder.reset();
Fernon 11:a9a23042fc9e 62 if (Button == 0) {
Fernon 11:a9a23042fc9e 63 Excecute =! Excecute;
Fernon 17:c5eea26e171d 64 }
yc238 12:dcf90cafb2a5 65 while (Excecute ) {
Fernon 16:b0ec8e6a8ad4 66 Pulses = Encoder.getPulses();
yc238 12:dcf90cafb2a5 67 Rotatie = (Pulses*twopi)/4192; // Aantal graden draaien in radialen
yc238 12:dcf90cafb2a5 68 Rotatieup = fmod(Rotatie,upperlimit); //Aantal graden draaien in radialen binnen het bereik van upperlimit
yc238 12:dcf90cafb2a5 69 pc.printf ("Rotatie = %f [radialen] \n", Rotatieup);
yc238 14:b9c925a8176a 70 Goal = PotMeter.read()*upperlimit; // Het doel waar hij naar toe moet
yc238 12:dcf90cafb2a5 71 Error = Goal-Rotatieup; // De error die het motortje maakt ten opzichte van je Goal
yc238 14:b9c925a8176a 72 pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
Fernon 16:b0ec8e6a8ad4 73 if (fabs(Error)<=0.15) {
Fernon 16:b0ec8e6a8ad4 74 timer.start();
Fernon 16:b0ec8e6a8ad4 75 } else {
Fernon 16:b0ec8e6a8ad4 76 timer.stop();
Fernon 16:b0ec8e6a8ad4 77 timer.reset();
Fernon 16:b0ec8e6a8ad4 78 }
Fernon 16:b0ec8e6a8ad4 79 if (timer.read() >= 5) {
yc238 14:b9c925a8176a 80 Excecute=false;
Fernon 17:c5eea26e171d 81 Excecute2 = true;
Fernon 17:c5eea26e171d 82 Error = 0;
Fernon 17:c5eea26e171d 83 }
Fernon 17:c5eea26e171d 84 }
Fernon 17:c5eea26e171d 85 while (Excecute2) {
Fernon 17:c5eea26e171d 86 Pulses = Encoder.getPulses();
Fernon 17:c5eea26e171d 87 Rotatie = (Pulses*twopi)/4192; // Aantal graden draaien in radialen
Fernon 17:c5eea26e171d 88 Rotatieup = fmod(Rotatie,upperlimit); //Aantal graden draaien in radialen binnen het bereik van upperlimit
Fernon 17:c5eea26e171d 89 pc.printf ("Rotatie = %f [radialen] \n", Rotatieup);
Fernon 17:c5eea26e171d 90 Goal = 0; // Het doel waar hij naar toe moet
Fernon 17:c5eea26e171d 91 Error = Goal-Rotatieup; // De error die het motortje maakt ten opzichte van je Goal
Fernon 17:c5eea26e171d 92 pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
Fernon 17:c5eea26e171d 93 if (fabs(Error)<=0.15) {
Fernon 17:c5eea26e171d 94 timer.start();
Fernon 17:c5eea26e171d 95 } else {
Fernon 17:c5eea26e171d 96 timer.stop();
Fernon 17:c5eea26e171d 97 timer.reset();
Fernon 17:c5eea26e171d 98 }
Fernon 17:c5eea26e171d 99 if (timer.read() >= 5) {
Fernon 17:c5eea26e171d 100 Excecute2=false;
Fernon 17:c5eea26e171d 101 Error = 0;
Fernon 11:a9a23042fc9e 102 }
Fernon 8:a2b725b502d8 103 }
Fernon 0:5a5f417fa1b2 104 }
Fernon 17:c5eea26e171d 105 }