EMG driven robot which shoots elastic bands
Fork of ROBOBIRDS_FINAL by
Diff: main.cpp
- Revision:
- 12:dcf90cafb2a5
- Parent:
- 11:a9a23042fc9e
- Child:
- 13:c60942a1da8e
--- a/main.cpp Mon Sep 28 21:16:40 2015 +0000 +++ b/main.cpp Wed Sep 30 19:25:46 2015 +0000 @@ -1,6 +1,7 @@ #include "mbed.h" #include "QEI.h" #include "math.h" +#include "HIDScope.h" DigitalOut Direction(D4); //1 = CCW - 0 = CW PwmOut PowerMotor(D5); //van 0 tot 1 @@ -9,6 +10,7 @@ QEI Encoder(D13,D12,NC,32,QEI::X2_ENCODING); //Encoder Serial pc(USBTX, USBRX); Ticker Pot; +HIDScope scope(2); double z=0; //initiele waarde potmeter const double twopi = 6.2831853071795; @@ -32,7 +34,6 @@ z = PotMeter.read(); //uitlezen Potmeter of voor EMG bijvoorbeeld } - int main() { upperlimit = n*twopi; @@ -42,28 +43,31 @@ while (true) { if (Button == 0) { Excecute =! Excecute; + } + while (Excecute ) { + 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 = z*upperlimit; // Het doel waar hij naar toe moet + Error = Goal-Rotatieup; // De error die het motortje maakt ten opzichte van je Goal + Errorv = 0-v; + if (Error >= 0) { //Bepalen van de rotatie richting + Direction =0; + } else { + Direction =1; } - - while (Excecute ) { - 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 = z*upperlimit; // Het doel waar hij naar toe moet - Error = Goal-Rotatieup; // De error die het motortje maakt ten opzichte van je Goal - Errorv = 0-v; - if (Error >= 0) { //Bepalen van de rotatie richting - Direction =0; - } else { - Direction =1; - } - pc.printf("Error = %f\n Goal = %f\n", Error, Goal); - v = Kp*fabs(Error)+Kd*fabs(Errorv); // Snelheid van motor - PowerMotor.write(v); // Snelheid die naar de motor geschreven wordt (wordt alleen in een schaal van 0-1 weergegeven). (uit ervaring van Motor4_Setpoint loopt dit maar tussen 0-0.1). + pc.printf("Error = %f\n Goal = %f\n", Error, Goal); + v = Kp*fabs(Error)+Kd*fabs(Errorv); // Snelheid van motor + PowerMotor.write(v); // Snelheid die naar de motor geschreven wordt (wordt alleen in een schaal van 0-1 weergegeven). (uit ervaring van Motor4_Setpoint loopt dit maar tussen 0-0.1). + if (Error<=0.001 && Error>=-0.001) { + PowerMotor.write(0); + Excecute=false; } + scope.set(0,Goal); + scope.set(1,Rotatieup); + scope.send(); } } - - - +} \ No newline at end of file