Script met een paar limiet toevoegingen en reset van arm als die te ver door draait.
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of Samenvoegen6 by
Diff: main.cpp
- Revision:
- 2:de3bb38dae4e
- Parent:
- 1:0e1d91965b8e
diff -r 0e1d91965b8e -r de3bb38dae4e main.cpp --- a/main.cpp Tue Oct 20 09:29:32 2015 +0000 +++ b/main.cpp Wed Oct 21 11:32:48 2015 +0000 @@ -20,8 +20,8 @@ //AnalogIn pot(A2); //AnalogIn pot1(A3); -QEI wheel2 (D10, D11, NC, 64,QEI::X4_ENCODING); // Function for Encoder1 on the motor1 to the Microcontroller -QEI wheel1 (D12, D13, NC, 64,QEI::X4_ENCODING); // Function for Encoder2 on the motor2 to the Microcontroller +QEI wheel1 (D10, D11, NC, 64,QEI::X4_ENCODING); // Function for Encoder1 on the motor1 to the Microcontroller +QEI wheel2 (D12, D13, NC, 64,QEI::X4_ENCODING); // Function for Encoder2 on the motor2 to the Microcontroller //**************** OUTPUTS **************** DigitalOut red(LED_RED); // LED declared for calibration @@ -33,10 +33,10 @@ DigitalOut magnet(D2); -DigitalOut motor2direction(D4); // D4 en D5 zijn motor 2 (op het motorshield) -PwmOut motor2speed(D5); -DigitalOut motor1direction(D7); // D6 en D7 zijn motor 1 (op het motorshield) -PwmOut motor1speed(D6); +DigitalOut motor1direction(D4); // D4 en D5 zijn motor 2 (op het motorshield) +PwmOut motor1speed(D5); +DigitalOut motor2direction(D7); // D6 en D7 zijn motor 1 (op het motorshield) +PwmOut motor2speed(D6); //**************** FUNCTIONS ************** /* HIDScope scope(4); */ // HIDScope declared with 4 scopes @@ -101,23 +101,24 @@ bool flag_left = true; bool flag_right = true; float pos_board = 0; // The position of the board begins at zero +int pos_board1 = 0; float Encoderread2 = 0; //********* VARIABLES FOR CONTROL 3 ************ -const float minimum_contract = 0.2; // Certain levels for muscle activity to activate motor -const float medium_contract = 0.5; // "Medium contract muscle" +const float minimum_contract = 0.4; // Certain levels for muscle activity to activate motor +const float medium_contract = 0.6; // "Medium contract muscle" const float maximum_leftbiceps = 0.8; // "Maximum contract muscle" const float on = 1.0; const float off = 0.0; -const float minimum_throw_angle = -5; +const float minimum_throw_angle = 20; const float maximum_throw_angle = 110; float pos_arm = 0; int pos_arm1 = 0; float Encoderread1 = 0; int switch_rondjes = 2; int rondjes = 0; -float pos_armrondjes_max = 5; +float pos_armrondjes_max = 3; bool problem1 = false; bool problem2 = false; @@ -125,7 +126,8 @@ //********* VARIABLES FOR CONTROL 4 ************ float reset_positie = 0; // Belangrijk -float reset_board = 0; +int reset_arm = 0; +int reset_board = 0; float speedcompensation; float speedcompensation2; int t = 5; // aftellen naar nieuw spel @@ -153,6 +155,8 @@ double cali_array1[2560] = {}; // array to store values in double cali_array2[2560] = {}; +double cali_array3[512] = {}; + void hoog_laag_filter() { u1 = EMG1; @@ -296,9 +300,12 @@ if (flag_s) { Encoderread1 = wheel1.getPulses(); pos_arm = (Encoderread1/((64.0*131.0)/360.0)); // Omrekenen naar graden van arm + pos_arm1 = pos_arm; Encoderread2 = wheel2.getPulses(); pos_board = (Encoderread2/((64.0*131.0)/360.0)); // Omrekenen naar graden van board + pos_board1 = pos_board; + flag_calibration = false; } //************************* FASE SWITCH ****************************************** @@ -307,6 +314,9 @@ j++; wait(0.01); // Problemen met EMG metingen die te hoog zijn op het begin van script na calibratie. vandaar ff wachten en de sample loop een paar keer doorlopen. pc.printf("waarde j = %i \n",j); + led_rood.write(0); + led_groen.write(1); + led_geel.write(0); } if( j > N) { @@ -315,37 +325,31 @@ //******************* Fase 1 ************************** case(1): + led_rood.write(1); + led_groen.write(0); + led_geel.write(0); rondjes = 0; if (y1> contract) { flag_right = false; flag_left = true; - led_rood.write(0); - led_groen.write(0); - led_geel.write(1); + } if (y2 > contract) { flag_right = true; flag_left = false; - led_rood.write(0); - led_groen.write(0); - led_geel.write(1); + } if (pos_board < maxleft) { flag_left = false; motor2speed.write(relax); - led_rood.write(1); - led_groen.write(0); - led_geel.write(0); + } if (pos_board > maxright) { flag_right = false; motor2speed.write(relax); - led_rood.write(1); - led_groen.write(0); - led_geel.write(0); } if (flag_left == true) { @@ -370,14 +374,17 @@ motor2speed.write(relax); fase = 2; fase_switch_wait = true; + led_rood.write(0); + led_groen.write(0); + led_geel.write(1); j = 0; - led_rood.write(0); - led_groen.write(1); - led_geel.write(0); } break; //******************* Fase 2 ************************** case(2): + led_rood.write(0); + led_groen.write(0); + led_geel.write(1); motor1direction.write(cw); pos_arm1 = (pos_arm - (rondjes * 360)); @@ -396,30 +403,18 @@ if (y2 > minimum_contract & y2 < medium_contract) { motor1speed.write((y2 - minimum_contract)/(medium_contract - minimum_contract)); - led_rood.write(0); - led_groen.write(0); - led_geel.write(1); } if (y2 > medium_contract) { // Hoger dan drempelwaarde = Actief motor1speed.write(1); - led_rood.write(0); - led_groen.write(0); - led_geel.write(1); } if (y2 < minimum_contract) { // Lager dan drempel = Rust motor1speed.write(relax); - led_rood.write(0); - led_groen.write(1); - led_geel.write(0); } if( rondjes == pos_armrondjes_max) { // max aantal draaing gemaakt!!!!!! problem1 = true; problem2 = true; motor1speed.write(relax); - led_rood.write(1); - led_groen.write(0); - led_geel.write(0); while (problem1) { j++; @@ -444,29 +439,21 @@ led_rood.write(0); wait(0.1); led_rood.write(1); - } + wait(1.5); while(problem2) { - motor1speed.write(relax); - wait(1.5); - while(1) { - led_rood.write(0); - led_groen.write(1); - led_geel.write(0); - - motor1direction.write(0); - problem_velocity = 0.05 + (pos_arm/720)*0.4; - motor1speed.write(problem_velocity); + motor1direction.write(ccw); + motor1speed.write(0.02); if (flag_s) { Encoderread1 = wheel1.getPulses(); pos_arm = (Encoderread1/((64.0*131.0)/360.0)); // Omrekenen naar graden van arm } - if (pos_arm < 0) { - led_rood.write(0); - led_groen.write(1); - led_geel.write(0); + if (pos_arm < 10) { + problem2 = false; + motor1speed.write(0); + rondjes = 0; wait(1); } } @@ -478,10 +465,6 @@ fase = 3; pc.printf("Van fase 2 naar fase 3\n"); - led_rood.write(0); - led_groen.write(1); - led_geel.write(0); - wait(2); j = 0; fase_switch_wait = true; @@ -492,48 +475,50 @@ break; //********************************************* Fase 3 ********************************************** case(3): - + led_rood.write(0); + led_groen.write(1); + led_geel.write(0); switch(switch_reset) { case(1): if(pos_arm < reset_positie) { //ene kant op draaien motor1direction.write(cw); - speedcompensation2 = ((reset_positie - pos_arm)/900.0 + (0.1)); - motor1speed.write(speedcompensation2); - } - if(pos_arm > reset_positie) { //andere kant op - motor1direction.write(ccw); - speedcompensation2 = ((pos_arm - reset_positie)/500.0 + (0.1)); + speedcompensation2 = 0.05;//((reset_arm - pos_arm1)/900.0 + (0.02)); motor1speed.write(speedcompensation2); } - if(pos_arm < reset_positie+3 && pos_arm > reset_positie-3) { // Dit gaat niet goed komen, omdat het precies die waarde moet zijn - for(int cali_index1 = 0; cali_index1 < 2560; cali_index1++) - - motor1speed.write(0); - arm = true; - switch_reset = 2; - } + if(pos_arm > reset_positie) //andere kant op + { + motor1direction.write(ccw); + speedcompensation2 = 0.05;//((pos_arm1 - reset_arm)/500.0 + (0.02)); + motor1speed.write(speedcompensation2); + } + if(pos_arm < reset_positie+5 && pos_arm > reset_positie-5) // Dit gaat niet goed komen, omdat het precies die waarde moet zijn + { + motor1speed.write(0); + arm = true; + switch_reset = 2; + } pc.printf("Positie_arm = %f \t \t snelheid = %f \n",pos_arm, speedcompensation); wait(0.0001); break; case(2): pc.printf("\t switch magneet automatisch \n"); - wait(1); + wait(0.2); magnet.write(on); - wait(0.5); + wait(2); switch_reset = 3; break; - + case(3): if(pos_board < reset_board) { motor2direction.write(cw); - speedcompensation = ((reset_board - pos_board)/500.0 + (0.1)); + speedcompensation = 0.05; //((reset_board - pos_board1)/500.0 + (0.1)); motor2speed.write(speedcompensation); } if(pos_board > reset_board) { motor2direction.write(ccw); - speedcompensation = ((pos_board - reset_board)/500.0 + (0.1)); + speedcompensation = 0.05;//((pos_board1 - reset_board)/500.0 + (0.05)); motor2speed.write(speedcompensation); } if(pos_board < reset_board+5 && pos_board > reset_board-5) {