Fernon Eijkhoudt / Mbed 2 deprecated RoboBird3

almost perfect

Dependencies:   Encoder HIDScope MODSERIAL QEI mbed

Fork of Robobird2 by Fernon Eijkhoudt

main.cpp@8:a2b725b502d8, 2015-09-28 (annotated)

Committer:
Fernon
Date:
Mon Sep 28 19:56:53 2015 +0000
Revision:
8:a2b725b502d8
Parent:
7:2042e359bfc3
Child:
9:4743f3bb39b2
P controller werkend! Met 1 Goal

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Fernon 0:5a5f417fa1b2 1 #include "mbed.h"
Fernon 0:5a5f417fa1b2 2 #include "QEI.h"
Fernon 8:a2b725b502d8 3 #include "math.h"
Fernon 0:5a5f417fa1b2 4
Fernon 1:bb11e38dda43 5 DigitalOut Direction(D4); //1 = CCW - 0 = CW
Fernon 1:bb11e38dda43 6 PwmOut PowerMotor(D5); //van 0 tot 1
Fernon 1:bb11e38dda43 7 AnalogIn PotMeter(A1);
Fernon 1:bb11e38dda43 8 QEI Encoder(D13,D12,NC,32,QEI::X2_ENCODING); //Encoder
Fernon 1:bb11e38dda43 9 Serial pc(USBTX, USBRX);
Fernon 2:f0e9ffc5df09 10 Ticker Pot;
Fernon 0:5a5f417fa1b2 11
Fernon 6:cf20f04dbab4 12 double z=0;
Fernon 8:a2b725b502d8 13 const double twopi = 6.2831853071795;
Fernon 8:a2b725b502d8 14 const double pi = twopi/2;
Fernon 8:a2b725b502d8 15 int Pulses;
Fernon 8:a2b725b502d8 16 double Rotatie;
Fernon 8:a2b725b502d8 17 double Rotatietwopi;
Fernon 8:a2b725b502d8 18 double Goal = 0;
Fernon 8:a2b725b502d8 19 double Error = 0;
Fernon 8:a2b725b502d8 20 double K = 2;
Fernon 8:a2b725b502d8 21 double v;
Fernon 2:f0e9ffc5df09 22
Fernon 5:d47e6a96256b 23 void readpot()
Fernon 2:f0e9ffc5df09 24 {
Fernon 7:2042e359bfc3 25 z = PotMeter.read()/10;
Fernon 2:f0e9ffc5df09 26 }
Fernon 0:5a5f417fa1b2 27
Fernon 8:a2b725b502d8 28
Fernon 0:5a5f417fa1b2 29 int main()
Fernon 0:5a5f417fa1b2 30 {
Fernon 2:f0e9ffc5df09 31 pc.baud(115200);
Fernon 2:f0e9ffc5df09 32 PowerMotor.write(0);
Fernon 6:cf20f04dbab4 33 Pot.attach(readpot,0.1); // Deze ticker moet de waarde uitlezen van de PotMeter 10 keer per seconde
Fernon 8:a2b725b502d8 34 //PC.attach(print,0.5);
Fernon 0:5a5f417fa1b2 35 while (true) {
Fernon 8:a2b725b502d8 36 Pulses = Encoder.getPulses();
Fernon 8:a2b725b502d8 37 Rotatie = (Pulses*twopi)/4192;
Fernon 8:a2b725b502d8 38 Rotatietwopi = fmod(Rotatie,twopi);
Fernon 7:2042e359bfc3 39 pc.printf ("Potmeter = %f\n", z); // het weergeven van de waarde waar z (PotMeter) op dat moment is
Fernon 8:a2b725b502d8 40 pc.printf ("Rotatie = %f [radialen] \n", Rotatietwopi);
Fernon 8:a2b725b502d8 41 if (z > 0.05) {
Fernon 8:a2b725b502d8 42 Goal = pi;
Fernon 8:a2b725b502d8 43 }
Fernon 8:a2b725b502d8 44 Error = Goal-Rotatietwopi;
Fernon 8:a2b725b502d8 45 if (Error >= 0) {
Fernon 8:a2b725b502d8 46 Direction =0;
Fernon 8:a2b725b502d8 47 } else {
Fernon 8:a2b725b502d8 48 Direction =1;
Fernon 8:a2b725b502d8 49 }
Fernon 8:a2b725b502d8 50 pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
Fernon 8:a2b725b502d8 51 v = K*fabs(Error);
Fernon 8:a2b725b502d8 52 PowerMotor.write(v);
Fernon 0:5a5f417fa1b2 53 }
Fernon 2:f0e9ffc5df09 54 }
Fernon 3:af3f0ed8c99e 55
Fernon 8:a2b725b502d8 56
Fernon 8:a2b725b502d8 57