Biorobotics10
With this script a Ball-E robot can be made and be operative for the use.
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of
Samenvoegen_7_5
by 
Biorobotics10
Diff: main.cpp
- Revision:
- 1:0e1d91965b8e
- Parent:
- 0:6616d722fed3
- Child:
- 2:de3bb38dae4e
diff -r 6616d722fed3 -r 0e1d91965b8e main.cpp
--- a/main.cpp Mon Oct 19 07:07:21 2015 +0000
+++ b/main.cpp Tue Oct 20 09:29:32 2015 +0000
@@ -25,10 +25,14 @@
//**************** OUTPUTS ****************
DigitalOut red(LED_RED); // LED declared for calibration
-DigitalOut green(LED_GREEN); // LED declared for sampling
+DigitalOut green(LED_GREEN); // LED declared for sampling
+
+DigitalOut led_rood(D1);
+DigitalOut led_geel(D3);
+DigitalOut led_groen(D9);
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)
@@ -42,13 +46,13 @@
const int led_on = 0; // Needed for the LEDs to turn on and off
const int led_off = 1;
const int relax = 0;
-const int graden = 360;
+const int graden = 360;
int games_played = -1; // -1 omdat het spel daar eerst loopt voor het spelen om alles na te checken
int games_played1 = 0;
bool flag_calibration = true;
-//********* VARIABLES FOR MOTOR CONTROL ********
+//********* VARIABLES FOR MOTOR CONTROL ********
const float cw = 1; // The number if: motor1 moves clock wise motor2 moves counterclockwise
const float ccw = 0; // The number if: motor1 moves counterclock wise motor2 moves clockwise
bool flag_s = false; // Var flag_s sensor ticker
@@ -70,13 +74,15 @@
const double high_a2 = -1.571102440190402e+00; // a1 is normalized to 1
const double high_a3 = 6.480567348620491e-01;
-double u1; double y1; // Declaring the input variables
-double u2; double y2;
+double u1;
+double y1; // Declaring the input variables
+double u2;
+double y2;
int calibratie_metingen = 0;
//********* VARIABLES FOR FASE SWITCH **********
-bool begin = true;
+bool begin = true;
int fase = 3; // To switch between different phases and begins with the reset phase
const float fasecontract = 0.7; // The value the EMG signal must be for fase change
float rightbiceps = y2;
@@ -88,13 +94,13 @@
bool fase_switch_wait = true;
//********* VARIABLES FOR CONTROL 2 ************
-const float contract = 0.2; // The minimum value for which the motor moves
-const float maxleft = -1000; // With this angle the motor should stop moving
-const float maxright = 1000; // With this angle the motor should stop moving
-const float speed_plate_1 = 0.4; // The speed of the plate in control 2
+const float contract = 0.5; // The minimum value for which the motor moves
+const float maxleft = -200; // With this angle the motor should stop moving
+const float maxright = 200; // With this angle the motor should stop moving
+const float speed_plate_1 = 0.1; // The speed of the plate in control 2
bool flag_left = true;
bool flag_right = true;
-float pos_board = 0; // The position of the board begins at zero
+float pos_board = 0; // The position of the board begins at zero
float Encoderread2 = 0;
@@ -111,8 +117,12 @@
float Encoderread1 = 0;
int switch_rondjes = 2;
int rondjes = 0;
+float pos_armrondjes_max = 5;
-//********* VARIABLES FOR CONTROL 4 ************
+bool problem1 = false;
+bool problem2 = false;
+float problem_velocity = 0;
+//********* VARIABLES FOR CONTROL 4 ************
float reset_positie = 0; // Belangrijk
float reset_board = 0;
@@ -126,7 +136,7 @@
// **************************************** Tickers *****************************************
Ticker Sensor_control; // Adds ticker function for the variable function : Sensors
-Ticker Motor_control;
+Ticker Motor_control;
Ticker EMG_Control;
//=============================================================================================== SUB LOOPS ==================================================================================================================
@@ -137,24 +147,24 @@
biquadFilter lowpass1(low_a2, low_a3, low_b1, low_b2, low_b3);
biquadFilter lowpass2(low_a2, low_a3, low_b1, low_b2, low_b3);
-
+
double cali_fact1 = 0.9; // calibration factor to normalize filter output to a scale of 0 - 1
double cali_fact2 = 0.9;
double cali_array1[2560] = {}; // array to store values in
-double cali_array2[2560] = {};
+double cali_array2[2560] = {};
-void hoog_laag_filter()
+void hoog_laag_filter()
{
- u1 = EMG1;
+ u1 = EMG1;
u2 = EMG2;
- y1 = highpass1.step(u1);
+ y1 = highpass1.step(u1);
y2 = highpass2.step(u2); // filter order is: high-pass --> rectify --> low-pass
- y1 = fabs(y1);
- y2 = fabs(y2);
+ y1 = fabs(y1);
+ y2 = fabs(y2);
y1 = lowpass1.step(y1)*cali_fact1;
y2 = lowpass2.step(y2)*cali_fact2; // roughly normalize to a scale of 0 - 1, where 0 is minimum and 1 is roughly the maximum output.
}
-
+
//======================================= TICKER LOOPS ==========================================
void SENSOR_LOOP()
@@ -167,20 +177,25 @@
flag_m = true;
}
-void samplego() { // ticker function, set flag to true every sample interval
+void samplego() // ticker function, set flag to true every sample interval
+{
sample_go = 1;
}
//================================================================================================== HEAD LOOP ================================================================================================================
-int main()
+int main()
{
-
+
pc.baud(115200);
Sensor_control.attach(&SENSOR_LOOP, 0.01); // TICKER FUNCTION
Motor_control.attach(&MOTOR_LOOP, 0.01);
EMG_Control.attach(&samplego, (float)1/Fs);
-
+
+ led_groen.write(0);
+ led_geel.write(0);
+ led_rood.write(0);
+
pc.printf("===============================================================\n");
pc.printf(" \t\t\t Ball-E\n");
pc.printf("In the module Biorobotics on the University of Twente is this script created\n");
@@ -189,315 +204,387 @@
pc.printf("The script will begin with a short calibration\n\n");
wait(2.5);
pc.printf("===============================================================\n");
+
//************************* CONTROL 1-EMG CALIBRATION ****************************
- while(1)
- {
-
- if(sample_go)
- {
+ while(1) {
+
+ if(sample_go) {
red.write(led_off); // Toggles red calibration LED off
green.write(led_on); // Toggles green sampling LED on
hoog_laag_filter();
sample_go = 0;
- }
-
- if (flag_calibration)
- { // function to calibrate the emg signals from the user. It takes 5 seconds of measurements of maximum output, then takes the max and normalizes to that.
- calibratie_metingen ++;
- green.write(led_off); // Toggles green sampling LED off
- red.write(led_on); //Toggles red calibration LED on
- cali_fact1 = 0.9; // calibration factor to normalize filter output to a scale of 0 - 1
- cali_fact2 = 0.9;
- double cali_max1 = 0; // declare max y1
- double cali_max2 = 0; //declare max y2
- pc.printf(" \n\n EMG Signal starting up Calibration measurement........... \n");
- wait(2);
- pc.printf("\t.....Calibrating Signal of EMG 1 and 2 .......\n");
- wait(0.5);
- pc.printf("\t......contract muscles..... \n");
-
- for(int cali_index1 = 0; cali_index1 < 2560; cali_index1++) // Records 5 seconds of y1
- {
- hoog_laag_filter();
- cali_array1[cali_index1] = y1;
- wait((float)1/Fs);
- }
- for(int cali_index2 = 0; cali_index2 < 2560; cali_index2++) // Records 5 seconds of y2
- {
- hoog_laag_filter();
- cali_array2[cali_index2] = y2;
- wait((float)1/Fs);
- }
- for(int cali_index3 = 0; cali_index3 < 2560; cali_index3++) // Scales y1
- {
- if(cali_array1[cali_index3] > cali_max1)
- {
- cali_max1 = cali_array1[cali_index3];
- }
- }
- for(int cali_index4 = 0; cali_index4 < 2560; cali_index4++) // Scales y2
- {
- if(cali_array2[cali_index4] > cali_max2)
- {
- cali_max2 = cali_array2[cali_index4];
- }
- }
- cali_fact1 = (double)1/cali_max1; // Calibration factor for y1
- cali_fact2 = (double)1/cali_max2; // Calibration factor for y2
-
- // Toggles green sampling LED off
- red.write(led_off);
- green.write(led_on);
- pc.printf(" \t....... Calibration has been completed!\n");
- wait(0.2);
- green.write(led_off);
- wait(0.2);
- green.write(led_on);
- wait(0.2);
- green.write(led_off);
- wait(0.2);
- green.write(led_on);
- wait(4);
- pc.printf("Beginning with Ball-E board settings\n");
- green.write(led_off);
- wait(2);
- y1 = 0;
- y2 = 0;
-
- j = 1; // Wachten van fase switch initialiseren
- fase_switch_wait = true;
- }
-
-
- //************************* CONTROL MOTOR ****************************************
-
-
-
- if (flag_s)
- {
- Encoderread1 = wheel1.getPulses();
- pos_arm = (Encoderread1/((64.0*131.0)/360.0)); // Omrekenen naar graden van arm
-
- Encoderread2 = wheel2.getPulses();
- pos_board = (Encoderread2/((64.0*131.0)/360.0)); // Omrekenen naar graden van board
- flag_calibration = false;
- }
- //************************* FASE SWITCH ******************************************
- //******************** Fase determination *************
- if (fase_switch_wait == true)
- {
- 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);
- }
-
- if( j > N){
- fase_switch_wait = false;
- switch(fase)
- {
+ }
+
+ if (flag_calibration) {
+ // function to calibrate the emg signals from the user. It takes 5 seconds of measurements of maximum output, then takes the max and normalizes to that.
+ calibratie_metingen ++;
+ cali_fact1 = 0.9; // calibration factor to normalize filter output to a scale of 0 - 1
+ cali_fact2 = 0.9;
+ double cali_max1 = 0; // declare max y1
+ double cali_max2 = 0; //declare max y2
+ pc.printf(" \n\n EMG Signal starting up Calibration measurement........... \n");
+ wait(2);
+ led_rood.write(0);
+ wait(0.2);
+ led_rood.write(1); //Toggles red calibration LED on
+ wait(0.2);
+ led_rood.write(0);
+ wait(0.2);
+ led_rood.write(1);
+ wait(0.2);
+ led_rood.write(0);
+ wait(0.2);
+ led_rood.write(1);
+ wait(1);
- //******************* Fase 1 **************************
- case(1):
- rondjes = 0;
- if (y1> contract)
- {
- flag_right = false;
- flag_left = true;
+ pc.printf("\t.....Calibrating Signal of EMG 1 and 2 .......\n");
+ led_rood.write(0);
+ led_geel.write(1);
+ wait(0.5);
+ pc.printf("\t......contract muscles..... \n");
+
+ for(int cali_index1 = 0; cali_index1 < 2560; cali_index1++) { // Records 5 seconds of y1
+ hoog_laag_filter();
+ cali_array1[cali_index1] = y1;
+ wait((float)1/Fs);
+ }
+ for(int cali_index2 = 0; cali_index2 < 2560; cali_index2++) { // Records 5 seconds of y2
+ hoog_laag_filter();
+ cali_array2[cali_index2] = y2;
+ wait((float)1/Fs);
+ }
+ for(int cali_index3 = 0; cali_index3 < 2560; cali_index3++) { // Scales y1
+ if(cali_array1[cali_index3] > cali_max1) {
+ cali_max1 = cali_array1[cali_index3];
}
-
- if (y2 > contract)
- {
- flag_right = true;
- flag_left = false;
- }
-
- if (pos_board <= maxleft)
- {
- flag_left = false;
- motor2speed.write(relax);
+ }
+ for(int cali_index4 = 0; cali_index4 < 2560; cali_index4++) { // Scales y2
+ if(cali_array2[cali_index4] > cali_max2) {
+ cali_max2 = cali_array2[cali_index4];
}
-
- if (pos_board >= maxright)
- {
- flag_right = false;
- motor2speed.write(relax);
- }
+ }
+ cali_fact1 = (double)1/cali_max1; // Calibration factor for y1
+ cali_fact2 = (double)1/cali_max2; // Calibration factor for y2
+
+ // Toggles green sampling LED off
+ led_geel.write(0);
+ pc.printf(" \t....... Calibration has been completed!\n");
+ wait(0.2);
+ led_groen.write(led_off);
+ wait(0.2);
+ led_groen.write(led_on);
+ wait(0.2);
+ led_groen.write(led_off);
+ wait(0.2);
+ led_groen.write(led_on);
+ wait(4);
+ pc.printf("Beginning with Ball-E board settings\n");
+ led_groen.write(led_off);
+ wait(2);
+ y1 = 0;
+ y2 = 0;
+
+ j = 1; // Wachten van fase switch initialiseren
+ fase_switch_wait = true;
+ }
+
+
+ //************************* CONTROL MOTOR ****************************************
+
+
+
+ if (flag_s) {
+ Encoderread1 = wheel1.getPulses();
+ pos_arm = (Encoderread1/((64.0*131.0)/360.0)); // Omrekenen naar graden van arm
- if (flag_left == true)
- {
- if (y1> contract)
- {
- motor2direction.write(ccw);
- motor2speed.write(speed_plate_1);
+ Encoderread2 = wheel2.getPulses();
+ pos_board = (Encoderread2/((64.0*131.0)/360.0)); // Omrekenen naar graden van board
+ flag_calibration = false;
+ }
+ //************************* FASE SWITCH ******************************************
+ //******************** Fase determination *************
+ if (fase_switch_wait == true) {
+ 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);
+ }
+
+ if( j > N) {
+ fase_switch_wait = false;
+ switch(fase) {
+
+ //******************* Fase 1 **************************
+ case(1):
+ rondjes = 0;
+ if (y1> contract) {
+ flag_right = false;
+ flag_left = true;
+ led_rood.write(0);
+ led_groen.write(0);
+ led_geel.write(1);
}
- else
- {
- motor2speed.write(relax);
+
+ if (y2 > contract) {
+ flag_right = true;
+ flag_left = false;
+ led_rood.write(0);
+ led_groen.write(0);
+ led_geel.write(1);
}
- }
-
- if (flag_right == true)
- {
- if (y2 > contract)
- {
- motor2direction.write(cw);
- motor2speed.write(speed_plate_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);
}
- else
- {
- motor2speed.write(relax);
+
+ if (flag_left == true) {
+ if (y1> contract) {
+ motor2direction.write(ccw);
+ motor2speed.write(speed_plate_1);
+ } else {
+ motor2speed.write(relax);
+ }
+ }
+
+ if (flag_right == true) {
+ if (y2 > contract) {
+ motor2direction.write(cw);
+ motor2speed.write(speed_plate_1);
+ } else {
+ motor2speed.write(relax);
+ }
}
- }
- pc.printf("Boardposition \t %f EMG1 en EMG2 signaal = %f \t %f\n", pos_board, y1, y2);
- if (y1> fasecontract && y2 > fasecontract)
- {
- motor2speed.write(relax);
- fase = 2;
- fase_switch_wait = true;
- j = 0;
- }
- break;
- //******************* Fase 2 **************************
- case(2):
- motor1direction.write(cw);
- pos_arm1 = (pos_arm - (rondjes * 360));
-
- switch(switch_rondjes){
+ pc.printf("Boardposition \t %f EMG1 en EMG2 signaal = %f \t %f\n", pos_board, y1, y2);
+ if (y1> fasecontract && y2 > fasecontract) {
+ motor2speed.write(relax);
+ fase = 2;
+ fase_switch_wait = true;
+ j = 0;
+ led_rood.write(0);
+ led_groen.write(1);
+ led_geel.write(0);
+ }
+ break;
+ //******************* Fase 2 **************************
+ case(2):
+ motor1direction.write(cw);
+ pos_arm1 = (pos_arm - (rondjes * 360));
+
+ switch(switch_rondjes) {
case(1):
- rondjes ++;
- switch_rondjes = 2;
- break;
+ rondjes ++;
+ switch_rondjes = 2;
+ break;
case(2):
- if(pos_arm1>360 & 368<pos_arm1)
- {
- switch_rondjes = 1;
- }
- break;
+ if(pos_arm1>360 & 368<pos_arm1) {
+ switch_rondjes = 1;
+ }
+ break;
}
-
-
- if (y2 > minimum_contract & y2 < medium_contract)
- {
+
+
+ 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 (y2 > medium_contract) // Hoger dan drempelwaarde = Actief
- {
- motor1speed.write((y2 - minimum_contract)/(medium_contract - minimum_contract));
+
+ 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++;
+ 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.
+ Encoderread1 = wheel1.getPulses();
+ pos_arm = (Encoderread1/((64.0*131.0)/360.0)); // Omrekenen naar graden van arm
+
+ if( j > N) {
+ problem1 = false;
+ }
+ }
+
+ wait(0.1);
+ led_rood.write(0);
+ wait(0.1);
+ led_rood.write(1);
+ wait(0.1);
+ led_rood.write(0);
+ wait(0.1);
+ led_rood.write(1);
+ wait(0.1);
+ led_rood.write(0);
+ wait(0.1);
+ led_rood.write(1);
}
- if (y2 < minimum_contract) // Lager dan drempel = Rust
- {
- motor1speed.write(relax);
+
+ 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);
+
+ 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);
+ problem2 = false;
+ wait(1);
+ }
+ }
}
- if (pos_arm1 > minimum_throw_angle & pos_arm1 < maximum_throw_angle)
- {
- if (y1> maximum_leftbiceps)
- {
+ if (pos_arm1 > minimum_throw_angle & pos_arm1 < maximum_throw_angle) {
+ if (y1> maximum_leftbiceps) {
magnet.write(off);
motor1speed.write(relax);
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;
- }
+ fase_switch_wait = true;
+
}
- pc.printf("Armposition \t %i \t EMG1 en EMG2 = %f \t %f \n", pos_arm1, y1, y2);
- break;
- //********************************************* Fase 3 **********************************************
- case(3):
-
- 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));
- 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);
+ }
+ pc.printf("Armposition \t %i \t EMG1 en EMG2 = %f \t %f \n", pos_arm1, y1, y2);
break;
-
- case(2):
- pc.printf("\t switch magneet automatisch \n");
- wait(1);
- magnet.write(on);
- wait(0.5);
- switch_reset = 3;
- break;
-
- case(3):
- if(pos_board < reset_board)
- {
- motor2direction.write(cw);
- speedcompensation = ((reset_board - pos_board)/500.0 + (0.1));
- motor2speed.write(speedcompensation);
- }
-
- if(pos_board > reset_board)
- {
- motor2direction.write(ccw);
- speedcompensation = ((pos_board - reset_board)/500.0 + (0.1));
- motor2speed.write(speedcompensation);
- }
- if(pos_board < reset_board+5 && pos_board > reset_board-5)
- {
- motor2speed.write(0);
- board1 = true;
- }
- if(board1 == true)
- {
- red=0;
- pc.printf("fase switch naar 1\n\n");
- board1 = false;
- arm = false;
- // flag_calibration = true; !!!!! Moet naar gekeken worden of we elke spel willen calibreren
- wait(2);
- games_played ++;
- games_played1 = games_played - (3*calibratie_metingen - 3);
- pc.printf("Games played total count = %i and loop %i\n",games_played,games_played1);
-
- if(games_played1 == 3){
+ //********************************************* Fase 3 **********************************************
+ case(3):
+
+ 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));
+ 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;
+ }
+ 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);
+ magnet.write(on);
+ wait(0.5);
+ switch_reset = 3;
+ break;
+
+ case(3):
+ if(pos_board < reset_board) {
+ motor2direction.write(cw);
+ speedcompensation = ((reset_board - pos_board)/500.0 + (0.1));
+ motor2speed.write(speedcompensation);
+ }
- flag_calibration = true;
- while(t){
- pc.printf("\tCalibratie begint over %i \n",t);
- t--;
- wait(1);
+ if(pos_board > reset_board) {
+ motor2direction.write(ccw);
+ speedcompensation = ((pos_board - reset_board)/500.0 + (0.1));
+ motor2speed.write(speedcompensation);
}
+ if(pos_board < reset_board+5 && pos_board > reset_board-5) {
+ motor2speed.write(0);
+ board1 = true;
}
- while(t){
- pc.printf("\tNieuw spel begint over %i \n",t);
- t--;
- wait(1);
- }
-
+ if(board1 == true) {
+ red=0;
+ pc.printf("fase switch naar 1\n\n");
+ board1 = false;
+ arm = false;
+ // flag_calibration = true; !!!!! Moet naar gekeken worden of we elke spel willen calibreren
+ wait(2);
+ games_played ++;
+ games_played1 = games_played - (3*calibratie_metingen - 3);
+ pc.printf("Games played total count = %i and loop %i\n",games_played,games_played1);
+
+ if(games_played1 == 3) {
+
+ flag_calibration = true;
+ while(t) {
+ pc.printf("\tCalibratie begint over %i \n",t);
+ t--;
+ led_geel.write(1);
+ wait(0.5);
+ led_geel.write(0);
+ wait(0.5);
+ }
+ }
+ while(t) {
+ pc.printf("\tNieuw spel begint over %i \n",t);
+ t--;
+ led_geel.write(1);
+ wait(0.5);
+ led_geel.write(0);
+ wait(0.5);
+ }
+
fase = 1; // Terug naar fase 1
switch_reset = 1; // De switch op orginele locatie zetten
t = 5;
-
+
+ }
+
+ pc.printf("Positie_board = %f \t \t snelheid = %f \n",pos_board, speedcompensation);
+ wait(0.0001);
+ break;
}
-
- pc.printf("Positie_board = %f \t \t snelheid = %f \n",pos_board, speedcompensation);
- wait(0.0001);
- break;
- }
- break;
- //=================================================== STOP SCRIPT ============================================================
+ break;
+ //=================================================== STOP SCRIPT ============================================================
}
- }
+ }
}
}
\ No newline at end of file