emg zooi er op
Dependencies: HIDScope MODSERIAL QEI mbed
Fork of Wearealltogheter_goed by
Diff: main.cpp
- Revision:
- 23:fdde3e4b9e69
- Parent:
- 22:ad85b8acf8b5
- Child:
- 24:bdd74b91abbb
diff -r ad85b8acf8b5 -r fdde3e4b9e69 main.cpp --- a/main.cpp Mon Oct 24 14:45:10 2016 +0000 +++ b/main.cpp Tue Oct 25 10:47:41 2016 +0000 @@ -1,12 +1,52 @@ #include "mbed.h" #include "HIDScope.h" #include "MODSERIAL.h" +#include "QEI.h" + +DigitalOut led_g(LED_GREEN); +DigitalOut led_b(LED_BLUE); +DigitalOut led_r(LED_RED); + +DigitalOut M1_Rotate(D2); // voltage only base rotation +PwmOut M1_Speed(D3); // voltage only base rotation + +MODSERIAL pc(USBTX, USBRX); + +//QEI wheel(PinName channelA, PinName channelB, PinName index, int pulsesPerRev, Encoding encoding=X2_ENCODING) +QEI motor2(D10,D11,NC,8400,QEI::X4_ENCODING); +QEI motor3(D12,D13,NC,8400,QEI::X4_ENCODING); + +DigitalOut M2_Rotate(D4); // encoder side pot 2 translation +PwmOut M2_Speed(D5); // encoder side pot 2 translation + +DigitalOut M3_Rotate(D7); // encoder side pot 1 spatel rotation +PwmOut M3_Speed(D6); // encoder side pot 1 spatel rotation + +DigitalIn links(SW3); +DigitalIn rechts(SW2); + +AnalogIn pot1(A4); // pot 1 motor 1 +AnalogIn pot2(A3); // pot 2 motor 3 //Define objects AnalogIn emg0( A0 ); AnalogIn emg1( A1 ); -DigitalIn buttonCalibrate(SW2); -MODSERIAL pc(USBTX, USBRX); +DigitalIn buttonCalibrate(D9); + +bool draairechts; +bool draailinks; +bool turn = 0; +float waiter = 0.1; +float afstand = 200; +float translation = 0; +float degrees3 = 0; + +float Puls_degree = (8400/360); +float wheel1 = 16; +float wheel2 = 31; +float wheel3 = 41; +float overbrenging = ((wheel2/wheel1)*(wheel3/wheel1)); +float pi = 3.14159265359; volatile float x; volatile float x_prev =0; @@ -16,6 +56,7 @@ double threshold_Left = 0; double threshold_Right= 0; Ticker sample_timer; +Ticker sample_timer2; HIDScope scope( 2 ); DigitalOut led(LED1); const double a1 = -1.6475; @@ -94,14 +135,117 @@ led = !led; pc.printf("%f, %f \n \r ", x, b); } +void GetDirections() +{ + pc.baud(115200); + if ((rechts == 0) && (links == 0) && (turn == 0)) { + draailinks = 0; + draairechts = 0; + turn = 1; + pc.printf("begin de actie \n \r "); + wait(waiter); + } else if ((rechts == 0) && (links == 0) && (turn == 1)) { + draailinks = 0; + draairechts = 0; + turn = 0; + pc.printf("breek de actie af \n \r "); + wait(waiter); + } else if ((rechts == 1) && (links == 1)&& (turn == 0)) { + + } else if ((rechts == 1) && (draailinks == 0)&& (turn == 0)) { + /* if the right button is pressed and the motor isn't rotating to the left, + then start rotating to the right etc*/ + draairechts = !draairechts; + pc.printf("draai naar rechts \n \r "); + wait(waiter); + } else if ((rechts == 1) && (draailinks == 1)&& (turn == 0)) { + draailinks = 0; + draairechts = !draairechts; + pc.printf("draai naar rechts na links \n \r "); + wait(waiter); + } else if ((links == 1) && (draairechts == 0)&& (turn == 0)) { + draailinks = !draailinks; + pc.printf("draai naar links \n \r "); + wait(waiter); + } else if ((links == 1) && (draairechts == 1) && (turn == 0)) { + draairechts = 0; + draailinks = !draailinks; + pc.printf("draai naar links na rechts \n \r "); + wait(waiter); + } + wait(2*waiter); +} + +float GetPositionM2() +{ + float pulses2 = motor2.getPulses(); + float degrees2 = (pulses2/Puls_degree); + float radians2 = (degrees2/360)*2*pi; + float translation = ((radians2/overbrenging)*32.25); + + return translation; +} +float GetRotationM3() +{ + float pulses3 = motor3.getPulses(); + float degrees3 = (pulses3/Puls_degree); + float radians3 = (degrees3/360)*2*pi; + + return degrees3; +} +void GoBack() +{ + while (GetPositionM2() > 0) { + M3_Speed = 0; + M2_Speed = 1; + M2_Rotate = 0; + pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2()); + led_r = 0; + } + M2_Speed = 0; + + + while (GetRotationM3() > 0) { + M3_Rotate = 1; + M3_Speed = 0.2; + led_r = 1; + led_b = 0; + pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2()); + + } + M3_Speed = 0; + + turn = 0; + + +} +void Burgerflip() +{ + if (GetPositionM2() > afstand) { + M3_Speed = 0.2; + M3_Rotate = 0; + M2_Speed = 0; + } else if (GetPositionM2() < afstand) { + M2_Speed = 1; + M2_Rotate = 1; + + } + if (GetRotationM3() > 150) { + GoBack(); + } +} int main() { + led_g = 1; + led_b = 1; + led_r = 1; + /**Attach the 'sample' function to the timer 'sample_timer'. * this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz */ //sample_timer.attach(&sample, 0.001953125); - sample_timer.attach(&filterSampleLeft, 0.001953125); //512 Hz + sample_timer2.attach(&filterSampleLeft, 0.001953125); //512 Hz sample_timer.attach(&filterSampleRight, 0.001953125); pc.baud(115200); pc.printf("please push the button to calibrate \n \r"); @@ -110,9 +254,32 @@ calibrate = true; threshold_Left = lowpassFilterLeft*0.7; threshold_Right = lowpassFilterRight*0.7; + } if (calibrate == true) { pc.printf("calibration complete, left = %f, right = %f \n \r", threshold_Left, threshold_Right); + GetDirections(); + if (draairechts == true) { + M1_Speed = 0.2; + M1_Rotate = 0; + } else if (draailinks == true) { + M1_Speed = 0.2; + M1_Rotate = 1; + } else if (turn == 1) { + /*M2_Speed = 0.5; + M2_Rotate = 1; + M3_Speed = 0.5; + M3_Rotate = 1;*/ + Burgerflip(); + } else if (turn == 0) { + M2_Speed = 0; + M3_Speed = 0; + } + if ((draailinks == false) && (draairechts == false)) { + M1_Speed = 0; + } + pc.printf("rotation %f translation %f \n \r ", GetRotationM3(), GetPositionM2()); + /* pulses = 8400 */ /*empty loop, sample() is executed periodically*/ } }