Fernon Eijkhoudt
/
ROBOBIRDS_FINAL
EMG driven robot which shoots elastic bands
Fork of RoboBirdV1 by
Diff: main.cpp
- Revision:
- 8:a2b725b502d8
- Parent:
- 7:2042e359bfc3
- Child:
- 9:4743f3bb39b2
--- a/main.cpp Mon Sep 28 18:48:19 2015 +0000 +++ b/main.cpp Mon Sep 28 19:56:53 2015 +0000 @@ -1,5 +1,6 @@ #include "mbed.h" #include "QEI.h" +#include "math.h" DigitalOut Direction(D4); //1 = CCW - 0 = CW PwmOut PowerMotor(D5); //van 0 tot 1 @@ -9,21 +10,48 @@ Ticker Pot; double z=0; - +const double twopi = 6.2831853071795; +const double pi = twopi/2; +int Pulses; +double Rotatie; +double Rotatietwopi; +double Goal = 0; +double Error = 0; +double K = 2; +double v; void readpot() { z = PotMeter.read()/10; } + int main() { pc.baud(115200); PowerMotor.write(0); Pot.attach(readpot,0.1); // Deze ticker moet de waarde uitlezen van de PotMeter 10 keer per seconde + //PC.attach(print,0.5); while (true) { + Pulses = Encoder.getPulses(); + Rotatie = (Pulses*twopi)/4192; + Rotatietwopi = fmod(Rotatie,twopi); pc.printf ("Potmeter = %f\n", z); // het weergeven van de waarde waar z (PotMeter) op dat moment is - PowerMotor.write(z); + pc.printf ("Rotatie = %f [radialen] \n", Rotatietwopi); + if (z > 0.05) { + Goal = pi; + } + Error = Goal-Rotatietwopi; + if (Error >= 0) { + Direction =0; + } else { + Direction =1; + } + pc.printf("Error = %f\n Goal = %f\n", Error, Goal); + v = K*fabs(Error); + PowerMotor.write(v); } } + +