EMG driven robot which shoots elastic bands
Fork of ROBOBIRDS_FINAL by
Diff: main.cpp
- Revision:
- 39:e5bf4b1293fa
- Parent:
- 38:eaf245d88d84
- Child:
- 40:cac08f589732
diff -r eaf245d88d84 -r e5bf4b1293fa main.cpp --- a/main.cpp Thu Oct 29 09:34:20 2015 +0000 +++ b/main.cpp Thu Oct 29 09:45:12 2015 +0000 @@ -1,8 +1,6 @@ #include "mbed.h" #include "QEI.h" #include "math.h" -#include "HIDScope.h" - // Motor 1 & 2 @@ -25,13 +23,10 @@ Ticker biquadTicker; Ticker MotorWrite; -Ticker Sender; Timer timer; Timer TijdEMGCal; -// Debugging -Serial pc(USBTX, USBRX); -HIDScope scope(6); +// LED DigitalOut ledF(LED_RED); DigitalOut ledC(LED_GREEN); @@ -212,6 +207,10 @@ void MotorSet() { + Pulses = Encoder.getPulses(); + Rotatie = (Pulses*twopi)/4200; + Pulses2 = Encoder2.getPulses(); + Rotatie2 = (Pulses2*twopi)/4200; // Eerst motor 1 (translatie) if (OutRange) { Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal @@ -255,18 +254,6 @@ } PowerMotor2.write(fabs(v2)); } -void Send() -{ - Pulses = Encoder.getPulses(); - Rotatie = (Pulses*twopi)/4200; - Pulses2 = Encoder2.getPulses(); - Rotatie2 = (Pulses2*twopi)/4200; - scope.set(0,Rotatie); - scope.set(1,y8); - scope.set(2,Rotatie2); - scope.set(3,y18); - scope.send(); -} void Calibration () { @@ -298,23 +285,18 @@ int main() { - pc.printf("Welkom\n"); upperlimit = n1*twopi; turnlimit = n2*twopi; ledF = 1; ledC = 1; - pc.baud(115200); PowerMotor.write(0); PowerMotor2.write(0); - Sender.attach(Send,1/Fs); MotorWrite.attach(MotorSet,1/Fs); biquadTicker.attach(myController,1/Fs2); PowerServo.period_ms(20); while (true) { - pc.printf("grote While Loop\n"); Calibration(); while (Excecute) { - pc.printf("Rotatie = %f \nRotatie2 = %f \n", Rotatie, Rotatie2); // Eerst wordt motor 1 aangestuurd if (Rotatie >= upperlimit) { //Als hij buiten bereik is Goal = upperlimit - margin-0.2; @@ -376,7 +358,6 @@ PowerServo.write(0.04); ledF = 1; Fired=Fired+1; - pc.printf("Fire = %i", Fired); if (Fired == 4) { wait (1); Home = true; @@ -391,7 +372,6 @@ } while (Home) { //Terugkeren naar vaste positie - pc.printf("Home\n"); OutRange = true; //Hiermee wordt het PID gedeelte van de motor control aangestuurd. OutRange2 = true; Goal = margin;