Alles in 1
Dependencies: Encoder HIDScope MODSERIAL QEI mbed
Fork of RoboBird3 by
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?
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 | 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 |