Biorobotics10 / Mbed 2 deprecated Ball-E_script

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

main.cpp@9:a5c2ddf2cb8e, 2015-10-27 (annotated)

Committer:
EmilyJCZ
Date:
Tue Oct 27 15:42:48 2015 +0000
Revision:
9:a5c2ddf2cb8e
Parent:
8:151e43b99156
Child:
10:57f090f3ddda
Bijna klaar met de comments.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
EmilyJCZ 3:f9a1df2271d2 1 //======================================================================= Script: Ball-E ==========================================================================
EmilyJCZ 3:f9a1df2271d2 2 // Authors: Ewoud Velu, Lisa Verhoeven, Robert van der Wal, Thijs van der Wal, Emily Zoetbrood
Wallie117 0:6616d722fed3 3 /* This is the script of a EMG measurment moving robot. The purpose of the robot is to amuse people with the disease of Ducenne.
EmilyJCZ 3:f9a1df2271d2 4 The robot is designed to throw a ball in to a certain chosen pocket.
EmilyJCZ 9:a5c2ddf2cb8e 5 In order to achieve this movement we use a ‘arm’ that can turn in the vertical plane and move in the horizontal plane. */
Wallie117 0:6616d722fed3 6
EmilyJCZ 9:a5c2ddf2cb8e 7 //================================================================================ LIBRARY DECLARATION ==============================================================================================================
EmilyJCZ 9:a5c2ddf2cb8e 8 /* Libraries are files which contain standard formulas for reading surtain information. Every library contains its own information. */
EmilyJCZ 5:0597358d0016 9 #include "mbed.h" // Standard library. This includes the reading of AnalogIn, DigitalOut, PwmOut and other standard formules.
EmilyJCZ 5:0597358d0016 10 #include "QEI.h" // This library includes the reading of of the Encoders from motors.
EmilyJCZ 5:0597358d0016 11 #include "MODSERIAL.h" // MODSERIAL inherits Serial and adds extensions for buffering.
EmilyJCZ 5:0597358d0016 12 #include "HIDScope.h" // This sends the measured EMG signal to the HIDScope.
EmilyJCZ 5:0597358d0016 13 #include "biquadFilter.h" // Because of this library we can make different filters.
EmilyJCZ 5:0597358d0016 14 #include <cmath> // This library declares a set of functions to compute common mathematical operations and transformations. Only used fabs.
EmilyJCZ 5:0597358d0016 15 #include <stdio.h> // This library defines three variable types, several macros, and various functions for performing input and output.
Wallie117 0:6616d722fed3 16
EmilyJCZ 9:a5c2ddf2cb8e 17 //================================================================================= INPUT, OUTPUT AND FUNCTION DECLARATION =============================================================================================
EmilyJCZ 9:a5c2ddf2cb8e 18 /* Here are different inputs, outputs and functions decleared. There are differnt inputs. For the input and the reading of the encoders.
EmilyJCZ 9:a5c2ddf2cb8e 19 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.
EmilyJCZ 9:a5c2ddf2cb8e 20 The one for the scope is in this script turned off because only putty is used but it is possible to turn it on. */
EmilyJCZ 5:0597358d0016 21 //**************************** INPUTS ******************************************************
EmilyJCZ 5:0597358d0016 22 AnalogIn EMG1(A0); // Input left biceps EMG.
EmilyJCZ 5:0597358d0016 23 AnalogIn EMG2(A1); // Input right biceps EMG.
EmilyJCZ 5:0597358d0016 24 QEI wheel1 (D10, D11, NC, 64,QEI::X4_ENCODING); // Function for Encoder1 on the motor1 to the Microcontroller.
EmilyJCZ 5:0597358d0016 25 QEI wheel2 (D12, D13, NC, 64,QEI::X4_ENCODING); // Function for Encoder2 on the motor2 to the Microcontroller.
Wallie117 0:6616d722fed3 26
EmilyJCZ 5:0597358d0016 27 //**************************** OUTPUTS *****************************************************
EmilyJCZ 5:0597358d0016 28 DigitalOut led_red(D1); // Output for red LED decleared.
EmilyJCZ 5:0597358d0016 29 DigitalOut led_yellow(D3); // Output for yellow LED decleared.
EmilyJCZ 5:0597358d0016 30 DigitalOut led_green(D9); // Output for green LED delceared.
EmilyJCZ 5:0597358d0016 31 DigitalOut magnet(D2); // Output for magnet.
EmilyJCZ 5:0597358d0016 32 DigitalOut motor1direction(D4); // Direction for motor 2 on motorshield. This motor moves the arm in fase 2.
EmilyJCZ 5:0597358d0016 33 PwmOut motor1speed(D5); // Speed for motor 2 on motorshield. This motor moves the arm in fase 2.
EmilyJCZ 5:0597358d0016 34 DigitalOut motor2direction(D7); // Direction for motor 1 on motorshield. This motor moves the board in fase 1.
EmilyJCZ 5:0597358d0016 35 PwmOut motor2speed(D6); // Speed for motor 1 on motorshield. This motor moves the board in fase 1.
Wallie117 1:0e1d91965b8e 36
EmilyJCZ 5:0597358d0016 37 //**************************** FUNCTIONS ***************************************************
EmilyJCZ 5:0597358d0016 38 //HIDScope scope(2); // HIDScope declared with 2 scopes.
EmilyJCZ 5:0597358d0016 39 MODSERIAL pc(USBTX, USBRX); // Function for Serial communication with the Microcontroller to the pc.
Wallie117 0:6616d722fed3 40
EmilyJCZ 9:a5c2ddf2cb8e 41 //========================================================================================= GLOBAL VARIABLES DECLARATION =====================================================================================================
EmilyJCZ 9:a5c2ddf2cb8e 42 /* 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.
EmilyJCZ 9:a5c2ddf2cb8e 43 Second for the control of the motor and than for the different controls and fase switch. At the bottom tickers are defined.
EmilyJCZ 9:a5c2ddf2cb8e 44 Tickers count constantly. Formulas can be attacht to them so they are contantly calculated. */
EmilyJCZ 9:a5c2ddf2cb8e 45
EmilyJCZ 5:0597358d0016 46 const int led_on = 0; // This constant turns the led on.
EmilyJCZ 5:0597358d0016 47 const int led_off = 1; // This constant turns the led off.
EmilyJCZ 5:0597358d0016 48 int games_played1 = 0; // Counts number of games played.
EmilyJCZ 5:0597358d0016 49 int games_played = -1; // Shows real number of games played. There is a -1 because the game is first reset before the first throw.
EmilyJCZ 5:0597358d0016 50 float dt = 0.01; // Time staps
Wallie117 1:0e1d91965b8e 51
EmilyJCZ 5:0597358d0016 52 //**************************** VARIABLES FOR MOTOR CONTROL *********************************
EmilyJCZ 5:0597358d0016 53 const float cw = 1; // Motor1 moves counter clock wise and Motor2 moves clock wise.
EmilyJCZ 5:0597358d0016 54 const float ccw = 0; // Motor1 moves clock wise and Motor2 moves counter clock wise.
EmilyJCZ 5:0597358d0016 55 bool flag_s = false; // Var flag_s sensor ticker
EmilyJCZ 5:0597358d0016 56 const int relax = 0; // The motor speed is zero.
Wallie117 0:6616d722fed3 57
EmilyJCZ 5:0597358d0016 58 //**************************** VARIABLES FOR CONTROL 1 *************************************
EmilyJCZ 5:0597358d0016 59 int Fs = 512; // Sampling frequency.
EmilyJCZ 5:0597358d0016 60 int calibration_measurements = 0; // Integer counts the number of calibrations done. It starts at 0.
EmilyJCZ 5:0597358d0016 61 //Filter coefficients. a1 is normalized to 1.
EmilyJCZ 5:0597358d0016 62 const double low_b1 = /*0.0055427172102806843;*/1.480219865318266e-04;
EmilyJCZ 5:0597358d0016 63 const double low_b2 = /*0.011085434420561369;*/2.960439730636533e-04;
EmilyJCZ 5:0597358d0016 64 const double low_b3 = /*0.0055427172102806843; */1.480219865318266e-04;
EmilyJCZ 5:0597358d0016 65 const double low_a2 = /*-1.778631777824585; */-1.965293372622690e+00;
EmilyJCZ 5:0597358d0016 66 const double low_a3 = /*0.80080264666570777; */9.658854605688177e-01;
EmilyJCZ 5:0597358d0016 67 const double high_b1 = /*0.63894552515902237;*/8.047897937631126e-01;
EmilyJCZ 5:0597358d0016 68 const double high_b2 = /*-1.2778910503180447; */-1.609579587526225e+00;
EmilyJCZ 5:0597358d0016 69 const double high_b3 = /*0.63894552515902237;*/8.047897937631126e-01;
EmilyJCZ 5:0597358d0016 70 const double high_a2 = /*-1.1429805025399009;*/-1.571102440190402e+00;
EmilyJCZ 5:0597358d0016 71 const double high_a3 = /*0.41280159809618855;*/6.480567348620491e-01;
EmilyJCZ 5:0597358d0016 72 // Declaring the input and output variables.
EmilyJCZ 5:0597358d0016 73 double u1; // Input from EMG 1. The left biceps.
EmilyJCZ 5:0597358d0016 74 double y1; // Output from the filters from u1.
EmilyJCZ 5:0597358d0016 75 double u2; // Input from EMG 2. The right biceps.
EmilyJCZ 5:0597358d0016 76 double y2; // Output from the filters from u2.
EmilyJCZ 9:a5c2ddf2cb8e 77 // Declaring variables for calibration
EmilyJCZ 5:0597358d0016 78 double cali_fact1 = 0.9; // Calibration factor to normalize filter output to a scale of 0 - 1. For signal y1.
EmilyJCZ 5:0597358d0016 79 double cali_fact2 = 0.9; // Calibration factor to normalize filter output to a scale of 0 - 1. For signal y2.
EmilyJCZ 5:0597358d0016 80 double cali_max1 = 0; // Declares max y1.
EmilyJCZ 5:0597358d0016 81 double cali_max2 = 0; // Declares max y2.
EmilyJCZ 5:0597358d0016 82 double cali_array1[2560] = {}; // Array to store values in for the 5 seconds calibartion for y1.
EmilyJCZ 5:0597358d0016 83 double cali_array2[2560] = {}; // Array to store values in for the 5 seconds calibartion for y2.
EmilyJCZ 6:1518fccc5616 84 bool flag_calibration = true; // Flag to start the calibration.
Wallie117 0:6616d722fed3 85
EmilyJCZ 5:0597358d0016 86 //**************************** VARIABLES FOR FASE SWITCH ***********************************
EmilyJCZ 5:0597358d0016 87 int fase = 3; // The fase is used in a switch to switch between the fases. It starts at the reset fase.
EmilyJCZ 6:1518fccc5616 88 int j = 1; // Wait time. Used in problem1 and fase switch
EmilyJCZ 6:1518fccc5616 89 int N = 200; // Maximum value of j.
EmilyJCZ 5:0597358d0016 90 bool fase_switch_wait = true; // Timer wait to switch between different fases.
EmilyJCZ 5:0597358d0016 91
EmilyJCZ 5:0597358d0016 92 //**************************** VARIABLES FOR CONTROL 2 *************************************
EmilyJCZ 5:0597358d0016 93 const float contract = 0.5; // The minimum value for y1 and y2 for which the motor moves.
EmilyJCZ 5:0597358d0016 94 const float fasecontract = 0.7; // The value y1 and y2 must be for the fase change.
EmilyJCZ 5:0597358d0016 95 const float maxleft = -200; // With this angle the motor should stop moving.
EmilyJCZ 5:0597358d0016 96 const float maxright = 200; // With this angle the motor should stop moving.
EmilyJCZ 5:0597358d0016 97 const float speed_plate_1 = 0.1; // The speed of the plate in control 2.
EmilyJCZ 5:0597358d0016 98 bool flag_left = true; // This flag determines if the signals from the left biceps should be measured. This is signal y1.
EmilyJCZ 5:0597358d0016 99 bool flag_right = true; // This flag determines if the signals from the right biceps should be measured. This is signal y2.
EmilyJCZ 5:0597358d0016 100 float pos_board = 0; // The begin position of the board. It begins at zero. Reads Encoder2 in degrees. Formula is applied in SensorTicker.
EmilyJCZ 5:0597358d0016 101 int pos_board1 = 0; //
EmilyJCZ 5:0597358d0016 102 float Encoderread2 = 0; // Reads travelled distance from Motor2. Reads pulses.
Wallie117 0:6616d722fed3 103
EmilyJCZ 5:0597358d0016 104 //**************************** VARIABLES FOR CONTROL 3 *************************************
EmilyJCZ 5:0597358d0016 105 const float minimum_contract = 0.4; // The minimum value for y2 for whicht the motor moves.
EmilyJCZ 5:0597358d0016 106 const float medium_contract = 0.5; // Value for different speeds of the motor.
EmilyJCZ 5:0597358d0016 107 const float maximum_leftbiceps = 0.7; // Value for y1 for which the magnet turns off.
EmilyJCZ 5:0597358d0016 108 const float on = 1.0; // This value turns the magnet on.
EmilyJCZ 5:0597358d0016 109 const float off = 0.0; // This value turns the magnet off.
EmilyJCZ 5:0597358d0016 110 const float minimum_throw_angle = 20; // The minimum angle the arm has to be in to be able to turn the magnet off.
EmilyJCZ 5:0597358d0016 111 const float maximum_throw_angle = 110; // The maximum angle for the arm to turn the magnet off.
EmilyJCZ 5:0597358d0016 112 float pos_arm = 0; // The begin position of the arm. It begins at zero. Reads Encoder1 in degrees. Formula is applied in SensorTicker.
EmilyJCZ 6:1518fccc5616 113 int pos_arm1 = 0; // This makes from the positon of the arm degrees within a cirkel as an integer.
EmilyJCZ 6:1518fccc5616 114 float pos_arm2 = 0; // This makes from the positon of the arm degrees within a cirkel as an float.
EmilyJCZ 5:0597358d0016 115 float previous_pos_arm = 0; // Needed to calculate the v_arm.
EmilyJCZ 5:0597358d0016 116 float v_arm = 0; // The speed of the arm.
EmilyJCZ 5:0597358d0016 117 float v_arm_com = 0; // The compensated speed of the arm.
EmilyJCZ 5:0597358d0016 118 float speed_control_arm = 0.000; //
EmilyJCZ 5:0597358d0016 119 float Encoderread1 = 0; // Reads travelled distance from Motor1.
EmilyJCZ 5:0597358d0016 120 int switch_rounds = 2; // Value of a switch to calculate the number of rounds made.
EmilyJCZ 5:0597358d0016 121 int rounds = 0; // Number of rounds made by the arm.
EmilyJCZ 5:0597358d0016 122 float pos_armrounds_max = 3; // Max rounds the arm can make.
EmilyJCZ 6:1518fccc5616 123 bool problem1 = false; // Stop for fase 2. It is a wait for problem2 begins.
EmilyJCZ 6:1518fccc5616 124 bool problem2 = false; // The reset of fase 2 when the arm reaches its maximum value.
EmilyJCZ 6:1518fccc5616 125 bool flag_v_arm = false; //
EmilyJCZ 6:1518fccc5616 126 float problem_velocity = 0; //
Wallie117 0:6616d722fed3 127
EmilyJCZ 5:0597358d0016 128 //**************************** VARIABLES FOR CONTROL 4 *************************************
EmilyJCZ 5:0597358d0016 129 float reset_position = 0; // The reset position of the arm.
EmilyJCZ 5:0597358d0016 130 int reset_arm = 0; // The reset position of the arm (?).
EmilyJCZ 5:0597358d0016 131 int reset_board = 0; // The reset position of the board.
EmilyJCZ 5:0597358d0016 132 float speedcompensation; // Speed of Motor2 during reset.
EmilyJCZ 5:0597358d0016 133 float speedcompensation2; // Speed of Motor1 during reset.
EmilyJCZ 5:0597358d0016 134 int t = 5; // Integer for count down to new game.
EmilyJCZ 5:0597358d0016 135 int switch_reset = 1; // Value of a switch for the different parts of the reset.
EmilyJCZ 5:0597358d0016 136 bool board = false; //
Wallie117 0:6616d722fed3 137
EmilyJCZ 5:0597358d0016 138 // **************************************** Tickers ****************************************
EmilyJCZ 5:0597358d0016 139 Ticker Sensor_control; // This ticker counts for the position of the motors and the speed of the arm.
EmilyJCZ 5:0597358d0016 140 Ticker EMG_Control; // This ticker counts for the filtering of the EMG signal
Wallie117 0:6616d722fed3 141
Wallie117 0:6616d722fed3 142 //=============================================================================================== SUB LOOPS ==================================================================================================================
EmilyJCZ 9:a5c2ddf2cb8e 143 /* 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.
EmilyJCZ 9:a5c2ddf2cb8e 144 Then there are two loops which are attached to a ticker so they are read through constantly.
EmilyJCZ 9:a5c2ddf2cb8e 145 One is for the reading of the encoders and one for turning on and off the EMG signal calibration and filtering. */
EmilyJCZ 9:a5c2ddf2cb8e 146
EmilyJCZ 9:a5c2ddf2cb8e 147 //**************************** CONTROL 1 - EMG CALIBRATION AND FILTERING ***********************************
EmilyJCZ 9:a5c2ddf2cb8e 148 /* 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.
EmilyJCZ 9:a5c2ddf2cb8e 149 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.
EmilyJCZ 9:a5c2ddf2cb8e 150 The filter order is highpass, recifyer and lowpass. The calibration is done at the same time as the lowpass filter.
EmilyJCZ 9:a5c2ddf2cb8e 151 The signal is multiplied with the calibarion factor to put the signal between 0 and 1. */
EmilyJCZ 9:a5c2ddf2cb8e 152
EmilyJCZ 9:a5c2ddf2cb8e 153 biquadFilter highpass1(high_a2, high_a3, high_b1, high_b2, high_b3);
Wallie117 0:6616d722fed3 154 biquadFilter highpass2(high_a2, high_a3, high_b1, high_b2, high_b3);
Wallie117 0:6616d722fed3 155 biquadFilter lowpass1(low_a2, low_a3, low_b1, low_b2, low_b3);
Wallie117 0:6616d722fed3 156 biquadFilter lowpass2(low_a2, low_a3, low_b1, low_b2, low_b3);
Wallie117 0:6616d722fed3 157
Wallie117 1:0e1d91965b8e 158 void hoog_laag_filter()
Wallie117 0:6616d722fed3 159 {
Wallie117 1:0e1d91965b8e 160 u1 = EMG1;
Wallie117 0:6616d722fed3 161 u2 = EMG2;
Wallie117 1:0e1d91965b8e 162 y1 = highpass1.step(u1);
EmilyJCZ 9:a5c2ddf2cb8e 163 y2 = highpass2.step(u2);
Wallie117 1:0e1d91965b8e 164 y1 = fabs(y1);
Wallie117 1:0e1d91965b8e 165 y2 = fabs(y2);
Wallie117 0:6616d722fed3 166 y1 = lowpass1.step(y1)*cali_fact1;
EmilyJCZ 9:a5c2ddf2cb8e 167 y2 = lowpass2.step(y2)*cali_fact2;
Wallie117 0:6616d722fed3 168 }
Wallie117 1:0e1d91965b8e 169
EmilyJCZ 9:a5c2ddf2cb8e 170 //**************************** TICKER LOOPS ****************************************************************
EmilyJCZ 9:a5c2ddf2cb8e 171 /* 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.
EmilyJCZ 9:a5c2ddf2cb8e 172 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. */
Wallie117 0:6616d722fed3 173 void SENSOR_LOOP()
Wallie117 0:6616d722fed3 174 {
EmilyJCZ 3:f9a1df2271d2 175 Encoderread1 = wheel1.getPulses();
EmilyJCZ 6:1518fccc5616 176 previous_pos_arm = pos_arm;
EmilyJCZ 9:a5c2ddf2cb8e 177 pos_arm = (Encoderread1/((64.0*131.0)/360.0));
EmilyJCZ 3:f9a1df2271d2 178 pos_arm1 = pos_arm;
Wallie117 4:f9f75c913d7d 179 v_arm = (pos_arm - previous_pos_arm)/dt;
EmilyJCZ 3:f9a1df2271d2 180
EmilyJCZ 3:f9a1df2271d2 181 Encoderread2 = wheel2.getPulses();
EmilyJCZ 9:a5c2ddf2cb8e 182 pos_board = (Encoderread2/((64.0*131.0)/360.0));
EmilyJCZ 3:f9a1df2271d2 183 pos_board1 = pos_board;
EmilyJCZ 3:f9a1df2271d2 184
Wallie117 0:6616d722fed3 185 flag_s = true;
Wallie117 0:6616d722fed3 186 }
Wallie117 0:6616d722fed3 187
EmilyJCZ 9:a5c2ddf2cb8e 188 /* This loop determines if the calibration can be done or not. */
EmilyJCZ 9:a5c2ddf2cb8e 189 void EMG_LOOP()
Wallie117 1:0e1d91965b8e 190 {
EmilyJCZ 3:f9a1df2271d2 191 if(flag_calibration == false)
EmilyJCZ 3:f9a1df2271d2 192 {
EmilyJCZ 3:f9a1df2271d2 193 hoog_laag_filter();
EmilyJCZ 3:f9a1df2271d2 194 }
Wallie117 0:6616d722fed3 195 }
Wallie117 0:6616d722fed3 196
Wallie117 0:6616d722fed3 197 //================================================================================================== HEAD LOOP ================================================================================================================
EmilyJCZ 9:a5c2ddf2cb8e 198 /* Here is the main loop defined. First
EmilyJCZ 9:a5c2ddf2cb8e 199 Then the tickers are attached to the subloops above. Next some lines to turn led off and some lines that are shown in putty.
EmilyJCZ 9:a5c2ddf2cb8e 200 After this a while loop starts. This loop is always on. It starts with the calibration. Then the board returns to its begin settings.
EmilyJCZ 9:a5c2ddf2cb8e 201 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. */
EmilyJCZ 9:a5c2ddf2cb8e 202
Wallie117 1:0e1d91965b8e 203 int main()
Wallie117 0:6616d722fed3 204 {
Wallie117 0:6616d722fed3 205 pc.baud(115200);
EmilyJCZ 9:a5c2ddf2cb8e 206 Sensor_control.attach(&SENSOR_LOOP, 0.01);
EmilyJCZ 5:0597358d0016 207 EMG_Control.attach(&EMG_LOOP, (float)1/Fs);
Wallie117 1:0e1d91965b8e 208
EmilyJCZ 5:0597358d0016 209 led_green.write(0);
EmilyJCZ 5:0597358d0016 210 led_yellow.write(0);
EmilyJCZ 5:0597358d0016 211 led_red.write(0);
Wallie117 1:0e1d91965b8e 212
Wallie117 0:6616d722fed3 213 pc.printf("===============================================================\n");
Wallie117 0:6616d722fed3 214 pc.printf(" \t\t\t Ball-E\n");
EmilyJCZ 8:151e43b99156 215 pc.printf("In the module Biorobotics on the University of Twente this script is created\n");
Wallie117 0:6616d722fed3 216 pc.printf("Autors:\tE. Velu, L. Verhoeven, R. v/d Wal, T. v/d Wal, E. Zoetbrood\n\n");
Wallie117 0:6616d722fed3 217 wait(3);
Wallie117 0:6616d722fed3 218 pc.printf("The script will begin with a short calibration\n\n");
Wallie117 0:6616d722fed3 219 wait(2.5);
Wallie117 0:6616d722fed3 220 pc.printf("===============================================================\n");
EmilyJCZ 3:f9a1df2271d2 221
EmilyJCZ 9:a5c2ddf2cb8e 222 //**************************** CONTROL 1 - EMG CALIBRATION AND FILTERING ***********************************
EmilyJCZ 3:f9a1df2271d2 223 while(1)
EmilyJCZ 3:f9a1df2271d2 224 {
EmilyJCZ 9:a5c2ddf2cb8e 225 if (flag_calibration)
EmilyJCZ 3:f9a1df2271d2 226 {
EmilyJCZ 5:0597358d0016 227 calibration_measurements ++;
Wallie117 1:0e1d91965b8e 228 pc.printf(" \n\n EMG Signal starting up Calibration measurement........... \n");
EmilyJCZ 3:f9a1df2271d2 229
EmilyJCZ 9:a5c2ddf2cb8e 230 cali_max1 = 0;
EmilyJCZ 9:a5c2ddf2cb8e 231 cali_max2 = 0;
EmilyJCZ 9:a5c2ddf2cb8e 232 cali_fact1 = 0.9;
Wallie117 7:d55569b92f30 233 cali_fact2 = 0.9;
Wallie117 7:d55569b92f30 234
Wallie117 1:0e1d91965b8e 235 wait(2);
EmilyJCZ 5:0597358d0016 236 led_red.write(0);
Wallie117 1:0e1d91965b8e 237 wait(0.2);
EmilyJCZ 9:a5c2ddf2cb8e 238 led_red.write(1);
Wallie117 1:0e1d91965b8e 239 wait(0.2);
EmilyJCZ 5:0597358d0016 240 led_red.write(0);
Wallie117 1:0e1d91965b8e 241 wait(0.2);
EmilyJCZ 5:0597358d0016 242 led_red.write(1);
Wallie117 1:0e1d91965b8e 243 wait(0.2);
EmilyJCZ 5:0597358d0016 244 led_red.write(0);
Wallie117 1:0e1d91965b8e 245 wait(0.2);
EmilyJCZ 5:0597358d0016 246 led_red.write(1);
Wallie117 1:0e1d91965b8e 247 wait(1);
EmilyJCZ 3:f9a1df2271d2 248
Wallie117 1:0e1d91965b8e 249 pc.printf("\t.....Calibrating Signal of EMG 1 and 2 .......\n");
EmilyJCZ 5:0597358d0016 250 led_red.write(0);
EmilyJCZ 5:0597358d0016 251 led_yellow.write(1);
Wallie117 7:d55569b92f30 252
Wallie117 1:0e1d91965b8e 253 wait(0.5);
Wallie117 1:0e1d91965b8e 254 pc.printf("\t......contract muscles..... \n");
Wallie117 1:0e1d91965b8e 255
EmilyJCZ 9:a5c2ddf2cb8e 256 for(int cali_index1 = 0; cali_index1 < 2560; cali_index1++) // Records 5 seconds of y1.
EmilyJCZ 3:f9a1df2271d2 257 {
Wallie117 1:0e1d91965b8e 258 hoog_laag_filter();
Wallie117 1:0e1d91965b8e 259 cali_array1[cali_index1] = y1;
Wallie117 1:0e1d91965b8e 260 wait((float)1/Fs);
Wallie117 1:0e1d91965b8e 261 }
EmilyJCZ 9:a5c2ddf2cb8e 262 for(int cali_index2 = 0; cali_index2 < 2560; cali_index2++) // Records 5 seconds of y2.
EmilyJCZ 3:f9a1df2271d2 263 {
Wallie117 1:0e1d91965b8e 264 hoog_laag_filter();
Wallie117 1:0e1d91965b8e 265 cali_array2[cali_index2] = y2;
Wallie117 1:0e1d91965b8e 266 wait((float)1/Fs);
Wallie117 1:0e1d91965b8e 267 }
EmilyJCZ 9:a5c2ddf2cb8e 268 for(int cali_index3 = 0; cali_index3 < 2560; cali_index3++) // Scales y1.
EmilyJCZ 3:f9a1df2271d2 269 {
EmilyJCZ 3:f9a1df2271d2 270 if(cali_array1[cali_index3] > cali_max1)
EmilyJCZ 3:f9a1df2271d2 271 {
Wallie117 1:0e1d91965b8e 272 cali_max1 = cali_array1[cali_index3];
Wallie117 0:6616d722fed3 273 }
Wallie117 1:0e1d91965b8e 274 }
EmilyJCZ 9:a5c2ddf2cb8e 275 for(int cali_index4 = 0; cali_index4 < 2560; cali_index4++) // Scales y2.
EmilyJCZ 3:f9a1df2271d2 276 {
Wallie117 1:0e1d91965b8e 277 if(cali_array2[cali_index4] > cali_max2) {
Wallie117 1:0e1d91965b8e 278 cali_max2 = cali_array2[cali_index4];
Wallie117 0:6616d722fed3 279 }
Wallie117 1:0e1d91965b8e 280 }
EmilyJCZ 9:a5c2ddf2cb8e 281 cali_fact1 = (double)1/cali_max1; // Calibration factor for y1.
EmilyJCZ 9:a5c2ddf2cb8e 282 cali_fact2 = (double)1/cali_max2; // Calibration factor for y2.
Wallie117 1:0e1d91965b8e 283
EmilyJCZ 5:0597358d0016 284 led_yellow.write(0);
Wallie117 1:0e1d91965b8e 285 pc.printf(" \t....... Calibration has been completed!\n");
Wallie117 1:0e1d91965b8e 286 wait(0.2);
EmilyJCZ 5:0597358d0016 287 led_green.write(led_off);
Wallie117 1:0e1d91965b8e 288 wait(0.2);
EmilyJCZ 5:0597358d0016 289 led_green.write(led_on);
Wallie117 1:0e1d91965b8e 290 wait(0.2);
EmilyJCZ 5:0597358d0016 291 led_green.write(led_off);
Wallie117 1:0e1d91965b8e 292 wait(0.2);
EmilyJCZ 5:0597358d0016 293 led_green.write(led_on);
Wallie117 1:0e1d91965b8e 294 wait(4);
Wallie117 1:0e1d91965b8e 295 pc.printf("Beginning with Ball-E board settings\n");
EmilyJCZ 5:0597358d0016 296 led_green.write(led_off);
Wallie117 1:0e1d91965b8e 297 wait(2);
Wallie117 1:0e1d91965b8e 298 y1 = 0;
Wallie117 1:0e1d91965b8e 299 y2 = 0;
Wallie117 1:0e1d91965b8e 300
EmilyJCZ 9:a5c2ddf2cb8e 301 j = 1; // Wait for the fase swith to initiate.
Wallie117 1:0e1d91965b8e 302 fase_switch_wait = true;
EmilyJCZ 3:f9a1df2271d2 303 flag_calibration = false;
Wallie117 1:0e1d91965b8e 304 }
Wallie117 1:0e1d91965b8e 305
EmilyJCZ 9:a5c2ddf2cb8e 306 if (flag_s) // Turns calibration off.
EmilyJCZ 3:f9a1df2271d2 307 {
Wallie117 1:0e1d91965b8e 308 flag_calibration = false;
Wallie117 1:0e1d91965b8e 309 }
EmilyJCZ 9:a5c2ddf2cb8e 310 //**************************** FASE SWITCH *****************************************************************
EmilyJCZ 9:a5c2ddf2cb8e 311
EmilyJCZ 9:a5c2ddf2cb8e 312 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.
EmilyJCZ 3:f9a1df2271d2 313 {
Wallie117 1:0e1d91965b8e 314 j++;
EmilyJCZ 9:a5c2ddf2cb8e 315 wait(0.01);
Wallie117 1:0e1d91965b8e 316 pc.printf("waarde j = %i \n",j);
EmilyJCZ 5:0597358d0016 317 led_red.write(0);
EmilyJCZ 5:0597358d0016 318 led_green.write(1);
EmilyJCZ 5:0597358d0016 319 led_yellow.write(0);
Wallie117 1:0e1d91965b8e 320 }
Wallie117 1:0e1d91965b8e 321
EmilyJCZ 9:a5c2ddf2cb8e 322 if( j > N) // When the wait is long enough the game starts.
EmilyJCZ 3:f9a1df2271d2 323 {
Wallie117 1:0e1d91965b8e 324 fase_switch_wait = false;
EmilyJCZ 3:f9a1df2271d2 325 switch(fase)
EmilyJCZ 3:f9a1df2271d2 326 {
EmilyJCZ 9:a5c2ddf2cb8e 327 //************************ Fase 1 ******************************************************************
Wallie117 1:0e1d91965b8e 328 case(1):
EmilyJCZ 5:0597358d0016 329 led_red.write(1);
EmilyJCZ 5:0597358d0016 330 led_green.write(0);
EmilyJCZ 5:0597358d0016 331 led_yellow.write(0);
EmilyJCZ 5:0597358d0016 332 rounds = 0;
EmilyJCZ 9:a5c2ddf2cb8e 333
EmilyJCZ 9:a5c2ddf2cb8e 334 if (y1> contract) // If the left arm is above this value flag_left turns true so the motor can run ccw.
EmilyJCZ 9:a5c2ddf2cb8e 335 { // The left and right can not be true at the same time so right has to turn off when left is on.
Wallie117 1:0e1d91965b8e 336 flag_right = false;
Wallie117 1:0e1d91965b8e 337 flag_left = true;
Wallie117 0:6616d722fed3 338 }
Wallie117 1:0e1d91965b8e 339
EmilyJCZ 3:f9a1df2271d2 340 if (y2 > contract)
EmilyJCZ 3:f9a1df2271d2 341 {
Wallie117 1:0e1d91965b8e 342 flag_right = true;
Wallie117 1:0e1d91965b8e 343 flag_left = false;
Wallie117 0:6616d722fed3 344 }
Wallie117 1:0e1d91965b8e 345
EmilyJCZ 3:f9a1df2271d2 346 if (pos_board < maxleft)
EmilyJCZ 3:f9a1df2271d2 347 {
Wallie117 1:0e1d91965b8e 348 flag_left = false;
Wallie117 1:0e1d91965b8e 349 motor2speed.write(relax);
Wallie117 1:0e1d91965b8e 350 }
Wallie117 1:0e1d91965b8e 351
EmilyJCZ 3:f9a1df2271d2 352 if (pos_board > maxright)
EmilyJCZ 3:f9a1df2271d2 353 {
Wallie117 1:0e1d91965b8e 354 flag_right = false;
Wallie117 1:0e1d91965b8e 355 motor2speed.write(relax);
Wallie117 0:6616d722fed3 356 }
Wallie117 1:0e1d91965b8e 357
EmilyJCZ 6:1518fccc5616 358 if (flag_left)
EmilyJCZ 3:f9a1df2271d2 359 {
EmilyJCZ 3:f9a1df2271d2 360 if (y1> contract)
EmilyJCZ 3:f9a1df2271d2 361 {
Wallie117 1:0e1d91965b8e 362 motor2direction.write(ccw);
Wallie117 1:0e1d91965b8e 363 motor2speed.write(speed_plate_1);
EmilyJCZ 3:f9a1df2271d2 364 }
EmilyJCZ 3:f9a1df2271d2 365 else
EmilyJCZ 3:f9a1df2271d2 366 {
Wallie117 1:0e1d91965b8e 367 motor2speed.write(relax);
Wallie117 1:0e1d91965b8e 368 }
Wallie117 1:0e1d91965b8e 369 }
Wallie117 1:0e1d91965b8e 370
EmilyJCZ 6:1518fccc5616 371 if (flag_right)
EmilyJCZ 3:f9a1df2271d2 372 {
EmilyJCZ 3:f9a1df2271d2 373 if (y2 > contract)
EmilyJCZ 3:f9a1df2271d2 374 {
Wallie117 1:0e1d91965b8e 375 motor2direction.write(cw);
Wallie117 1:0e1d91965b8e 376 motor2speed.write(speed_plate_1);
EmilyJCZ 3:f9a1df2271d2 377 }
EmilyJCZ 3:f9a1df2271d2 378 else
EmilyJCZ 3:f9a1df2271d2 379 {
Wallie117 1:0e1d91965b8e 380 motor2speed.write(relax);
Wallie117 1:0e1d91965b8e 381 }
Wallie117 0:6616d722fed3 382 }
Wallie117 1:0e1d91965b8e 383 pc.printf("Boardposition \t %f EMG1 en EMG2 signaal = %f \t %f\n", pos_board, y1, y2);
EmilyJCZ 3:f9a1df2271d2 384 if (y1> fasecontract && y2 > fasecontract)
EmilyJCZ 3:f9a1df2271d2 385 {
Wallie117 1:0e1d91965b8e 386 motor2speed.write(relax);
Wallie117 1:0e1d91965b8e 387 fase = 2;
Wallie117 1:0e1d91965b8e 388 fase_switch_wait = true;
EmilyJCZ 5:0597358d0016 389 led_red.write(0);
EmilyJCZ 5:0597358d0016 390 led_green.write(0);
EmilyJCZ 5:0597358d0016 391 led_yellow.write(1);
Wallie117 1:0e1d91965b8e 392 j = 0;
Wallie117 1:0e1d91965b8e 393 }
Wallie117 1:0e1d91965b8e 394 break;
EmilyJCZ 9:a5c2ddf2cb8e 395 //************************ Fase 2 ******************************************************************
EmilyJCZ 9:a5c2ddf2cb8e 396
Wallie117 1:0e1d91965b8e 397 case(2):
EmilyJCZ 5:0597358d0016 398 led_red.write(0);
EmilyJCZ 5:0597358d0016 399 led_green.write(0);
EmilyJCZ 5:0597358d0016 400 led_yellow.write(1);
Wallie117 1:0e1d91965b8e 401 motor1direction.write(cw);
EmilyJCZ 5:0597358d0016 402 pos_arm1 = (pos_arm - (rounds * 360));
EmilyJCZ 5:0597358d0016 403 pos_arm2 = pos_arm1;
EmilyJCZ 5:0597358d0016 404
EmilyJCZ 5:0597358d0016 405 switch(switch_rounds)
EmilyJCZ 3:f9a1df2271d2 406 {
Wallie117 0:6616d722fed3 407 case(1):
EmilyJCZ 5:0597358d0016 408 rounds ++;
EmilyJCZ 5:0597358d0016 409 switch_rounds = 2;
Wallie117 1:0e1d91965b8e 410 break;
Wallie117 0:6616d722fed3 411 case(2):
EmilyJCZ 3:f9a1df2271d2 412 if(pos_arm1>360 & 368<pos_arm1)
EmilyJCZ 3:f9a1df2271d2 413 {
EmilyJCZ 5:0597358d0016 414 switch_rounds = 1;
Wallie117 1:0e1d91965b8e 415 }
Wallie117 1:0e1d91965b8e 416 break;
Wallie117 0:6616d722fed3 417 }
Wallie117 1:0e1d91965b8e 418
EmilyJCZ 3:f9a1df2271d2 419 if (y2 > minimum_contract & y2 < medium_contract)
EmilyJCZ 3:f9a1df2271d2 420 {
Wallie117 7:d55569b92f30 421 speed_control_arm = 1;//((v_arm/304) + 0.15*(1 -(v_arm/304)));
Wallie117 4:f9f75c913d7d 422 motor1speed.write(speed_control_arm);
Wallie117 1:0e1d91965b8e 423 }
EmilyJCZ 3:f9a1df2271d2 424 if (y2 > medium_contract) // Hoger dan drempelwaarde = Actief
EmilyJCZ 3:f9a1df2271d2 425 {
Wallie117 7:d55569b92f30 426 speed_control_arm = 1;//((v_arm/304) + 0.15*(1 -(v_arm/304)));
EmilyJCZ 5:0597358d0016 427 motor1speed.write(speed_control_arm);
Wallie117 1:0e1d91965b8e 428 }
EmilyJCZ 3:f9a1df2271d2 429 if (y2 < minimum_contract) // Lager dan drempel = Rust
EmilyJCZ 3:f9a1df2271d2 430 {
Wallie117 1:0e1d91965b8e 431 motor1speed.write(relax);
Wallie117 0:6616d722fed3 432 }
Wallie117 1:0e1d91965b8e 433
EmilyJCZ 5:0597358d0016 434 if(rounds == pos_armrounds_max) // max aantal draaing gemaakt!!!!!!
EmilyJCZ 3:f9a1df2271d2 435 {
Wallie117 1:0e1d91965b8e 436 problem1 = true;
Wallie117 1:0e1d91965b8e 437 problem2 = true;
Wallie117 1:0e1d91965b8e 438 motor1speed.write(relax);
EmilyJCZ 3:f9a1df2271d2 439
EmilyJCZ 3:f9a1df2271d2 440 while (problem1)
EmilyJCZ 3:f9a1df2271d2 441 {
Wallie117 1:0e1d91965b8e 442 j++;
Wallie117 1:0e1d91965b8e 443 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.
Wallie117 1:0e1d91965b8e 444
EmilyJCZ 3:f9a1df2271d2 445 if( j > N)
EmilyJCZ 3:f9a1df2271d2 446 {
EmilyJCZ 3:f9a1df2271d2 447 problem1 = false;
Wallie117 1:0e1d91965b8e 448 }
EmilyJCZ 3:f9a1df2271d2 449 }
EmilyJCZ 3:f9a1df2271d2 450
Wallie117 1:0e1d91965b8e 451 wait(0.1);
EmilyJCZ 5:0597358d0016 452 led_red.write(0);
Wallie117 1:0e1d91965b8e 453 wait(0.1);
EmilyJCZ 5:0597358d0016 454 led_red.write(1);
Wallie117 1:0e1d91965b8e 455 wait(0.1);
EmilyJCZ 5:0597358d0016 456 led_red.write(0);
Wallie117 1:0e1d91965b8e 457 wait(0.1);
EmilyJCZ 5:0597358d0016 458 led_red.write(1);
Wallie117 1:0e1d91965b8e 459 wait(0.1);
EmilyJCZ 5:0597358d0016 460 led_red.write(0);
Wallie117 1:0e1d91965b8e 461 wait(0.1);
EmilyJCZ 5:0597358d0016 462 led_red.write(1);
Wallie117 2:de3bb38dae4e 463 wait(1.5);
Wallie117 1:0e1d91965b8e 464
EmilyJCZ 3:f9a1df2271d2 465 while(problem2)
EmilyJCZ 3:f9a1df2271d2 466 {
Wallie117 2:de3bb38dae4e 467 motor1direction.write(ccw);
EmilyJCZ 6:1518fccc5616 468 if(pos_arm < 170)
EmilyJCZ 6:1518fccc5616 469 {
EmilyJCZ 6:1518fccc5616 470 if(v_arm > 200)
EmilyJCZ 6:1518fccc5616 471 {
Wallie117 4:f9f75c913d7d 472 flag_v_arm = true;
Wallie117 4:f9f75c913d7d 473 }
EmilyJCZ 6:1518fccc5616 474 }
EmilyJCZ 6:1518fccc5616 475 if(flag_v_arm)
EmilyJCZ 6:1518fccc5616 476 {
Wallie117 4:f9f75c913d7d 477 v_arm_com = v_arm;
EmilyJCZ 6:1518fccc5616 478 }
EmilyJCZ 5:0597358d0016 479 speed_control_arm = (0.4*((pos_arm2 - reset_arm)/500.0) + (0.05) - (v_arm_com/304)*0.3);
Wallie117 4:f9f75c913d7d 480 motor1speed.write(speed_control_arm);
EmilyJCZ 3:f9a1df2271d2 481
EmilyJCZ 3:f9a1df2271d2 482 if (pos_arm < 10)
EmilyJCZ 3:f9a1df2271d2 483 {
Wallie117 4:f9f75c913d7d 484 flag_v_arm = false;
Wallie117 1:0e1d91965b8e 485 problem2 = false;
Wallie117 2:de3bb38dae4e 486 motor1speed.write(0);
EmilyJCZ 5:0597358d0016 487 rounds = 0;
Wallie117 1:0e1d91965b8e 488 wait(1);
Wallie117 1:0e1d91965b8e 489 }
Wallie117 1:0e1d91965b8e 490 }
Wallie117 0:6616d722fed3 491 }
EmilyJCZ 3:f9a1df2271d2 492 if (pos_arm1 > minimum_throw_angle & pos_arm1 < maximum_throw_angle)
EmilyJCZ 3:f9a1df2271d2 493 {
EmilyJCZ 3:f9a1df2271d2 494 if (y1> maximum_leftbiceps)
EmilyJCZ 3:f9a1df2271d2 495 {
Wallie117 0:6616d722fed3 496 magnet.write(off);
Wallie117 7:d55569b92f30 497 wait(0.1);
Wallie117 0:6616d722fed3 498 motor1speed.write(relax);
Wallie117 0:6616d722fed3 499 fase = 3;
Wallie117 0:6616d722fed3 500 pc.printf("Van fase 2 naar fase 3\n");
EmilyJCZ 3:f9a1df2271d2 501
Wallie117 0:6616d722fed3 502 wait(2);
Wallie117 0:6616d722fed3 503 j = 0;
Wallie117 1:0e1d91965b8e 504 fase_switch_wait = true;
Wallie117 0:6616d722fed3 505 }
Wallie117 1:0e1d91965b8e 506 }
EmilyJCZ 5:0597358d0016 507 pc.printf("Snelheid arm = \t %f \t EMG1 en EMG2 = %f \t %f \n", speed_control_arm, y1, y2);
Wallie117 0:6616d722fed3 508 break;
EmilyJCZ 9:a5c2ddf2cb8e 509 //************************ Fase 3 ******************************************************************
Wallie117 1:0e1d91965b8e 510 case(3):
EmilyJCZ 5:0597358d0016 511 led_red.write(0);
EmilyJCZ 5:0597358d0016 512 led_green.write(1);
EmilyJCZ 5:0597358d0016 513 led_yellow.write(0);
EmilyJCZ 3:f9a1df2271d2 514 switch(switch_reset)
EmilyJCZ 3:f9a1df2271d2 515 {
Wallie117 1:0e1d91965b8e 516 case(1):
EmilyJCZ 5:0597358d0016 517 if(pos_arm < reset_position) //ene kant op draaien
EmilyJCZ 3:f9a1df2271d2 518 {
Wallie117 1:0e1d91965b8e 519 motor1direction.write(cw);
EmilyJCZ 3:f9a1df2271d2 520 speedcompensation2 = 0.05; //((reset_arm - pos_arm1)/900.0 + (0.02));
Wallie117 1:0e1d91965b8e 521 motor1speed.write(speedcompensation2);
Wallie117 1:0e1d91965b8e 522 }
EmilyJCZ 5:0597358d0016 523 if(pos_arm > reset_position) //andere kant op
EmilyJCZ 3:f9a1df2271d2 524 {
EmilyJCZ 3:f9a1df2271d2 525 motor1direction.write(ccw);
EmilyJCZ 3:f9a1df2271d2 526 speedcompensation2 = 0.05;//((pos_arm1 - reset_arm)/500.0 + (0.02));
EmilyJCZ 3:f9a1df2271d2 527 motor1speed.write(speedcompensation2);
EmilyJCZ 3:f9a1df2271d2 528 }
EmilyJCZ 5:0597358d0016 529 if(pos_arm < reset_position+5 && pos_arm > reset_position-5) // Dit gaat niet goed komen, omdat het precies die waarde moet zijn
EmilyJCZ 3:f9a1df2271d2 530 {
EmilyJCZ 3:f9a1df2271d2 531 motor1speed.write(0);
EmilyJCZ 3:f9a1df2271d2 532 switch_reset = 2;
EmilyJCZ 3:f9a1df2271d2 533 }
Wallie117 1:0e1d91965b8e 534 pc.printf("Positie_arm = %f \t \t snelheid = %f \n",pos_arm, speedcompensation);
Wallie117 1:0e1d91965b8e 535 wait(0.0001);
Wallie117 1:0e1d91965b8e 536 break;
Wallie117 1:0e1d91965b8e 537
Wallie117 1:0e1d91965b8e 538 case(2):
Wallie117 1:0e1d91965b8e 539 pc.printf("\t switch magneet automatisch \n");
Wallie117 2:de3bb38dae4e 540 wait(0.2);
Wallie117 1:0e1d91965b8e 541 magnet.write(on);
Wallie117 2:de3bb38dae4e 542 wait(2);
Wallie117 1:0e1d91965b8e 543 switch_reset = 3;
Wallie117 1:0e1d91965b8e 544 break;
EmilyJCZ 3:f9a1df2271d2 545
Wallie117 1:0e1d91965b8e 546 case(3):
EmilyJCZ 3:f9a1df2271d2 547 if(pos_board < reset_board)
EmilyJCZ 3:f9a1df2271d2 548 {
Wallie117 1:0e1d91965b8e 549 motor2direction.write(cw);
Wallie117 2:de3bb38dae4e 550 speedcompensation = 0.05; //((reset_board - pos_board1)/500.0 + (0.1));
Wallie117 1:0e1d91965b8e 551 motor2speed.write(speedcompensation);
Wallie117 1:0e1d91965b8e 552 }
Wallie117 0:6616d722fed3 553
EmilyJCZ 3:f9a1df2271d2 554 if(pos_board > reset_board)
EmilyJCZ 3:f9a1df2271d2 555 {
Wallie117 1:0e1d91965b8e 556 motor2direction.write(ccw);
Wallie117 2:de3bb38dae4e 557 speedcompensation = 0.05;//((pos_board1 - reset_board)/500.0 + (0.05));
Wallie117 1:0e1d91965b8e 558 motor2speed.write(speedcompensation);
Wallie117 0:6616d722fed3 559 }
EmilyJCZ 3:f9a1df2271d2 560
EmilyJCZ 3:f9a1df2271d2 561 if(pos_board < reset_board+5 && pos_board > reset_board-5)
EmilyJCZ 3:f9a1df2271d2 562 {
Wallie117 1:0e1d91965b8e 563 motor2speed.write(0);
EmilyJCZ 5:0597358d0016 564 board = true;
Wallie117 0:6616d722fed3 565 }
EmilyJCZ 3:f9a1df2271d2 566
EmilyJCZ 6:1518fccc5616 567 if(board)
EmilyJCZ 3:f9a1df2271d2 568 {
Wallie117 1:0e1d91965b8e 569 pc.printf("fase switch naar 1\n\n");
EmilyJCZ 5:0597358d0016 570 board = false;
Wallie117 1:0e1d91965b8e 571 wait(2);
Wallie117 1:0e1d91965b8e 572 games_played ++;
Wallie117 7:d55569b92f30 573 games_played1 = games_played - (5*calibration_measurements - 5);
Wallie117 1:0e1d91965b8e 574 pc.printf("Games played total count = %i and loop %i\n",games_played,games_played1);
Wallie117 1:0e1d91965b8e 575
Wallie117 7:d55569b92f30 576 if(games_played1 == 5)
EmilyJCZ 3:f9a1df2271d2 577 {
Wallie117 1:0e1d91965b8e 578 flag_calibration = true;
EmilyJCZ 3:f9a1df2271d2 579 while(t)
EmilyJCZ 3:f9a1df2271d2 580 {
Wallie117 1:0e1d91965b8e 581 pc.printf("\tCalibratie begint over %i \n",t);
Wallie117 1:0e1d91965b8e 582 t--;
EmilyJCZ 5:0597358d0016 583 led_yellow.write(1);
Wallie117 1:0e1d91965b8e 584 wait(0.5);
EmilyJCZ 5:0597358d0016 585 led_yellow.write(0);
Wallie117 1:0e1d91965b8e 586 wait(0.5);
Wallie117 1:0e1d91965b8e 587 }
Wallie117 1:0e1d91965b8e 588 }
EmilyJCZ 3:f9a1df2271d2 589 while(t)
EmilyJCZ 3:f9a1df2271d2 590 {
Wallie117 1:0e1d91965b8e 591 pc.printf("\tNieuw spel begint over %i \n",t);
Wallie117 1:0e1d91965b8e 592 t--;
EmilyJCZ 5:0597358d0016 593 led_yellow.write(1);
Wallie117 1:0e1d91965b8e 594 wait(0.5);
EmilyJCZ 5:0597358d0016 595 led_yellow.write(0);
Wallie117 1:0e1d91965b8e 596 wait(0.5);
Wallie117 1:0e1d91965b8e 597 }
Wallie117 1:0e1d91965b8e 598
Wallie117 0:6616d722fed3 599 fase = 1; // Terug naar fase 1
Wallie117 0:6616d722fed3 600 switch_reset = 1; // De switch op orginele locatie zetten
Wallie117 0:6616d722fed3 601 t = 5;
Wallie117 1:0e1d91965b8e 602
Wallie117 1:0e1d91965b8e 603 }
Wallie117 1:0e1d91965b8e 604
Wallie117 1:0e1d91965b8e 605 pc.printf("Positie_board = %f \t \t snelheid = %f \n",pos_board, speedcompensation);
Wallie117 1:0e1d91965b8e 606 wait(0.0001);
Wallie117 1:0e1d91965b8e 607 break;
Wallie117 0:6616d722fed3 608 }
Wallie117 1:0e1d91965b8e 609 break;
EmilyJCZ 9:a5c2ddf2cb8e 610 //================================================================================================== END SCRIPT ===============================================================================================================
Wallie117 0:6616d722fed3 611 }
Wallie117 1:0e1d91965b8e 612 }
Wallie117 0:6616d722fed3 613 }
Wallie117 0:6616d722fed3 614 }