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:
- 5:0597358d0016
- Parent:
- 4:f9f75c913d7d
- Child:
- 6:1518fccc5616
--- a/main.cpp Fri Oct 23 09:47:23 2015 +0000
+++ b/main.cpp Mon Oct 26 06:21:49 2015 +0000
@@ -2,151 +2,142 @@
// 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 and move in the horizontal plane.
+ In order to achieve this movement we use a ‘arm’ that can turn in the vertical plane and move in the horizontal plane.
*/
-//******************************************** Library DECLARATION **************************************
+//*********************************************** LIBRARY DECLARATION ********************************************
// 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> //
+#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" // This sends the measured EMG signal to the HIDScope.
+#include "biquadFilter.h" // Because of this library we can make different filters.
+#include <cmath> // This library declares a set of functions to compute common mathematical operations and transformations. Only used fabs.
+#include <stdio.h> // This library defines three variable types, several macros, and various functions for performing input and output.
-//******************************************* FUNCTION DECLA *******************************************
-//**************** INPUTS *****************
-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
+//*********************************************** FUNCTION DECLARATION *******************************************
+//**************************** INPUTS ******************************************************
+AnalogIn EMG1(A0); // Input left biceps EMG.
+AnalogIn EMG2(A1); // Input right biceps 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.
-//**************** OUTPUTS ****************
-DigitalOut red(LED_RED); // LED declared for calibration
-DigitalOut green(LED_GREEN); // LED declared for sampling
+//**************************** OUTPUTS *****************************************************
+DigitalOut led_red(D1); // Output for red LED decleared.
+DigitalOut led_yellow(D3); // Output for yellow LED decleared.
+DigitalOut led_green(D9); // Output for green LED delceared.
+DigitalOut magnet(D2); // Output for magnet.
+DigitalOut motor1direction(D4); // Direction for motor 2 on motorshield. This motor moves the arm in fase 2.
+PwmOut motor1speed(D5); // Speed for motor 2 on motorshield. This motor moves the arm in fase 2.
+DigitalOut motor2direction(D7); // Direction for motor 1 on motorshield. This motor moves the board in fase 1.
+PwmOut motor2speed(D6); // Speed for motor 1 on motorshield. This motor moves the board in fase 1.
-DigitalOut led_rood(D1);
-DigitalOut led_geel(D3);
-DigitalOut led_groen(D9);
+//**************************** FUNCTIONS ***************************************************
+//HIDScope scope(2); // HIDScope declared with 2 scopes.
+MODSERIAL pc(USBTX, USBRX); // Function for Serial communication with the Microcontroller to the pc.
-DigitalOut magnet(D2);
+//*********************************************** GLOBAL VARIABLES DECLARATION ***********************************
+const int led_on = 0; // This constant turns the led on.
+const int led_off = 1; // This constant turns the led off.
+int games_played1 = 0; // Counts number of games played.
+int games_played = -1; // Shows real number of games played. There is a -1 because the game is first reset before the first throw.
+float dt = 0.01; // Time staps
-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.
+//**************************** VARIABLES FOR MOTOR CONTROL *********************************
+const float cw = 1; // Motor1 moves counter clock wise and Motor2 moves clock wise.
+const float ccw = 0; // Motor1 moves clock wise and Motor2 moves counter clock wise.
+bool flag_s = false; // Var flag_s sensor ticker
+const int relax = 0; // The motor speed is zero.
-//******************************* 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;
-int games_played = -1; // -1 omdat het spel daar eerst loopt voor het spelen om alles na te checken
-int games_played1 = 0;
-float dt = 0.01;
-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;
+//**************************** VARIABLES FOR CONTROL 1 *************************************
+int Fs = 512; // Sampling frequency.
+int calibration_measurements = 0; // Integer counts the number of calibrations done. It starts at 0.
+//Filter coefficients. a1 is normalized to 1.
+const double low_b1 = /*0.0055427172102806843;*/1.480219865318266e-04;
+const double low_b2 = /*0.011085434420561369;*/2.960439730636533e-04;
+const double low_b3 = /*0.0055427172102806843; */1.480219865318266e-04;
+const double low_a2 = /*-1.778631777824585; */-1.965293372622690e+00;
+const double low_a3 = /*0.80080264666570777; */9.658854605688177e-01;
+const double high_b1 = /*0.63894552515902237;*/8.047897937631126e-01;
+const double high_b2 = /*-1.2778910503180447; */-1.609579587526225e+00;
+const double high_b3 = /*0.63894552515902237;*/8.047897937631126e-01;
+const double high_a2 = /*-1.1429805025399009;*/-1.571102440190402e+00;
+const double high_a3 = /*0.41280159809618855;*/6.480567348620491e-01;
+// Declaring the input and output variables.
+double u1; // Input from EMG 1. The left biceps.
+double y1; // Output from the filters from u1.
+double u2; // Input from EMG 2. The right biceps.
+double y2; // Output from the filters from u2.
-//********* VARIABLES FOR CONTROL 1 ************
-volatile bool sample_go; // Ticker EMG function
-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] = {};
+double cali_fact1 = 0.9; // Calibration factor to normalize filter output to a scale of 0 - 1. For signal y1.
+double cali_fact2 = 0.9; // Calibration factor to normalize filter output to a scale of 0 - 1. For signal y2.
+double cali_max1 = 0; // Declares max y1.
+double cali_max2 = 0; // Declares max y2.
+double cali_array1[2560] = {}; // Array to store values in for the 5 seconds calibartion for y1.
+double cali_array2[2560] = {}; // Array to store values in for the 5 seconds calibartion for y2.
+volatile bool sample_go; // Ticker EMG function.
+bool flag_calibration = true; // Flag to start calibration.
-//********* 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
-int reset = 0;
-int button_pressed = 0;
-int j = 1;
-int N = 200;
-bool fase_switch_wait = true;
+//**************************** VARIABLES FOR FASE SWITCH ***********************************
+int fase = 3; // The fase is used in a switch to switch between the fases. It starts at the reset fase.
+int j = 1; // Starting up a new part of the game.
+int N = 200; // Stop for j.
+bool fase_switch_wait = true; // Timer wait to switch between different fases.
+
+//**************************** VARIABLES FOR CONTROL 2 *************************************
+const float contract = 0.5; // The minimum value for y1 and y2 for which the motor moves.
+const float fasecontract = 0.7; // The value y1 and y2 must be for the fase change.
+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; // This flag determines if the signals from the left biceps should be measured. This is signal y1.
+bool flag_right = true; // This flag determines if the signals from the right biceps should be measured. This is signal y2.
+float pos_board = 0; // The begin position of the board. It begins at zero. Reads Encoder2 in degrees. Formula is applied in SensorTicker.
+int pos_board1 = 0; //
+float Encoderread2 = 0; // Reads travelled distance from Motor2. Reads pulses.
-//********* 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;
+//**************************** VARIABLES FOR CONTROL 3 *************************************
+const float minimum_contract = 0.4; // The minimum value for y2 for whicht the motor moves.
+const float medium_contract = 0.5; // Value for different speeds of the motor.
+const float maximum_leftbiceps = 0.7; // Value for y1 for which the magnet turns off.
+const float on = 1.0; // This value turns the magnet on.
+const float off = 0.0; // This value turns the magnet off.
+const float minimum_throw_angle = 20; // The minimum angle the arm has to be in to be able to turn the magnet off.
+const float maximum_throw_angle = 110; // The maximum angle for the arm to turn the magnet off.
+float pos_arm = 0; // The begin position of the arm. It begins at zero. Reads Encoder1 in degrees. Formula is applied in SensorTicker.
+int pos_arm1 = 0; // This makes from the positon of the arm degrees within a cirkel.
+float pos_arm2 = 0; // This makes from the positon of the arm degrees within a cirkel.
+float previous_pos_arm = 0; // Needed to calculate the v_arm.
+float v_arm = 0; // The speed of the arm.
+float v_arm_com = 0; // The compensated speed of the arm.
+float speed_control_arm = 0.000; //
+float Encoderread1 = 0; // Reads travelled distance from Motor1.
+int switch_rounds = 2; // Value of a switch to calculate the number of rounds made.
+int rounds = 0; // Number of rounds made by the arm.
+float pos_armrounds_max = 3; // Max rounds the arm can make.
+bool problem1 = false; //
+bool problem2 = false; //
+bool flag_v_arm = false; //
+float problem_velocity = 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 previous_pos_arm = 0;
-float v_arm = 0;
-float v_arm_com = 0;
-float speed_control_arm = 0.000;
-float Encoderread1 = 0;
-int switch_rondjes = 2;
-int rondjes = 0;
-float pos_armrondjes_max = 3;
-bool problem1 = false;
-bool problem2 = false;
-bool flag_v_arm = false;
-float problem_velocity = 0;
+//**************************** VARIABLES FOR CONTROL 4 *************************************
+float reset_position = 0; // The reset position of the arm.
+int reset_arm = 0; // The reset position of the arm (?).
+int reset_board = 0; // The reset position of the board.
+float speedcompensation; // Speed of Motor2 during reset.
+float speedcompensation2; // Speed of Motor1 during reset.
+int t = 5; // Integer for count down to new game.
+int switch_reset = 1; // Value of a switch for the different parts of the reset.
+bool board = false; //
-//********* VARIABLES FOR CONTROL 4 ************
-
-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;
-
-// **************************************** Tickers *****************************************
-Ticker Sensor_control; // Adds ticker function for the variable function : Sensors
-Ticker Motor_control;
-Ticker EMG_Control;
+// **************************************** Tickers ****************************************
+/* Tickers count constantly. The formulas attacht to the ticker count with them. */
+Ticker Sensor_control; // This ticker counts for the position of the motors and the speed of the arm.
+Ticker EMG_Control; // This ticker counts for the filtering of the EMG signal
//=============================================================================================== 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 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);
@@ -156,59 +147,50 @@
u1 = EMG1;
u2 = EMG2;
y1 = highpass1.step(u1);
- y2 = highpass2.step(u2); // filter order is: high-pass --> rectify --> low-pass
+ y2 = highpass2.step(u2); // filter order is: high-pass --> rectify --> low-pass
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.
+ 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 ==========================================
+//**************************** TICKER LOOPS ************************************************
void SENSOR_LOOP()
{
Encoderread1 = wheel1.getPulses();
- pos_arm = (Encoderread1/((64.0*131.0)/360.0)); // Omrekenen naar graden van arm
+ pos_arm = (Encoderread1/((64.0*131.0)/360.0)); // Omrekenen naar graden van arm
pos_arm1 = pos_arm;
+ previous_pos_arm = pos_arm;
v_arm = (pos_arm - previous_pos_arm)/dt;
- previous_pos_arm = pos_arm;
Encoderread2 = wheel2.getPulses();
- pos_board = (Encoderread2/((64.0*131.0)/360.0)); // Omrekenen naar graden van board
+ pos_board = (Encoderread2/((64.0*131.0)/360.0)); // Omrekenen naar graden van board
pos_board1 = pos_board;
flag_s = true;
}
-void MOTOR_LOOP()
-{
- flag_m = true;
-}
-
-void samplego() // ticker function, set flag to true every sample interval
+void EMG_LOOP() // ticker function, set flag to true every sample interval
{
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);
- EMG_Control.attach(&samplego, (float)1/Fs);
+ EMG_Control.attach(&EMG_LOOP, (float)1/Fs);
- led_groen.write(0);
- led_geel.write(0);
- led_rood.write(0);
+ led_green.write(0);
+ led_yellow.write(0);
+ led_red.write(0);
pc.printf("===============================================================\n");
pc.printf(" \t\t\t Ball-E\n");
@@ -229,30 +211,26 @@
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
+ calibration_measurements ++;
pc.printf(" \n\n EMG Signal starting up Calibration measurement........... \n");
wait(2);
- led_rood.write(0);
+ led_red.write(0);
wait(0.2);
- led_rood.write(1); //Toggles red calibration LED on
+ led_red.write(1); //Toggles red calibration LED on
wait(0.2);
- led_rood.write(0);
+ led_red.write(0);
wait(0.2);
- led_rood.write(1);
+ led_red.write(1);
wait(0.2);
- led_rood.write(0);
+ led_red.write(0);
wait(0.2);
- led_rood.write(1);
+ led_red.write(1);
wait(1);
pc.printf("\t.....Calibrating Signal of EMG 1 and 2 .......\n");
- led_rood.write(0);
- led_geel.write(1);
+ led_red.write(0);
+ led_yellow.write(1);
wait(0.5);
pc.printf("\t......contract muscles..... \n");
@@ -285,19 +263,19 @@
cali_fact2 = (double)1/cali_max2; // Calibration factor for y2
// Toggles green sampling LED off
- led_geel.write(0);
+ led_yellow.write(0);
pc.printf(" \t....... Calibration has been completed!\n");
wait(0.2);
- led_groen.write(led_off);
+ led_green.write(led_off);
wait(0.2);
- led_groen.write(led_on);
+ led_green.write(led_on);
wait(0.2);
- led_groen.write(led_off);
+ led_green.write(led_off);
wait(0.2);
- led_groen.write(led_on);
+ led_green.write(led_on);
wait(4);
pc.printf("Beginning with Ball-E board settings\n");
- led_groen.write(led_off);
+ led_green.write(led_off);
wait(2);
y1 = 0;
y2 = 0;
@@ -320,9 +298,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);
+ led_red.write(0);
+ led_green.write(1);
+ led_yellow.write(0);
}
if( j > N)
@@ -332,10 +310,10 @@
{
//******************* Fase 1 **************************
case(1):
- led_rood.write(1);
- led_groen.write(0);
- led_geel.write(0);
- rondjes = 0;
+ led_red.write(1);
+ led_green.write(0);
+ led_yellow.write(0);
+ rounds = 0;
if (y1> contract)
{
flag_right = false;
@@ -391,30 +369,31 @@
motor2speed.write(relax);
fase = 2;
fase_switch_wait = true;
- led_rood.write(0);
- led_groen.write(0);
- led_geel.write(1);
+ led_red.write(0);
+ led_green.write(0);
+ led_yellow.write(1);
j = 0;
}
break;
//******************* Fase 2 **************************
case(2):
- led_rood.write(0);
- led_groen.write(0);
- led_geel.write(1);
+ led_red.write(0);
+ led_green.write(0);
+ led_yellow.write(1);
motor1direction.write(cw);
- pos_arm1 = (pos_arm - (rondjes * 360));
-
- switch(switch_rondjes)
+ pos_arm1 = (pos_arm - (rounds * 360));
+ pos_arm2 = pos_arm1;
+
+ switch(switch_rounds)
{
case(1):
- rondjes ++;
- switch_rondjes = 2;
+ rounds ++;
+ switch_rounds = 2;
break;
case(2):
if(pos_arm1>360 & 368<pos_arm1)
{
- switch_rondjes = 1;
+ switch_rounds = 1;
}
break;
}
@@ -426,14 +405,15 @@
}
if (y2 > medium_contract) // Hoger dan drempelwaarde = Actief
{
- motor1speed.write(1);
+ speed_control_arm = ((v_arm/304) + 0.15*(1 -(v_arm/304)));
+ motor1speed.write(speed_control_arm);
}
if (y2 < minimum_contract) // Lager dan drempel = Rust
{
motor1speed.write(relax);
}
- if(rondjes == pos_armrondjes_max) // max aantal draaing gemaakt!!!!!!
+ if(rounds == pos_armrounds_max) // max aantal draaing gemaakt!!!!!!
{
problem1 = true;
problem2 = true;
@@ -453,17 +433,17 @@
}
wait(0.1);
- led_rood.write(0);
+ led_red.write(0);
wait(0.1);
- led_rood.write(1);
+ led_red.write(1);
wait(0.1);
- led_rood.write(0);
+ led_red.write(0);
wait(0.1);
- led_rood.write(1);
+ led_red.write(1);
wait(0.1);
- led_rood.write(0);
+ led_red.write(0);
wait(0.1);
- led_rood.write(1);
+ led_red.write(1);
wait(1.5);
while(problem2)
@@ -477,7 +457,7 @@
if(flag_v_arm){
v_arm_com = v_arm;
}
- speed_control_arm = (0.4*((pos_arm1 - reset_arm)/500.0) + (0.05) - (v_arm_com/304)*0.3);
+ speed_control_arm = (0.4*((pos_arm2 - reset_arm)/500.0) + (0.05) - (v_arm_com/304)*0.3);
motor1speed.write(speed_control_arm);
if (pos_arm < 10)
@@ -485,7 +465,7 @@
flag_v_arm = false;
problem2 = false;
motor1speed.write(0);
- rondjes = 0;
+ rounds = 0;
wait(1);
}
}
@@ -504,32 +484,31 @@
fase_switch_wait = true;
}
}
- pc.printf("Armposition \t %i \t EMG1 en EMG2 = %f \t %f \n", pos_arm1, y1, y2);
+ pc.printf("Snelheid arm = \t %f \t EMG1 en EMG2 = %f \t %f \n", speed_control_arm, y1, y2);
break;
//********************************************* Fase 3 **********************************************
case(3):
- led_rood.write(0);
- led_groen.write(1);
- led_geel.write(0);
+ led_red.write(0);
+ led_green.write(1);
+ led_yellow.write(0);
switch(switch_reset)
{
case(1):
- if(pos_arm < reset_positie) //ene kant op draaien
+ if(pos_arm < reset_position) //ene kant op draaien
{
motor1direction.write(cw);
speedcompensation2 = 0.05; //((reset_arm - pos_arm1)/900.0 + (0.02));
motor1speed.write(speedcompensation2);
}
- if(pos_arm > reset_positie) //andere kant op
+ if(pos_arm > reset_position) //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
+ if(pos_arm < reset_position+5 && pos_arm > reset_position-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);
@@ -562,19 +541,16 @@
if(pos_board < reset_board+5 && pos_board > reset_board-5)
{
motor2speed.write(0);
- board1 = true;
+ board = true;
}
- if(board1 == true)
+ if(board == 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
+ board = false;
wait(2);
games_played ++;
- games_played1 = games_played - (3*calibratie_metingen - 3);
+ games_played1 = games_played - (3*calibration_measurements - 3);
pc.printf("Games played total count = %i and loop %i\n",games_played,games_played1);
if(games_played1 == 3)
@@ -584,9 +560,9 @@
{
pc.printf("\tCalibratie begint over %i \n",t);
t--;
- led_geel.write(1);
+ led_yellow.write(1);
wait(0.5);
- led_geel.write(0);
+ led_yellow.write(0);
wait(0.5);
}
}
@@ -594,9 +570,9 @@
{
pc.printf("\tNieuw spel begint over %i \n",t);
t--;
- led_geel.write(1);
+ led_yellow.write(1);
wait(0.5);
- led_geel.write(0);
+ led_yellow.write(0);
wait(0.5);
}