Fernon Eijkhoudt / Mbed 2 deprecated RoboBirdV1

Alles in 1

Dependencies:   Encoder HIDScope MODSERIAL QEI mbed

Fork of RoboBird3 by Fernon Eijkhoudt

main.cpp@16:b0ec8e6a8ad4, 2015-10-07 (annotated)

Committer:
Fernon
Date:
Wed Oct 07 18:17:58 2015 +0000
Revision:
16:b0ec8e6a8ad4
Parent:
15:f7e2b20f4dca
Child:
17:c5eea26e171d
Working P controller!

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;
yc238 14:b9c925a8176a 31 double Fs=100;
Fernon 10:e210675cbe71 32
yc238 15:f7e2b20f4dca 33 double n = 3;
Fernon 2:f0e9ffc5df09 34
yc238 14:b9c925a8176a 35 void MotorSet()
Fernon 2:f0e9ffc5df09 36 {
yc238 14:b9c925a8176a 37 v=Kp*Error;
yc238 14:b9c925a8176a 38 if (v>=0) {
yc238 14:b9c925a8176a 39 Direction=1;
yc238 14:b9c925a8176a 40 } else {
yc238 14:b9c925a8176a 41 Direction=0;
yc238 14:b9c925a8176a 42 }
yc238 14:b9c925a8176a 43 PowerMotor.write(fabs(v));
Fernon 2:f0e9ffc5df09 44 }
yc238 15:f7e2b20f4dca 45 void Send()
yc238 15:f7e2b20f4dca 46 {
yc238 15:f7e2b20f4dca 47 scope.set(0,Goal);
yc238 15:f7e2b20f4dca 48 scope.set(1,Rotatieup);
yc238 15:f7e2b20f4dca 49 scope.set(2,Error);
Fernon 16:b0ec8e6a8ad4 50 scope.send();
Fernon 16:b0ec8e6a8ad4 51 }
Fernon 0:5a5f417fa1b2 52 int main()
Fernon 0:5a5f417fa1b2 53 {
Fernon 11:a9a23042fc9e 54 upperlimit = n*twopi;
Fernon 2:f0e9ffc5df09 55 pc.baud(115200);
Fernon 2:f0e9ffc5df09 56 PowerMotor.write(0);
Fernon 16:b0ec8e6a8ad4 57 Sender.attach(Send,0.5/Fs);
yc238 15:f7e2b20f4dca 58 MotorWrite.attach(MotorSet,1/Fs); // Deze ticker moet de waarde uitlezen van de PotMeter Fs keer per seconde
Fernon 0:5a5f417fa1b2 59 while (true) {
Fernon 11:a9a23042fc9e 60 if (Button == 0) {
Fernon 11:a9a23042fc9e 61 Excecute =! Excecute;
Fernon 16:b0ec8e6a8ad4 62 }
yc238 12:dcf90cafb2a5 63 while (Excecute ) {
Fernon 16:b0ec8e6a8ad4 64 Pulses = Encoder.getPulses();
yc238 12:dcf90cafb2a5 65 Rotatie = (Pulses*twopi)/4192; // Aantal graden draaien in radialen
yc238 12:dcf90cafb2a5 66 Rotatieup = fmod(Rotatie,upperlimit); //Aantal graden draaien in radialen binnen het bereik van upperlimit
yc238 12:dcf90cafb2a5 67 pc.printf ("Rotatie = %f [radialen] \n", Rotatieup);
yc238 14:b9c925a8176a 68 Goal = PotMeter.read()*upperlimit; // Het doel waar hij naar toe moet
yc238 12:dcf90cafb2a5 69 Error = Goal-Rotatieup; // De error die het motortje maakt ten opzichte van je Goal
yc238 14:b9c925a8176a 70 pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
Fernon 16:b0ec8e6a8ad4 71 if (fabs(Error)<=0.15) {
Fernon 16:b0ec8e6a8ad4 72 timer.start();
Fernon 16:b0ec8e6a8ad4 73 } else {
Fernon 16:b0ec8e6a8ad4 74 timer.stop();
Fernon 16:b0ec8e6a8ad4 75 timer.reset();
Fernon 16:b0ec8e6a8ad4 76 }
Fernon 16:b0ec8e6a8ad4 77 if (timer.read() >= 5) {
yc238 14:b9c925a8176a 78 Excecute=false;
Fernon 16:b0ec8e6a8ad4 79 Goal = 0;
Fernon 11:a9a23042fc9e 80 }
Fernon 8:a2b725b502d8 81 }
Fernon 0:5a5f417fa1b2 82 }
Fernon 16:b0ec8e6a8ad4 83 }