Fernon Eijkhoudt
/
ROBOBIRDS_FINAL
EMG driven robot which shoots elastic bands
Fork of RoboBirdV1 by
Diff: main.cpp
- Revision:
- 16:b0ec8e6a8ad4
- Parent:
- 15:f7e2b20f4dca
- Child:
- 17:c5eea26e171d
--- a/main.cpp Wed Oct 07 17:24:39 2015 +0000 +++ b/main.cpp Wed Oct 07 18:17:58 2015 +0000 @@ -1,24 +1,24 @@ #include "mbed.h" -#include "encoder.h" +#include "QEI.h" #include "math.h" #include "HIDScope.h" DigitalOut Direction(D4); //1 = CCW - 0 = CW PwmOut PowerMotor(D5); //van 0 tot 1 PwmOut PowerServo(D7); -DigitalIn Button(D3); -DigitalIn Button2(D2); +DigitalIn Button(PTC6); +//DigitalIn Button2(D2); AnalogIn PotMeter(A1); -Encoder MotorEncoder(D12,D13); //Encoder +QEI Encoder(D13,D12,NC,32,QEI::X2_ENCODING); //Encoder Serial pc(USBTX, USBRX); Ticker MotorWrite; Ticker Sender; +Timer timer; HIDScope scope(3); double z=0; //initiele waarde potmeter const double twopi = 6.2831853071795; -const double pi = twopi/2; int Pulses; double Rotatie; //aantal graden dat de motor is gedraaid double Rotatieup=0; //aantal graden dat de motor is gedraaid in een bereik van n*pi @@ -47,33 +47,37 @@ scope.set(0,Goal); scope.set(1,Rotatieup); scope.set(2,Error); -// scope.set(2,v); - scope.send(); - } + scope.send(); +} int main() { upperlimit = n*twopi; pc.baud(115200); PowerMotor.write(0); - Sender.attach(Send,1/Fs/2); + Sender.attach(Send,0.5/Fs); MotorWrite.attach(MotorSet,1/Fs); // Deze ticker moet de waarde uitlezen van de PotMeter Fs keer per seconde while (true) { if (Button == 0) { Excecute =! Excecute; - } + } while (Excecute ) { - Pulses = MotorEncoder.getPosition(); + Pulses = Encoder.getPulses(); Rotatie = (Pulses*twopi)/4192; // Aantal graden draaien in radialen Rotatieup = fmod(Rotatie,upperlimit); //Aantal graden draaien in radialen binnen het bereik van upperlimit - pc.printf ("Potmeter = %f\n", z); pc.printf ("Rotatie = %f [radialen] \n", Rotatieup); Goal = PotMeter.read()*upperlimit; // Het doel waar hij naar toe moet Error = Goal-Rotatieup; // De error die het motortje maakt ten opzichte van je Goal pc.printf("Error = %f\n Goal = %f\n", Error, Goal); - if (Error>=-0.001 && Error<=0.001) { + if (fabs(Error)<=0.15) { + timer.start(); + } else { + timer.stop(); + timer.reset(); + } + if (timer.read() >= 5) { Excecute=false; - v=0; + Goal = 0; } } } -} \ No newline at end of file +}