Fernon Eijkhoudt
/
ROBOBIRDS_FINAL
EMG driven robot which shoots elastic bands
Fork of RoboBirdV1 by
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?
User | Revision | Line number | New 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 |