Fernon Eijkhoudt / Mbed 2 deprecated RoboBirdV1

Alles in 1

Dependencies:   Encoder HIDScope MODSERIAL QEI mbed

Fork of RoboBird3 by Fernon Eijkhoudt

main.cpp@9:4743f3bb39b2, 2015-09-28 (annotated)

Committer:
Fernon
Date:
Mon Sep 28 20:13:03 2015 +0000
Revision:
9:4743f3bb39b2
Parent:
8:a2b725b502d8
Child:
10:e210675cbe71
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 0:5a5f417fa1b2 34 while (true) {
Fernon 8:a2b725b502d8 35 Pulses = Encoder.getPulses();
Fernon 8:a2b725b502d8 36 Rotatie = (Pulses*twopi)/4192;
Fernon 8:a2b725b502d8 37 Rotatietwopi = fmod(Rotatie,twopi);
Fernon 9:4743f3bb39b2 38
Fernon 9:4743f3bb39b2 39 pc.printf ("Potmeter = %f\n", z);return 0; pc.printf ("Rotatie = %f [radialen] \n", Rotatietwopi);
Fernon 9:4743f3bb39b2 40 // if (z > 0.05) {
Fernon 9:4743f3bb39b2 41 // Goal = pi;
Fernon 9:4743f3bb39b2 42 // }
Fernon 8:a2b725b502d8 43 Error = Goal-Rotatietwopi;
Fernon 8:a2b725b502d8 44 if (Error >= 0) {
Fernon 8:a2b725b502d8 45 Direction =0;
Fernon 8:a2b725b502d8 46 } else {
Fernon 9:4743f3bb39b2 47 Direction =1;
Fernon 8:a2b725b502d8 48 }
Fernon 8:a2b725b502d8 49 pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
Fernon 8:a2b725b502d8 50 v = K*fabs(Error);
Fernon 8:a2b725b502d8 51 PowerMotor.write(v);
Fernon 0:5a5f417fa1b2 52 }
Fernon 2:f0e9ffc5df09 53 }
Fernon 3:af3f0ed8c99e 54
Fernon 8:a2b725b502d8 55
Fernon 8:a2b725b502d8 56
Fernon 9:4743f3bb39b2 57
Fernon 9:4743f3bb39b2 58
Fernon 9:4743f3bb39b2 59
Fernon 9:4743f3bb39b2 60
Fernon 9:4743f3bb39b2 61
Fernon 9:4743f3bb39b2 62
Fernon 9:4743f3bb39b2 63
Fernon 9:4743f3bb39b2 64
Fernon 9:4743f3bb39b2 65
Fernon 9:4743f3bb39b2 66
Fernon 9:4743f3bb39b2 67
Fernon 9:4743f3bb39b2 68
Fernon 9:4743f3bb39b2 69
Fernon 9:4743f3bb39b2 70
Fernon 9:4743f3bb39b2 71
Fernon 9:4743f3bb39b2 72
Fernon 9:4743f3bb39b2 73
Fernon 9:4743f3bb39b2 74
Fernon 9:4743f3bb39b2 75
Fernon 9:4743f3bb39b2 76
Fernon 9:4743f3bb39b2 77
Fernon 9:4743f3bb39b2 78
Fernon 9:4743f3bb39b2 79
Fernon 9:4743f3bb39b2 80
Fernon 9:4743f3bb39b2 81
Fernon 9:4743f3bb39b2 82
Fernon 9:4743f3bb39b2 83
Fernon 9:4743f3bb39b2 84
Fernon 9:4743f3bb39b2 85
Fernon 9:4743f3bb39b2 86
Fernon 9:4743f3bb39b2 87
Fernon 9:4743f3bb39b2 88
Fernon 9:4743f3bb39b2 89
Fernon 9:4743f3bb39b2 90
Fernon 9:4743f3bb39b2 91
Fernon 9:4743f3bb39b2 92
Fernon 9:4743f3bb39b2 93
Fernon 9:4743f3bb39b2 94
Fernon 9:4743f3bb39b2 95
Fernon 9:4743f3bb39b2 96
Fernon 9:4743f3bb39b2 97
Fernon 9:4743f3bb39b2 98
Fernon 9:4743f3bb39b2 99
Fernon 9:4743f3bb39b2 100