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:
- 3:f9a1df2271d2
- Parent:
- 2:de3bb38dae4e
- Child:
- 4:f9f75c913d7d
diff -r de3bb38dae4e -r f9a1df2271d2 main.cpp
--- a/main.cpp Wed Oct 21 11:32:48 2015 +0000
+++ b/main.cpp Fri Oct 23 08:24:34 2015 +0000
@@ -1,162 +1,149 @@
-//================================================================ Script: Ball-E ==================================================================
-// Author: Ewoud Velu, Lisa Verhoeven, Robert van der Wal, Thijs van der Wal, Emily Zoetbrood
+//======================================================================= Script: Ball-E ==========================================================================
+// Authors: Ewoud Velu, Lisa Verhoeven, Robert van der Wal, Thijs van der Wal, Emily Zoetbrood
/* This is the script of a EMG measurment moving robot. The purpose of the robot is to amuse people with the disease of Ducenne.
- The robot is designed to throw a ball in to a certain chosen pocket. In order to achieve this movement we use a ‘shoulder’ that can turn in the vertical plane
+ The robot is designed to throw a ball in to a certain chosen pocket.
+ In order to achieve this movement we use a ‘shoulder’ that can turn in the vertical plane and move in the horizontal plane.
*/
//******************************************** Library DECLARATION **************************************
-#include "mbed.h"
-#include "QEI.h"
-#include "MODSERIAL.h"
-#include "HIDScope.h"
-#include "biquadFilter.h"
-#include <cmath>
-#include <stdio.h>
+// Libraries are files which contain standard formulas for reading surtain information. Every library contains its own information.
+#include "mbed.h" // Standard library. This includes the reading of AnalogIn, DigitalOut, PwmOut and other standard formules.
+#include "QEI.h" // This library includes the reading of of the Encoders from motors.
+#include "MODSERIAL.h" // MODSERIAL inherits Serial and adds extensions for buffering.
+#include "HIDScope.h" //
+#include "biquadFilter.h" //
+#include <cmath> //
+#include <stdio.h> //
//******************************************* FUNCTION DECLA *******************************************
//**************** INPUTS *****************
-AnalogIn EMG1(A0); // Input EMG
-AnalogIn EMG2(A1); // Input EMG
-//AnalogIn pot(A2);
-//AnalogIn pot1(A3);
+AnalogIn EMG1(A0); // Input EMG
+AnalogIn EMG2(A1); // Input EMG
-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
+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
-DigitalOut green(LED_GREEN); // LED declared for sampling
+DigitalOut red(LED_RED); // LED declared for calibration
+DigitalOut green(LED_GREEN); // LED declared for sampling
-DigitalOut led_rood(D1);
-DigitalOut led_geel(D3);
-DigitalOut led_groen(D9);
+DigitalOut led_rood(D1);
+DigitalOut led_geel(D3);
+DigitalOut led_groen(D9);
-DigitalOut magnet(D2);
+DigitalOut magnet(D2);
-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);
+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
-MODSERIAL pc(USBTX, USBRX); // Function for Serial communication with the Microcontroller to the pc.
+MODSERIAL pc(USBTX, USBRX); // Function for Serial communication with the Microcontroller to the pc.
//******************************* GLOBAL VARIABLES DECLARATION ************************************
-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;
-
-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;
+const int led_on = 0; // Needed for the LEDs to turn on and off
+const int led_off = 1;
+const int relax = 0;
+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 ********
-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
-bool flag_m = false; // Var flag_m motor ticker
-float ain = 0;
+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
+bool flag_m = false; // Var flag_m motor ticker
+float ain = 0;
//********* VARIABLES FOR CONTROL 1 ************
volatile bool sample_go; // Ticker EMG function
-int Fs = 512; // Sampling frequency
-
-const double low_b1 = 1.480219865318266e-04; //Filter coefficients
-const double low_b2 = 2.960439730636533e-04;
-const double low_b3 = 1.480219865318266e-04;
-const double low_a2 = -1.965293372622690e+00; // a1 is normalized to 1
-const double low_a3 = 9.658854605688177e-01;
-const double high_b1 = 8.047897937631126e-01;
-const double high_b2 = -1.609579587526225e+00;
-const double high_b3 = 8.047897937631126e-01;
-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;
-
-int calibratie_metingen = 0;
+int Fs = 512; // Sampling frequency
+int calibratie_metingen = 0;
+const double low_b1 = 1.480219865318266e-04; //Filter coefficients
+const double low_b2 = 2.960439730636533e-04;
+const double low_b3 = 1.480219865318266e-04;
+const double low_a2 = -1.965293372622690e+00; // a1 is normalized to 1
+const double low_a3 = 9.658854605688177e-01;
+const double high_b1 = 8.047897937631126e-01;
+const double high_b2 = -1.609579587526225e+00;
+const double high_b3 = 8.047897937631126e-01;
+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 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_array3[512] = {};
//********* VARIABLES FOR FASE SWITCH **********
-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;
-float leftbiceps = ain;
-int reset = 0;
-int button_pressed = 0;
-int j = 1;
-int N = 200;
-bool fase_switch_wait = 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
+int reset = 0;
+int button_pressed = 0;
+int j = 1;
+int N = 200;
+bool fase_switch_wait = true;
//********* VARIABLES FOR 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
-int pos_board1 = 0;
-float Encoderread2 = 0;
-
+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
+int pos_board1 = 0;
+float Encoderread2 = 0;
//********* VARIABLES FOR CONTROL 3 ************
-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 = 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 = 3;
+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 = 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 = 3;
+bool problem1 = false;
+bool problem2 = false;
+float problem_velocity = 0;
-bool problem1 = false;
-bool problem2 = false;
-float problem_velocity = 0;
//********* VARIABLES FOR CONTROL 4 ************
-float reset_positie = 0; // Belangrijk
-int reset_arm = 0;
-int reset_board = 0;
+float reset_positie = 0;
+int reset_arm = 0;
+int reset_board = 0;
float speedcompensation;
float speedcompensation2;
-int t = 5; // aftellen naar nieuw spel
-int switch_reset = 1;
-bool arm = false;
-bool board1 = false;
-
+int t = 5; // aftellen naar nieuw spel
+int switch_reset = 1;
+bool arm = false;
+bool board1 = false;
// **************************************** Tickers *****************************************
-Ticker Sensor_control; // Adds ticker function for the variable function : Sensors
-Ticker Motor_control;
-Ticker EMG_Control;
+Ticker Sensor_control; // Adds ticker function for the variable function : Sensors
+Ticker Motor_control;
+Ticker EMG_Control;
//=============================================================================================== SUB LOOPS ==================================================================================================================
//**************************** CONTROL 1-EMG CALIBRATION ***********************************
-
biquadFilter highpass1(high_a2, high_a3, high_b1, high_b2, high_b3); // Different objects for different inputs, otherwise the v1 and v2 variables get fucked up
biquadFilter highpass2(high_a2, high_a3, high_b1, high_b2, high_b3);
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_array3[512] = {};
-
void hoog_laag_filter()
{
u1 = EMG1;
@@ -169,10 +156,17 @@
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()
{
+ 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_s = true;
}
@@ -183,14 +177,20 @@
void samplego() // ticker function, set flag to true every sample interval
{
- sample_go = 1;
+ if(flag_calibration == false)
+ {
+ red.write(led_off); // Toggles red calibration LED off
+ green.write(led_on); // Toggles green sampling LED on
+ hoog_laag_filter();
+ sample_go = 0;
+ sample_go = 1;
+ }
}
//================================================================================================== HEAD LOOP ================================================================================================================
int main()
{
-
pc.baud(115200);
Sensor_control.attach(&SENSOR_LOOP, 0.01); // TICKER FUNCTION
Motor_control.attach(&MOTOR_LOOP, 0.01);
@@ -208,25 +208,24 @@
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) {
- red.write(led_off); // Toggles red calibration LED off
- green.write(led_on); // Toggles green sampling LED on
- hoog_laag_filter();
+ while(1)
+ {
+ if(sample_go)
+ {
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 ++;
+ 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);
@@ -240,29 +239,34 @@
wait(0.2);
led_rood.write(1);
wait(1);
-
+
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
+ 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
+ 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) {
+ 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
+ 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];
}
@@ -290,27 +294,19 @@
j = 1; // Wachten van fase switch initialiseren
fase_switch_wait = true;
+ flag_calibration = false;
}
//************************* CONTROL MOTOR ****************************************
-
-
-
- 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;
-
+ if (flag_s)
+ {
flag_calibration = false;
}
//************************* FASE SWITCH ******************************************
//******************** Fase determination *************
- if (fase_switch_wait == true) {
+ 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);
@@ -319,58 +315,69 @@
led_geel.write(0);
}
- if( j > N) {
+ if( j > N)
+ {
fase_switch_wait = false;
- switch(fase) {
-
+ switch(fase)
+ {
//******************* Fase 1 **************************
case(1):
- led_rood.write(1);
- led_groen.write(0);
- led_geel.write(0);
+ led_rood.write(1);
+ led_groen.write(0);
+ led_geel.write(0);
rondjes = 0;
- if (y1> contract) {
+ if (y1> contract)
+ {
flag_right = false;
flag_left = true;
-
}
- if (y2 > contract) {
+ if (y2 > contract)
+ {
flag_right = true;
flag_left = false;
-
}
- if (pos_board < maxleft) {
+ if (pos_board < maxleft)
+ {
flag_left = false;
motor2speed.write(relax);
-
}
- if (pos_board > maxright) {
+ if (pos_board > maxright)
+ {
flag_right = false;
motor2speed.write(relax);
}
- if (flag_left == true) {
- if (y1> contract) {
+ if (flag_left == true)
+ {
+ if (y1> contract)
+ {
motor2direction.write(ccw);
motor2speed.write(speed_plate_1);
- } else {
+ }
+ else
+ {
motor2speed.write(relax);
}
}
- if (flag_right == true) {
- if (y2 > contract) {
+ if (flag_right == true)
+ {
+ if (y2 > contract)
+ {
motor2direction.write(cw);
motor2speed.write(speed_plate_1);
- } else {
+ }
+ 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) {
+ if (y1> fasecontract && y2 > fasecontract)
+ {
motor2speed.write(relax);
fase = 2;
fase_switch_wait = true;
@@ -382,51 +389,58 @@
break;
//******************* Fase 2 **************************
case(2):
- led_rood.write(0);
- led_groen.write(0);
- led_geel.write(1);
+ led_rood.write(0);
+ led_groen.write(0);
+ led_geel.write(1);
motor1direction.write(cw);
pos_arm1 = (pos_arm - (rondjes * 360));
- switch(switch_rondjes) {
+ switch(switch_rondjes)
+ {
case(1):
rondjes ++;
switch_rondjes = 2;
break;
case(2):
- if(pos_arm1>360 & 368<pos_arm1) {
+ 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));
}
- if (y2 > medium_contract) { // Hoger dan drempelwaarde = Actief
+ if (y2 > medium_contract) // Hoger dan drempelwaarde = Actief
+ {
motor1speed.write(1);
}
- if (y2 < minimum_contract) { // Lager dan drempel = Rust
+ if (y2 < minimum_contract) // Lager dan drempel = Rust
+ {
motor1speed.write(relax);
}
- if( rondjes == pos_armrondjes_max) { // max aantal draaing gemaakt!!!!!!
+ if(rondjes == pos_armrondjes_max) // max aantal draaing gemaakt!!!!!!
+ {
problem1 = true;
problem2 = true;
motor1speed.write(relax);
-
- while (problem1) {
+
+ 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;
+ if( j > N)
+ {
+ problem1 = false;
}
- }
-
+ }
+
wait(0.1);
led_rood.write(0);
wait(0.1);
@@ -441,16 +455,18 @@
led_rood.write(1);
wait(1.5);
- while(problem2) {
+ while(problem2)
+ {
motor1direction.write(ccw);
motor1speed.write(0.02);
-
- if (flag_s) {
+
+ if (flag_s)
+ {
Encoderread1 = wheel1.getPulses();
pos_arm = (Encoderread1/((64.0*131.0)/360.0)); // Omrekenen naar graden van arm
}
- if (pos_arm < 10) {
-
+ if (pos_arm < 10)
+ {
problem2 = false;
motor1speed.write(0);
rondjes = 0;
@@ -458,45 +474,48 @@
}
}
}
- 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");
-
+
wait(2);
j = 0;
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):
- led_rood.write(0);
- led_groen.write(1);
- led_geel.write(0);
- switch(switch_reset) {
+ 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
+ if(pos_arm < reset_positie) //ene kant op draaien
+ {
motor1direction.write(cw);
- speedcompensation2 = 0.05;//((reset_arm - pos_arm1)/900.0 + (0.02));
+ speedcompensation2 = 0.05; //((reset_arm - pos_arm1)/900.0 + (0.02));
motor1speed.write(speedcompensation2);
}
- 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;
- }
+ 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;
@@ -508,24 +527,30 @@
wait(2);
switch_reset = 3;
break;
-
+
case(3):
- if(pos_board < reset_board) {
+ if(pos_board < reset_board)
+ {
motor2direction.write(cw);
speedcompensation = 0.05; //((reset_board - pos_board1)/500.0 + (0.1));
motor2speed.write(speedcompensation);
}
- if(pos_board > reset_board) {
+ if(pos_board > reset_board)
+ {
motor2direction.write(ccw);
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) {
+
+ if(pos_board < reset_board+5 && pos_board > reset_board-5)
+ {
motor2speed.write(0);
board1 = true;
}
- if(board1 == true) {
+
+ if(board1 == true)
+ {
red=0;
pc.printf("fase switch naar 1\n\n");
board1 = false;
@@ -536,10 +561,11 @@
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) {
-
+ if(games_played1 == 3)
+ {
flag_calibration = true;
- while(t) {
+ while(t)
+ {
pc.printf("\tCalibratie begint over %i \n",t);
t--;
led_geel.write(1);
@@ -548,7 +574,8 @@
wait(0.5);
}
}
- while(t) {
+ while(t)
+ {
pc.printf("\tNieuw spel begint over %i \n",t);
t--;
led_geel.write(1);