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:
- 10:57f090f3ddda
- Parent:
- 9:a5c2ddf2cb8e
--- a/main.cpp Tue Oct 27 15:42:48 2015 +0000
+++ b/main.cpp Thu Oct 29 15:16:35 2015 +0000
@@ -15,7 +15,7 @@
#include <stdio.h> // This library defines three variable types, several macros, and various functions for performing input and output.
//================================================================================= INPUT, OUTPUT AND FUNCTION DECLARATION =============================================================================================
-/* Here are different inputs, outputs and functions decleared. There are differnt inputs. For the input and the reading of the encoders.
+/* Here are different inputs, outputs and functions decleared. There are differnt inputs. For the input and the reading of the encoders.
The outputs are for the different leds, the magnet and the motor. There is also an function decleared for the MODSERIAL and one for the scope.
The one for the scope is in this script turned off because only putty is used but it is possible to turn it on. */
//**************************** INPUTS ******************************************************
@@ -25,11 +25,11 @@
QEI wheel2 (D12, D13, NC, 64,QEI::X4_ENCODING); // Function for Encoder2 on the motor2 to the Microcontroller.
//**************************** OUTPUTS *****************************************************
-DigitalOut led_red(D1); // Output for red LED decleared.
+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.
+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.
@@ -39,41 +39,41 @@
MODSERIAL pc(USBTX, USBRX); // Function for Serial communication with the Microcontroller to the pc.
//========================================================================================= GLOBAL VARIABLES DECLARATION =====================================================================================================
-/* For every loop there are different variables and constants needed. All these variables and constants are decleared down here. First the constants and variables for the main program.
-Second for the control of the motor and than for the different controls and fase switch. At the bottom tickers are defined.
-Tickers count constantly. Formulas can be attacht to them so they are contantly calculated. */
+/* For every loop there are different variables and constants needed. All these variables and constants are decleared down here. First the constants and variables for the main program.
+Second for the control of the motor and than for the different controls and fase switch. At the bottom tickers are defined.
+Tickers count constantly. Formulas can be attacht to them so they are contantly calculated. */
-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.
+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
+double dt = 0.01; // Time staps
//**************************** 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.
+const int cw = 1; // Motor1 moves counter clock wise and Motor2 moves clock wise.
+const int 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.
//**************************** VARIABLES FOR CONTROL 1 *************************************
int Fs = 512; // Sampling frequency.
-int calibration_measurements = 0; // Integer counts the number of calibrations done. It starts at 0.
+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_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_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_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.
+double y2; // Output from the filters from u2.
// Declaring variables for calibration
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.
@@ -84,73 +84,77 @@
bool flag_calibration = true; // Flag to start the calibration.
//**************************** 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 fase = 3; // The fase is used in a switch to switch between the fases. It starts at the reset fase.
int j = 1; // Wait time. Used in problem1 and fase switch
int N = 200; // Maximum value of 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.
+const double contract = 0.5; // The minimum value for y1 and y2 for which the motor moves.
+const double fasecontract = 0.7; // The value y1 and y2 must be for the fase change.
+const double maxleft = -200; // With this angle the motor should stop moving.
+const double maxright = 200; // With this angle the motor should stop moving.
+const double 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.
+double 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; //
+double Encoderread2 = 0; // Reads travelled distance from Motor2. Reads pulses.
//**************************** 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.
+const double minimum_contract = 0.4; // The minimum value for y2 for whicht the motor moves.
+const double medium_contract = 0.5; // Value for different speeds of the motor.
+const double maximum_leftbiceps = 0.7; // Value for y1 for which the magnet turns off.
+const double on = 1.0; // This value turns the magnet on.
+const double off = 0.0; // This value turns the magnet off.
+const double minimum_throw_angle = 35; // The minimum angle the arm has to be in to be able to turn the magnet off.
+const double maximum_throw_angle = 100; // The maximum angle for the arm to turn the magnet off.
+double 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 as an integer.
-float pos_arm2 = 0; // This makes from the positon of the arm degrees within a cirkel as an float.
-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.
+double pos_arm2 = 0; // This makes from the positon of the arm degrees within a cirkel as an float.
+double previous_pos_arm = 0; // Needed to calculate the v_arm.
+double v_arm = 0; // The speed of the arm.
+double v_arm_com = 0; // The compensated speed of the arm.
+double speed_control_arm = 0.000; //
+double 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.
+double pos_armrounds_max = 2; // Max rounds the arm can make.
bool problem1 = false; // Stop for fase 2. It is a wait for problem2 begins.
-bool problem2 = false; // The reset of fase 2 when the arm reaches its maximum value.
-bool flag_v_arm = false; //
-float problem_velocity = 0; //
+bool problem2 = false; // The reset of fase 2 when the arm reaches its maximum value.
+bool flag_v_arm = false; //
+double 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.
+double reset_position = 0; // The reset position of the arm.
+double reset_arm = 0; // The reset position of the arm (?).
+double reset_board = 0; // The reset position of the board.
+double speedcompensation; // Speed of Motor2 during reset.
+double 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; //
+bool flag_calibration_LED = false;
+bool reset_case3 = false;
+bool flag_arm_magnet = false;
// **************************************** Tickers ****************************************
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
+Ticker LED_control;
//=============================================================================================== SUB LOOPS ==================================================================================================================
/* Different subloops are used so the head loop doesn't get to full. One loop is used for the filtering and calibration of the EMG signals.
-Then there are two loops which are attached to a ticker so they are read through constantly.
+Then there are two loops which are attached to a ticker so they are read through constantly.
One is for the reading of the encoders and one for turning on and off the EMG signal calibration and filtering. */
//**************************** CONTROL 1 - EMG CALIBRATION AND FILTERING ***********************************
-/* There are highpass and lowpass filters used. For both the EMG signals is a highpass and a lowpass filter. The function biquadFilter makes these filters.
+/* There are highpass and lowpass filters used. For both the EMG signals is a highpass and a lowpass filter. The function biquadFilter makes these filters.
In the void loop is the filtering done. The u1 and u2 are the reading of the EMG signals. After reading the filters are used. These are the y signals.
-The filter order is highpass, recifyer and lowpass. The calibration is done at the same time as the lowpass filter.
+The filter order is highpass, recifyer and lowpass. The calibration is done at the same time as the lowpass filter.
The signal is multiplied with the calibarion factor to put the signal between 0 and 1. */
-biquadFilter highpass1(high_a2, high_a3, high_b1, high_b2, high_b3);
+biquadFilter highpass1(high_a2, high_a3, high_b1, high_b2, high_b3);
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);
@@ -160,42 +164,70 @@
u1 = EMG1;
u2 = EMG2;
y1 = highpass1.step(u1);
- y2 = highpass2.step(u2);
+ y2 = highpass2.step(u2);
y1 = fabs(y1);
y2 = fabs(y2);
y1 = lowpass1.step(y1)*cali_fact1;
- y2 = lowpass2.step(y2)*cali_fact2;
+ y2 = lowpass2.step(y2)*cali_fact2;
}
//**************************** TICKER LOOPS ****************************************************************
-/* In the SENSOR_LOOP the signal from the encoders are read. This signal is calculated in degrees. This is the pos_arm and the pos_board.
+/* In the SENSOR_LOOP the signal from the encoders are read. This signal is calculated in degrees. This is the pos_arm and the pos_board.
For the arm there are also spreeds calculated. These are used to give a speed to the motor later on. Also the flag for the calibration is turned off here. */
void SENSOR_LOOP()
{
Encoderread1 = wheel1.getPulses();
previous_pos_arm = pos_arm;
- pos_arm = (Encoderread1/((64.0*131.0)/360.0));
+ pos_arm = (Encoderread1/((64.0*131.0)/360.0));
pos_arm1 = pos_arm;
v_arm = (pos_arm - previous_pos_arm)/dt;
Encoderread2 = wheel2.getPulses();
- pos_board = (Encoderread2/((64.0*131.0)/360.0));
+ pos_board = (Encoderread2/((64.0*131.0)/360.0));
pos_board1 = pos_board;
-
+
flag_s = true;
}
-/* This loop determines if the calibration can be done or not. */
-void EMG_LOOP()
+void LED_LOOP()
{
- if(flag_calibration == false)
- {
+ if(flag_calibration_LED == true) {
+ led_red.write(1);
+ led_green.write(0);
+ led_yellow.write(0);
+ wait(0.1);
+ led_red.write(1);
+ led_green.write(1);
+ led_yellow.write(0);
+ wait(0.1);
+ led_red.write(0);
+ led_green.write(1);
+ led_yellow.write(0);
+ wait(0.1);
+ led_red.write(0);
+ led_green.write(1);
+ led_yellow.write(1);
+ wait(0.1);
+ led_red.write(0);
+ led_green.write(0);
+ led_yellow.write(1);
+ wait(0.1);
+ led_red.write(0);
+ led_green.write(0);
+ led_yellow.write(0);
+ wait(0.1);
+ }
+}
+/* This loop determines if the calibration can be done or not. */
+void EMG_LOOP()
+{
+ if(flag_calibration == false) {
hoog_laag_filter();
}
}
//================================================================================================== HEAD LOOP ================================================================================================================
-/* Here is the main loop defined. First
+/* Here is the main loop defined. First
Then the tickers are attached to the subloops above. Next some lines to turn led off and some lines that are shown in putty.
After this a while loop starts. This loop is always on. It starts with the calibration. Then the board returns to its begin settings.
Then the game can begin in the switch with fase 1, then fase 2 and eventually fase 3. After 5 games the calibration needs to be done again. */
@@ -203,8 +235,9 @@
int main()
{
pc.baud(115200);
- Sensor_control.attach(&SENSOR_LOOP, 0.01);
+ Sensor_control.attach(&SENSOR_LOOP, 0.01);
EMG_Control.attach(&EMG_LOOP, (float)1/Fs);
+ LED_control.attach(&LED_LOOP, 1);
led_green.write(0);
led_yellow.write(0);
@@ -214,28 +247,39 @@
pc.printf(" \t\t\t Ball-E\n");
pc.printf("In the module Biorobotics on the University of Twente this script is created\n");
pc.printf("Autors:\tE. Velu, L. Verhoeven, R. v/d Wal, T. v/d Wal, E. Zoetbrood\n\n");
+ led_red.write(1);
+ led_green.write(1);
+ led_yellow.write(1);
wait(3);
+ led_red.write(0);
+ led_green.write(0);
+ led_yellow.write(0);
pc.printf("The script will begin with a short calibration\n\n");
wait(2.5);
+ led_red.write(1);
+ led_green.write(1);
+ led_yellow.write(1);
pc.printf("===============================================================\n");
//**************************** CONTROL 1 - EMG CALIBRATION AND FILTERING ***********************************
- while(1)
- {
- if (flag_calibration)
- {
+ while(1) {
+ if (flag_calibration) {
+ wait(0.4);
+ led_red.write(0);
+ led_green.write(0);
+ led_yellow.write(0);
calibration_measurements ++;
pc.printf(" \n\n EMG Signal starting up Calibration measurement........... \n");
-
- cali_max1 = 0;
- cali_max2 = 0;
- cali_fact1 = 0.9;
+
+ cali_max1 = 0;
+ cali_max2 = 0;
+ cali_fact1 = 0.9;
cali_fact2 = 0.9;
-
+
wait(2);
led_red.write(0);
wait(0.2);
- led_red.write(1);
+ led_red.write(1);
wait(0.2);
led_red.write(0);
wait(0.2);
@@ -249,31 +293,26 @@
pc.printf("\t.....Calibrating Signal of EMG 1 and 2 .......\n");
led_red.write(0);
led_yellow.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];
}
@@ -298,91 +337,87 @@
y1 = 0;
y2 = 0;
- j = 1; // Wait for the fase swith to initiate.
+ j = 1; // Wait for the fase swith to initiate.
fase_switch_wait = true;
flag_calibration = false;
}
- if (flag_s) // Turns calibration off.
- {
+ if (flag_s) { // Turns calibration off.
flag_calibration = false;
}
//**************************** FASE SWITCH *****************************************************************
- if (fase_switch_wait) // There needs to be a wait before the script can start so the sample loop has the time to do the calibration.
- {
+ if (fase_switch_wait) { // There needs to be a wait before the script can start so the sample loop has the time to do the calibration.
j++;
- wait(0.01);
+ wait(0.01);
pc.printf("waarde j = %i \n",j);
led_red.write(0);
led_green.write(1);
led_yellow.write(0);
}
- if( j > N) // When the wait is long enough the game starts.
- {
+ if( j > N) { // When the wait is long enough the game starts.
fase_switch_wait = false;
- switch(fase)
- {
- //************************ Fase 1 ******************************************************************
+ switch(fase) {
+ //************************ Fase 1 ******************************************************************
case(1):
led_red.write(1);
led_green.write(0);
led_yellow.write(0);
rounds = 0;
-
- if (y1> contract) // If the left arm is above this value flag_left turns true so the motor can run ccw.
- { // The left and right can not be true at the same time so right has to turn off when left is on.
+
+ /* If the left arm is above this value flag_left turns true so the motor can run ccw.
+ The left and right arm can not be true at the same time so right has to turn off when left is on.
+ This is the same for the left and right arm. */
+ 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)
- {
+ /* To make sure the ball is thrown in the direction of the board there are maximum values defined.
+ This is done for the left and right arm.
+ If the maximum value is reached, the flag turns off and the motor speed is zero.*/
+ 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)
- {
- if (y1> contract)
- {
+ /* When flag_left is true, the left biceps can move the motor in ccw direction.
+ This is the same for flag_right and the right biceps in the cw direction.
+ There is also a speed defined in which the motor runs.
+ There still is the minimum value for the mucle contraction before the motor runs.
+ This is done so the motor only moves when you really contract your muscle and not with spontaneous contraction. */
+ if (flag_left) {
+ if (y1> contract) {
motor2direction.write(ccw);
motor2speed.write(speed_plate_1);
- }
- else
- {
+ } else {
motor2speed.write(relax);
}
}
- if (flag_right)
- {
- if (y2 > contract)
- {
+ if (flag_right) {
+ 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)
- {
+ pc.printf("Boardposition \t %.2f EMG1 en EMG2 signaal = %.2f \t %.2f\n", pos_board, y1, y2);
+ /* When both the muscles are above a value the next fase is turnt on.
+ Some values need to be reset and the leds needs to change. */
+ if (y1> fasecontract && y2 > fasecontract) {
motor2speed.write(relax);
fase = 2;
fase_switch_wait = true;
@@ -392,58 +427,49 @@
j = 0;
}
break;
- //************************ Fase 2 ******************************************************************
-
+ //************************ Fase 2 ******************************************************************
case(2):
led_red.write(0);
led_green.write(0);
led_yellow.write(1);
motor1direction.write(cw);
pos_arm1 = (pos_arm - (rounds * 360));
- pos_arm2 = pos_arm1;
-
- switch(switch_rounds)
- {
+ pos_arm2 = (pos_arm - (rounds * 360));
+
+ switch(switch_rounds) {
case(1):
rounds ++;
switch_rounds = 2;
break;
case(2):
- if(pos_arm1>360 & 368<pos_arm1)
- {
+ if(pos_arm1>360) {
switch_rounds = 1;
}
break;
}
- if (y2 > minimum_contract & y2 < medium_contract)
- {
- speed_control_arm = 1;//((v_arm/304) + 0.15*(1 -(v_arm/304)));
+ if (y2 > minimum_contract & y2 < medium_contract) {
+ speed_control_arm = (1.0*(y2));
motor1speed.write(speed_control_arm);
}
- if (y2 > medium_contract) // Hoger dan drempelwaarde = Actief
- {
- speed_control_arm = 1;//((v_arm/304) + 0.15*(1 -(v_arm/304)));
+ if (y2 > medium_contract) { // Hoger dan drempelwaarde = Actief
+ speed_control_arm = (1.1*(y2));
motor1speed.write(speed_control_arm);
}
- if (y2 < minimum_contract) // Lager dan drempel = Rust
- {
+ if (y2 < minimum_contract) { // Lager dan drempel = Rust
motor1speed.write(relax);
}
- if(rounds == pos_armrounds_max) // max aantal draaing gemaakt!!!!!!
- {
+ if(rounds == pos_armrounds_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.
- if( j > N)
- {
+ if( j > N) {
problem1 = false;
}
}
@@ -462,25 +488,23 @@
led_red.write(1);
wait(1.5);
- while(problem2)
- {
+ while(problem2) {
motor1direction.write(ccw);
- if(pos_arm < 170)
- {
- if(v_arm > 200)
- {
- flag_v_arm = true;
- }
+ if(pos_arm > 260) {
+ motor1speed.write(0.2);
+ }
+ if(pos_arm > 100 && pos_arm <260) {
+ motor1speed.write(0.07);
}
- if(flag_v_arm)
- {
- v_arm_com = v_arm;
- }
- 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 > 15 && pos_arm <100) {
+ motor1speed.write(0.05);
+ }
+ if(pos_arm < 10) {
+ motor1speed.write(0);
+ }
+ pc.printf("Positie van de arm = %.4f \n", pos_arm);
- if (pos_arm < 10)
- {
+ if (pos_arm < 10) {
flag_v_arm = false;
problem2 = false;
motor1speed.write(0);
@@ -489,49 +513,117 @@
}
}
}
- 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);
- wait(0.1);
motor1speed.write(relax);
- fase = 3;
+ flag_arm_magnet = true;
+ led_red.write(0);
+ led_green.write(0);
+ led_yellow.write(0);
+ wait(0.2);
+ led_yellow.write(1);
+ wait(0.2);
+ led_yellow.write(0);
+ wait(0.2);
+ led_yellow.write(1);
+ wait(0.2);
+ led_yellow.write(0);
+ wait(0.2);
+ led_red.write(0);
+ led_green.write(1);
+ led_yellow.write(1);
+ wait(0.2);
pc.printf("Van fase 2 naar fase 3\n");
-
- wait(2);
- j = 0;
- fase_switch_wait = true;
+ flag_arm_magnet = true;
+ wait(1.6);
}
}
- pc.printf("Snelheid arm = \t %f \t EMG1 en EMG2 = %f \t %f \n", speed_control_arm, y1, y2);
+ if(flag_arm_magnet == true) {
+
+ pc.printf(" \n\n arm magneet laten oppakken! \n");
+ while(flag_arm_magnet) {
+
+ pos_arm2 = (pos_arm - (rounds * 360));
+
+ pc.printf("pos_arm = %f \n",pos_arm2);
+ magnet = 1;
+
+ pos_arm2 = (pos_arm - (rounds * 360));
+ motor1direction.write(cw);
+ if(pos_arm2 > 0 && pos_arm2 < 200) {
+ motor1speed.write(0.3);
+ }
+ if(pos_arm2 > 200 && pos_arm2 < 260) {
+ motor1speed.write(0.15);
+ }
+ if(pos_arm2 > 260 && pos_arm2 < 350) {
+ motor1speed.write(0.04);
+ }
+ if(pos_arm2 > 350) {
+ motor1speed.write(0);
+ flag_arm_magnet = false;
+ fase = 3;
+ j = 0;
+ fase_switch_wait = true;
+ wait(1.5);
+ }
+ }
+ }
+ pc.printf("positie arm = \t %f \t EMG1 en EMG2 = %f \t %f \n", pos_arm2, y1, y2);
break;
- //************************ Fase 3 ******************************************************************
+//======================================= Reset arm om bal te pakken ===================================================================
+
+
+
+ //************************ Fase 3 ******************************************************************
case(3):
led_red.write(0);
led_green.write(1);
led_yellow.write(0);
- switch(switch_reset)
- {
+ switch(switch_reset) {
case(1):
- if(pos_arm < reset_position) //ene kant op draaien
- {
+
+ speedcompensation2 = (((pos_arm - reset_arm)/2300)+0.03);
+ speedcompensation = (0.05);
+
+ if(pos_arm<100) {
+ speedcompensation2 = 0.05;
+ }
+
+ 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_position) { //andere kant op
+ motor1direction.write(ccw);
motor1speed.write(speedcompensation2);
}
- 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_board < reset_board) {
+ motor2direction.write(cw);
+ motor2speed.write(speedcompensation);
+ }
+
+ if(pos_board > reset_board) {
+ motor2direction.write(ccw);
+ motor2speed.write(speedcompensation);
}
- if(pos_arm < reset_position+5 && pos_arm > reset_position-5) // Dit gaat niet goed komen, omdat het precies die waarde moet zijn
- {
+
+ if(pos_arm < reset_position+4 && pos_arm > reset_position-4) { // Dit gaat niet goed komen, omdat het precies die waarde moet zijn
motor1speed.write(0);
- switch_reset = 2;
+ }
+
+ if(pos_board < (reset_board+0.2) && pos_board > (reset_board-0.2)) {
+ motor2speed.write(0);
}
- pc.printf("Positie_arm = %f \t \t snelheid = %f \n",pos_arm, speedcompensation);
+
+ if( pos_arm < reset_position+4 && pos_arm > reset_position-4 && pos_board < (reset_board+0.2) && pos_board > (reset_board-0.2)) {
+ switch_reset = 2;
+ wait(0.5);
+ }
+
+ pc.printf("Positie_arm = %.2f \t \t snelheid = %.2f \n",pos_arm, speedcompensation2);
wait(0.0001);
break;
@@ -544,50 +636,69 @@
break;
case(3):
- if(pos_board < reset_board)
- {
- motor2direction.write(cw);
- speedcompensation = 0.05; //((reset_board - pos_board1)/500.0 + (0.1));
- motor2speed.write(speedcompensation);
- }
+ speedcompensation2 = 0.05;
- if(pos_board > reset_board)
- {
- motor2direction.write(ccw);
- speedcompensation = 0.05;//((pos_board1 - reset_board)/500.0 + (0.05));
- motor2speed.write(speedcompensation);
+ if(pos_arm < reset_position) { //ene kant op draaien
+ motor1direction.write(cw);
+ motor1speed.write(speedcompensation2);
}
-
- if(pos_board < reset_board+5 && pos_board > reset_board-5)
- {
- motor2speed.write(0);
+ if(pos_arm > reset_position) { //andere kant op
+ motor1direction.write(ccw);
+ motor1speed.write(speedcompensation2);
+ }
+ if(pos_arm < reset_position + 0.5 && pos_arm > reset_position - 0.5) { // Dit gaat niet goed komen, omdat het precies die waarde moet zijn
+ motor1speed.write(0);
board = true;
+ switch_reset = 1;
}
-
- if(board)
- {
+ if(board) {
pc.printf("fase switch naar 1\n\n");
board = false;
wait(2);
games_played ++;
- games_played1 = games_played - (5*calibration_measurements - 5);
+ 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 == 5)
- {
- flag_calibration = true;
- while(t)
- {
- pc.printf("\tCalibratie begint over %i \n",t);
- t--;
- led_yellow.write(1);
- wait(0.5);
- led_yellow.write(0);
- wait(0.5);
+ if(games_played1 == 3) {
+ pc.printf("Choose option calibration = left \t or go with game = right \n");
+ wait(2);
+ reset_case3 = true;
+ while(reset_case3) {
+ wait(0.05);
+ // flag_calibration_LED = true;
+ pc.printf("y1 = %.3f \t y2 = %.3f \n", y1, y2);
+ if(y1 > 0.6) {
+ flag_calibration_LED = false;
+ pc.printf("Calibration is active");
+ while(t) {
+ pc.printf("\tCalibratie begint over %i \n",t);
+ t--;
+ led_yellow.write(1);
+ wait(0.5);
+ led_yellow.write(0);
+ wait(0.5);
+ if(t == 0) {
+ flag_calibration = true;
+ reset_case3 = false;
+ }
+ }
+ }
+ if(y2 > 0.6) {
+ flag_calibration_LED = false;
+ reset_case3 = false;
+ pc.printf("The game is on!!");
+ while(t) {
+ pc.printf("\tNieuw spel begint over %i \n",t);
+ t--;
+ led_yellow.write(1);
+ wait(0.5);
+ led_yellow.write(0);
+ wait(0.5);
+ }
+ }
}
}
- while(t)
- {
+ while(t) {
pc.printf("\tNieuw spel begint over %i \n",t);
t--;
led_yellow.write(1);
@@ -599,10 +710,10 @@
fase = 1; // Terug naar fase 1
switch_reset = 1; // De switch op orginele locatie zetten
t = 5;
-
+ rounds = 0;
}
- pc.printf("Positie_board = %f \t \t snelheid = %f \n",pos_board, speedcompensation);
+ pc.printf("Positie_board = %.2f \t \t snelheid = %.2f \n",pos_board, v_arm);
wait(0.0001);
break;
}