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@10:57f090f3ddda, 2015-10-29 (annotated)

Committer:
Wallie117
Date:
Thu Oct 29 15:16:35 2015 +0000
Revision:
10:57f090f3ddda
Parent:
9:a5c2ddf2cb8e 
Final script of Ball-E for project.

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 =============================================================================================
Wallie117 10:57f090f3ddda 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 *****************************************************
Wallie117 10:57f090f3ddda 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.
Wallie117 10:57f090f3ddda 31 DigitalOut magnet(D2); // Output for magnet.
Wallie117 10:57f090f3ddda 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 =====================================================================================================
Wallie117 10:57f090f3ddda 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.
Wallie117 10:57f090f3ddda 43 Second for the control of the motor and than for the different controls and fase switch. At the bottom tickers are defined.
Wallie117 10:57f090f3ddda 44 Tickers count constantly. Formulas can be attacht to them so they are contantly calculated. */
EmilyJCZ 9:a5c2ddf2cb8e 45
Wallie117 10:57f090f3ddda 46 const int led_on = 0; // This constant turns the led on.
Wallie117 10:57f090f3ddda 47 const int led_off = 1; // This constant turns the led off.
Wallie117 10:57f090f3ddda 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.
Wallie117 10:57f090f3ddda 50 double dt = 0.01; // Time staps
Wallie117 1:0e1d91965b8e 51
EmilyJCZ 5:0597358d0016 52 //**************************** VARIABLES FOR MOTOR CONTROL *********************************
Wallie117 10:57f090f3ddda 53 const int cw = 1; // Motor1 moves counter clock wise and Motor2 moves clock wise.
Wallie117 10:57f090f3ddda 54 const int 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.
Wallie117 10:57f090f3ddda 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.
Wallie117 10:57f090f3ddda 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;
Wallie117 10:57f090f3ddda 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;
Wallie117 10:57f090f3ddda 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.
Wallie117 10:57f090f3ddda 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 ***********************************
Wallie117 10:57f090f3ddda 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 *************************************
Wallie117 10:57f090f3ddda 93 const double contract = 0.5; // The minimum value for y1 and y2 for which the motor moves.
Wallie117 10:57f090f3ddda 94 const double fasecontract = 0.7; // The value y1 and y2 must be for the fase change.
Wallie117 10:57f090f3ddda 95 const double maxleft = -200; // With this angle the motor should stop moving.
Wallie117 10:57f090f3ddda 96 const double maxright = 200; // With this angle the motor should stop moving.
Wallie117 10:57f090f3ddda 97 const double speed_plate_1 = 0.1; // The speed of the plate in control 2.
Wallie117 10:57f090f3ddda 98 bool flag_left = true; // This flag determines if the signals from the left biceps should be measured. This is signal y1.
Wallie117 10:57f090f3ddda 99 bool flag_right = true; // This flag determines if the signals from the right biceps should be measured. This is signal y2.
Wallie117 10:57f090f3ddda 100 double pos_board = 0; // The begin position of the board. It begins at zero. Reads Encoder2 in degrees. Formula is applied in SensorTicker.
Wallie117 10:57f090f3ddda 101 int pos_board1 = 0; //
Wallie117 10:57f090f3ddda 102 double Encoderread2 = 0; // Reads travelled distance from Motor2. Reads pulses.
Wallie117 0:6616d722fed3 103
EmilyJCZ 5:0597358d0016 104 //**************************** VARIABLES FOR CONTROL 3 *************************************
Wallie117 10:57f090f3ddda 105 const double minimum_contract = 0.4; // The minimum value for y2 for whicht the motor moves.
Wallie117 10:57f090f3ddda 106 const double medium_contract = 0.5; // Value for different speeds of the motor.
Wallie117 10:57f090f3ddda 107 const double maximum_leftbiceps = 0.7; // Value for y1 for which the magnet turns off.
Wallie117 10:57f090f3ddda 108 const double on = 1.0; // This value turns the magnet on.
Wallie117 10:57f090f3ddda 109 const double off = 0.0; // This value turns the magnet off.
Wallie117 10:57f090f3ddda 110 const double minimum_throw_angle = 35; // The minimum angle the arm has to be in to be able to turn the magnet off.
Wallie117 10:57f090f3ddda 111 const double maximum_throw_angle = 100; // The maximum angle for the arm to turn the magnet off.
Wallie117 10:57f090f3ddda 112 double 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.
Wallie117 10:57f090f3ddda 114 double pos_arm2 = 0; // This makes from the positon of the arm degrees within a cirkel as an float.
Wallie117 10:57f090f3ddda 115 double previous_pos_arm = 0; // Needed to calculate the v_arm.
Wallie117 10:57f090f3ddda 116 double v_arm = 0; // The speed of the arm.
Wallie117 10:57f090f3ddda 117 double v_arm_com = 0; // The compensated speed of the arm.
Wallie117 10:57f090f3ddda 118 double speed_control_arm = 0.000; //
Wallie117 10:57f090f3ddda 119 double Encoderread1 = 0; // Reads travelled distance from Motor1.
Wallie117 10:57f090f3ddda 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.
Wallie117 10:57f090f3ddda 122 double pos_armrounds_max = 2; // Max rounds the arm can make.
EmilyJCZ 6:1518fccc5616 123 bool problem1 = false; // Stop for fase 2. It is a wait for problem2 begins.
Wallie117 10:57f090f3ddda 124 bool problem2 = false; // The reset of fase 2 when the arm reaches its maximum value.
Wallie117 10:57f090f3ddda 125 bool flag_v_arm = false; //
Wallie117 10:57f090f3ddda 126 double problem_velocity = 0; //
Wallie117 0:6616d722fed3 127
EmilyJCZ 5:0597358d0016 128 //**************************** VARIABLES FOR CONTROL 4 *************************************
Wallie117 10:57f090f3ddda 129 double reset_position = 0; // The reset position of the arm.
Wallie117 10:57f090f3ddda 130 double reset_arm = 0; // The reset position of the arm (?).
Wallie117 10:57f090f3ddda 131 double reset_board = 0; // The reset position of the board.
Wallie117 10:57f090f3ddda 132 double speedcompensation; // Speed of Motor2 during reset.
Wallie117 10:57f090f3ddda 133 double 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 10:57f090f3ddda 137 bool flag_calibration_LED = false;
Wallie117 10:57f090f3ddda 138 bool reset_case3 = false;
Wallie117 10:57f090f3ddda 139 bool flag_arm_magnet = false;
Wallie117 0:6616d722fed3 140
EmilyJCZ 5:0597358d0016 141 // **************************************** Tickers ****************************************
EmilyJCZ 5:0597358d0016 142 Ticker Sensor_control; // This ticker counts for the position of the motors and the speed of the arm.
EmilyJCZ 5:0597358d0016 143 Ticker EMG_Control; // This ticker counts for the filtering of the EMG signal
Wallie117 10:57f090f3ddda 144 Ticker LED_control;
Wallie117 0:6616d722fed3 145
Wallie117 0:6616d722fed3 146 //=============================================================================================== SUB LOOPS ==================================================================================================================
EmilyJCZ 9:a5c2ddf2cb8e 147 /* 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.
Wallie117 10:57f090f3ddda 148 Then there are two loops which are attached to a ticker so they are read through constantly.
EmilyJCZ 9:a5c2ddf2cb8e 149 One is for the reading of the encoders and one for turning on and off the EMG signal calibration and filtering. */
EmilyJCZ 9:a5c2ddf2cb8e 150
EmilyJCZ 9:a5c2ddf2cb8e 151 //**************************** CONTROL 1 - EMG CALIBRATION AND FILTERING ***********************************
Wallie117 10:57f090f3ddda 152 /* 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 153 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.
Wallie117 10:57f090f3ddda 154 The filter order is highpass, recifyer and lowpass. The calibration is done at the same time as the lowpass filter.
EmilyJCZ 9:a5c2ddf2cb8e 155 The signal is multiplied with the calibarion factor to put the signal between 0 and 1. */
EmilyJCZ 9:a5c2ddf2cb8e 156
Wallie117 10:57f090f3ddda 157 biquadFilter highpass1(high_a2, high_a3, high_b1, high_b2, high_b3);
Wallie117 0:6616d722fed3 158 biquadFilter highpass2(high_a2, high_a3, high_b1, high_b2, high_b3);
Wallie117 0:6616d722fed3 159 biquadFilter lowpass1(low_a2, low_a3, low_b1, low_b2, low_b3);
Wallie117 0:6616d722fed3 160 biquadFilter lowpass2(low_a2, low_a3, low_b1, low_b2, low_b3);
Wallie117 0:6616d722fed3 161
Wallie117 1:0e1d91965b8e 162 void hoog_laag_filter()
Wallie117 0:6616d722fed3 163 {
Wallie117 1:0e1d91965b8e 164 u1 = EMG1;
Wallie117 0:6616d722fed3 165 u2 = EMG2;
Wallie117 1:0e1d91965b8e 166 y1 = highpass1.step(u1);
Wallie117 10:57f090f3ddda 167 y2 = highpass2.step(u2);
Wallie117 1:0e1d91965b8e 168 y1 = fabs(y1);
Wallie117 1:0e1d91965b8e 169 y2 = fabs(y2);
Wallie117 0:6616d722fed3 170 y1 = lowpass1.step(y1)*cali_fact1;
Wallie117 10:57f090f3ddda 171 y2 = lowpass2.step(y2)*cali_fact2;
Wallie117 0:6616d722fed3 172 }
Wallie117 1:0e1d91965b8e 173
EmilyJCZ 9:a5c2ddf2cb8e 174 //**************************** TICKER LOOPS ****************************************************************
Wallie117 10:57f090f3ddda 175 /* 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 176 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 177 void SENSOR_LOOP()
Wallie117 0:6616d722fed3 178 {
EmilyJCZ 3:f9a1df2271d2 179 Encoderread1 = wheel1.getPulses();
EmilyJCZ 6:1518fccc5616 180 previous_pos_arm = pos_arm;
Wallie117 10:57f090f3ddda 181 pos_arm = (Encoderread1/((64.0*131.0)/360.0));
EmilyJCZ 3:f9a1df2271d2 182 pos_arm1 = pos_arm;
Wallie117 4:f9f75c913d7d 183 v_arm = (pos_arm - previous_pos_arm)/dt;
EmilyJCZ 3:f9a1df2271d2 184
EmilyJCZ 3:f9a1df2271d2 185 Encoderread2 = wheel2.getPulses();
Wallie117 10:57f090f3ddda 186 pos_board = (Encoderread2/((64.0*131.0)/360.0));
EmilyJCZ 3:f9a1df2271d2 187 pos_board1 = pos_board;
Wallie117 10:57f090f3ddda 188
Wallie117 0:6616d722fed3 189 flag_s = true;
Wallie117 0:6616d722fed3 190 }
Wallie117 0:6616d722fed3 191
Wallie117 10:57f090f3ddda 192 void LED_LOOP()
Wallie117 1:0e1d91965b8e 193 {
Wallie117 10:57f090f3ddda 194 if(flag_calibration_LED == true) {
Wallie117 10:57f090f3ddda 195 led_red.write(1);
Wallie117 10:57f090f3ddda 196 led_green.write(0);
Wallie117 10:57f090f3ddda 197 led_yellow.write(0);
Wallie117 10:57f090f3ddda 198 wait(0.1);
Wallie117 10:57f090f3ddda 199 led_red.write(1);
Wallie117 10:57f090f3ddda 200 led_green.write(1);
Wallie117 10:57f090f3ddda 201 led_yellow.write(0);
Wallie117 10:57f090f3ddda 202 wait(0.1);
Wallie117 10:57f090f3ddda 203 led_red.write(0);
Wallie117 10:57f090f3ddda 204 led_green.write(1);
Wallie117 10:57f090f3ddda 205 led_yellow.write(0);
Wallie117 10:57f090f3ddda 206 wait(0.1);
Wallie117 10:57f090f3ddda 207 led_red.write(0);
Wallie117 10:57f090f3ddda 208 led_green.write(1);
Wallie117 10:57f090f3ddda 209 led_yellow.write(1);
Wallie117 10:57f090f3ddda 210 wait(0.1);
Wallie117 10:57f090f3ddda 211 led_red.write(0);
Wallie117 10:57f090f3ddda 212 led_green.write(0);
Wallie117 10:57f090f3ddda 213 led_yellow.write(1);
Wallie117 10:57f090f3ddda 214 wait(0.1);
Wallie117 10:57f090f3ddda 215 led_red.write(0);
Wallie117 10:57f090f3ddda 216 led_green.write(0);
Wallie117 10:57f090f3ddda 217 led_yellow.write(0);
Wallie117 10:57f090f3ddda 218 wait(0.1);
Wallie117 10:57f090f3ddda 219 }
Wallie117 10:57f090f3ddda 220 }
Wallie117 10:57f090f3ddda 221 /* This loop determines if the calibration can be done or not. */
Wallie117 10:57f090f3ddda 222 void EMG_LOOP()
Wallie117 10:57f090f3ddda 223 {
Wallie117 10:57f090f3ddda 224 if(flag_calibration == false) {
EmilyJCZ 3:f9a1df2271d2 225 hoog_laag_filter();
EmilyJCZ 3:f9a1df2271d2 226 }
Wallie117 0:6616d722fed3 227 }
Wallie117 0:6616d722fed3 228
Wallie117 0:6616d722fed3 229 //================================================================================================== HEAD LOOP ================================================================================================================
Wallie117 10:57f090f3ddda 230 /* Here is the main loop defined. First
EmilyJCZ 9:a5c2ddf2cb8e 231 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 232 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 233 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 234
Wallie117 1:0e1d91965b8e 235 int main()
Wallie117 0:6616d722fed3 236 {
Wallie117 0:6616d722fed3 237 pc.baud(115200);
Wallie117 10:57f090f3ddda 238 Sensor_control.attach(&SENSOR_LOOP, 0.01);
EmilyJCZ 5:0597358d0016 239 EMG_Control.attach(&EMG_LOOP, (float)1/Fs);
Wallie117 10:57f090f3ddda 240 LED_control.attach(&LED_LOOP, 1);
Wallie117 1:0e1d91965b8e 241
EmilyJCZ 5:0597358d0016 242 led_green.write(0);
EmilyJCZ 5:0597358d0016 243 led_yellow.write(0);
EmilyJCZ 5:0597358d0016 244 led_red.write(0);
Wallie117 1:0e1d91965b8e 245
Wallie117 0:6616d722fed3 246 pc.printf("===============================================================\n");
Wallie117 0:6616d722fed3 247 pc.printf(" \t\t\t Ball-E\n");
EmilyJCZ 8:151e43b99156 248 pc.printf("In the module Biorobotics on the University of Twente this script is created\n");
Wallie117 0:6616d722fed3 249 pc.printf("Autors:\tE. Velu, L. Verhoeven, R. v/d Wal, T. v/d Wal, E. Zoetbrood\n\n");
Wallie117 10:57f090f3ddda 250 led_red.write(1);
Wallie117 10:57f090f3ddda 251 led_green.write(1);
Wallie117 10:57f090f3ddda 252 led_yellow.write(1);
Wallie117 0:6616d722fed3 253 wait(3);
Wallie117 10:57f090f3ddda 254 led_red.write(0);
Wallie117 10:57f090f3ddda 255 led_green.write(0);
Wallie117 10:57f090f3ddda 256 led_yellow.write(0);
Wallie117 0:6616d722fed3 257 pc.printf("The script will begin with a short calibration\n\n");
Wallie117 0:6616d722fed3 258 wait(2.5);
Wallie117 10:57f090f3ddda 259 led_red.write(1);
Wallie117 10:57f090f3ddda 260 led_green.write(1);
Wallie117 10:57f090f3ddda 261 led_yellow.write(1);
Wallie117 0:6616d722fed3 262 pc.printf("===============================================================\n");
EmilyJCZ 3:f9a1df2271d2 263
EmilyJCZ 9:a5c2ddf2cb8e 264 //**************************** CONTROL 1 - EMG CALIBRATION AND FILTERING ***********************************
Wallie117 10:57f090f3ddda 265 while(1) {
Wallie117 10:57f090f3ddda 266 if (flag_calibration) {
Wallie117 10:57f090f3ddda 267 wait(0.4);
Wallie117 10:57f090f3ddda 268 led_red.write(0);
Wallie117 10:57f090f3ddda 269 led_green.write(0);
Wallie117 10:57f090f3ddda 270 led_yellow.write(0);
EmilyJCZ 5:0597358d0016 271 calibration_measurements ++;
Wallie117 1:0e1d91965b8e 272 pc.printf(" \n\n EMG Signal starting up Calibration measurement........... \n");
Wallie117 10:57f090f3ddda 273
Wallie117 10:57f090f3ddda 274 cali_max1 = 0;
Wallie117 10:57f090f3ddda 275 cali_max2 = 0;
Wallie117 10:57f090f3ddda 276 cali_fact1 = 0.9;
Wallie117 7:d55569b92f30 277 cali_fact2 = 0.9;
Wallie117 10:57f090f3ddda 278
Wallie117 1:0e1d91965b8e 279 wait(2);
EmilyJCZ 5:0597358d0016 280 led_red.write(0);
Wallie117 1:0e1d91965b8e 281 wait(0.2);
Wallie117 10:57f090f3ddda 282 led_red.write(1);
Wallie117 1:0e1d91965b8e 283 wait(0.2);
EmilyJCZ 5:0597358d0016 284 led_red.write(0);
Wallie117 1:0e1d91965b8e 285 wait(0.2);
EmilyJCZ 5:0597358d0016 286 led_red.write(1);
Wallie117 1:0e1d91965b8e 287 wait(0.2);
EmilyJCZ 5:0597358d0016 288 led_red.write(0);
Wallie117 1:0e1d91965b8e 289 wait(0.2);
EmilyJCZ 5:0597358d0016 290 led_red.write(1);
Wallie117 1:0e1d91965b8e 291 wait(1);
EmilyJCZ 3:f9a1df2271d2 292
Wallie117 1:0e1d91965b8e 293 pc.printf("\t.....Calibrating Signal of EMG 1 and 2 .......\n");
EmilyJCZ 5:0597358d0016 294 led_red.write(0);
EmilyJCZ 5:0597358d0016 295 led_yellow.write(1);
Wallie117 10:57f090f3ddda 296
Wallie117 1:0e1d91965b8e 297 wait(0.5);
Wallie117 1:0e1d91965b8e 298 pc.printf("\t......contract muscles..... \n");
Wallie117 1:0e1d91965b8e 299
Wallie117 10:57f090f3ddda 300 for(int cali_index1 = 0; cali_index1 < 2560; cali_index1++) { // Records 5 seconds of y1.
Wallie117 1:0e1d91965b8e 301 hoog_laag_filter();
Wallie117 1:0e1d91965b8e 302 cali_array1[cali_index1] = y1;
Wallie117 1:0e1d91965b8e 303 wait((float)1/Fs);
Wallie117 1:0e1d91965b8e 304 }
Wallie117 10:57f090f3ddda 305 for(int cali_index2 = 0; cali_index2 < 2560; cali_index2++) { // Records 5 seconds of y2.
Wallie117 1:0e1d91965b8e 306 hoog_laag_filter();
Wallie117 1:0e1d91965b8e 307 cali_array2[cali_index2] = y2;
Wallie117 1:0e1d91965b8e 308 wait((float)1/Fs);
Wallie117 1:0e1d91965b8e 309 }
Wallie117 10:57f090f3ddda 310 for(int cali_index3 = 0; cali_index3 < 2560; cali_index3++) { // Scales y1.
Wallie117 10:57f090f3ddda 311 if(cali_array1[cali_index3] > cali_max1) {
Wallie117 1:0e1d91965b8e 312 cali_max1 = cali_array1[cali_index3];
Wallie117 0:6616d722fed3 313 }
Wallie117 1:0e1d91965b8e 314 }
Wallie117 10:57f090f3ddda 315 for(int cali_index4 = 0; cali_index4 < 2560; cali_index4++) { // Scales y2.
Wallie117 1:0e1d91965b8e 316 if(cali_array2[cali_index4] > cali_max2) {
Wallie117 1:0e1d91965b8e 317 cali_max2 = cali_array2[cali_index4];
Wallie117 0:6616d722fed3 318 }
Wallie117 1:0e1d91965b8e 319 }
EmilyJCZ 9:a5c2ddf2cb8e 320 cali_fact1 = (double)1/cali_max1; // Calibration factor for y1.
EmilyJCZ 9:a5c2ddf2cb8e 321 cali_fact2 = (double)1/cali_max2; // Calibration factor for y2.
Wallie117 1:0e1d91965b8e 322
EmilyJCZ 5:0597358d0016 323 led_yellow.write(0);
Wallie117 1:0e1d91965b8e 324 pc.printf(" \t....... Calibration has been completed!\n");
Wallie117 1:0e1d91965b8e 325 wait(0.2);
EmilyJCZ 5:0597358d0016 326 led_green.write(led_off);
Wallie117 1:0e1d91965b8e 327 wait(0.2);
EmilyJCZ 5:0597358d0016 328 led_green.write(led_on);
Wallie117 1:0e1d91965b8e 329 wait(0.2);
EmilyJCZ 5:0597358d0016 330 led_green.write(led_off);
Wallie117 1:0e1d91965b8e 331 wait(0.2);
EmilyJCZ 5:0597358d0016 332 led_green.write(led_on);
Wallie117 1:0e1d91965b8e 333 wait(4);
Wallie117 1:0e1d91965b8e 334 pc.printf("Beginning with Ball-E board settings\n");
EmilyJCZ 5:0597358d0016 335 led_green.write(led_off);
Wallie117 1:0e1d91965b8e 336 wait(2);
Wallie117 1:0e1d91965b8e 337 y1 = 0;
Wallie117 1:0e1d91965b8e 338 y2 = 0;
Wallie117 1:0e1d91965b8e 339
Wallie117 10:57f090f3ddda 340 j = 1; // Wait for the fase swith to initiate.
Wallie117 1:0e1d91965b8e 341 fase_switch_wait = true;
EmilyJCZ 3:f9a1df2271d2 342 flag_calibration = false;
Wallie117 1:0e1d91965b8e 343 }
Wallie117 1:0e1d91965b8e 344
Wallie117 10:57f090f3ddda 345 if (flag_s) { // Turns calibration off.
Wallie117 1:0e1d91965b8e 346 flag_calibration = false;
Wallie117 1:0e1d91965b8e 347 }
EmilyJCZ 9:a5c2ddf2cb8e 348 //**************************** FASE SWITCH *****************************************************************
EmilyJCZ 9:a5c2ddf2cb8e 349
Wallie117 10:57f090f3ddda 350 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.
Wallie117 1:0e1d91965b8e 351 j++;
Wallie117 10:57f090f3ddda 352 wait(0.01);
Wallie117 1:0e1d91965b8e 353 pc.printf("waarde j = %i \n",j);
EmilyJCZ 5:0597358d0016 354 led_red.write(0);
EmilyJCZ 5:0597358d0016 355 led_green.write(1);
EmilyJCZ 5:0597358d0016 356 led_yellow.write(0);
Wallie117 1:0e1d91965b8e 357 }
Wallie117 1:0e1d91965b8e 358
Wallie117 10:57f090f3ddda 359 if( j > N) { // When the wait is long enough the game starts.
Wallie117 1:0e1d91965b8e 360 fase_switch_wait = false;
Wallie117 10:57f090f3ddda 361 switch(fase) {
Wallie117 10:57f090f3ddda 362 //************************ Fase 1 ******************************************************************
Wallie117 1:0e1d91965b8e 363 case(1):
EmilyJCZ 5:0597358d0016 364 led_red.write(1);
EmilyJCZ 5:0597358d0016 365 led_green.write(0);
EmilyJCZ 5:0597358d0016 366 led_yellow.write(0);
EmilyJCZ 5:0597358d0016 367 rounds = 0;
Wallie117 10:57f090f3ddda 368
Wallie117 10:57f090f3ddda 369 /* If the left arm is above this value flag_left turns true so the motor can run ccw.
Wallie117 10:57f090f3ddda 370 The left and right arm can not be true at the same time so right has to turn off when left is on.
Wallie117 10:57f090f3ddda 371 This is the same for the left and right arm. */
Wallie117 10:57f090f3ddda 372 if (y1> contract) {
Wallie117 1:0e1d91965b8e 373 flag_right = false;
Wallie117 1:0e1d91965b8e 374 flag_left = true;
Wallie117 0:6616d722fed3 375 }
Wallie117 1:0e1d91965b8e 376
Wallie117 10:57f090f3ddda 377 if (y2 > contract) {
Wallie117 1:0e1d91965b8e 378 flag_right = true;
Wallie117 1:0e1d91965b8e 379 flag_left = false;
Wallie117 0:6616d722fed3 380 }
Wallie117 1:0e1d91965b8e 381
Wallie117 10:57f090f3ddda 382 /* To make sure the ball is thrown in the direction of the board there are maximum values defined.
Wallie117 10:57f090f3ddda 383 This is done for the left and right arm.
Wallie117 10:57f090f3ddda 384 If the maximum value is reached, the flag turns off and the motor speed is zero.*/
Wallie117 10:57f090f3ddda 385 if (pos_board < maxleft) {
Wallie117 1:0e1d91965b8e 386 flag_left = false;
Wallie117 1:0e1d91965b8e 387 motor2speed.write(relax);
Wallie117 1:0e1d91965b8e 388 }
Wallie117 1:0e1d91965b8e 389
Wallie117 10:57f090f3ddda 390 if (pos_board > maxright) {
Wallie117 1:0e1d91965b8e 391 flag_right = false;
Wallie117 1:0e1d91965b8e 392 motor2speed.write(relax);
Wallie117 0:6616d722fed3 393 }
Wallie117 1:0e1d91965b8e 394
Wallie117 10:57f090f3ddda 395 /* When flag_left is true, the left biceps can move the motor in ccw direction.
Wallie117 10:57f090f3ddda 396 This is the same for flag_right and the right biceps in the cw direction.
Wallie117 10:57f090f3ddda 397 There is also a speed defined in which the motor runs.
Wallie117 10:57f090f3ddda 398 There still is the minimum value for the mucle contraction before the motor runs.
Wallie117 10:57f090f3ddda 399 This is done so the motor only moves when you really contract your muscle and not with spontaneous contraction. */
Wallie117 10:57f090f3ddda 400 if (flag_left) {
Wallie117 10:57f090f3ddda 401 if (y1> contract) {
Wallie117 1:0e1d91965b8e 402 motor2direction.write(ccw);
Wallie117 1:0e1d91965b8e 403 motor2speed.write(speed_plate_1);
Wallie117 10:57f090f3ddda 404 } else {
Wallie117 1:0e1d91965b8e 405 motor2speed.write(relax);
Wallie117 1:0e1d91965b8e 406 }
Wallie117 1:0e1d91965b8e 407 }
Wallie117 1:0e1d91965b8e 408
Wallie117 10:57f090f3ddda 409 if (flag_right) {
Wallie117 10:57f090f3ddda 410 if (y2 > contract) {
Wallie117 1:0e1d91965b8e 411 motor2direction.write(cw);
Wallie117 1:0e1d91965b8e 412 motor2speed.write(speed_plate_1);
Wallie117 10:57f090f3ddda 413 } else {
Wallie117 1:0e1d91965b8e 414 motor2speed.write(relax);
Wallie117 1:0e1d91965b8e 415 }
Wallie117 0:6616d722fed3 416 }
Wallie117 10:57f090f3ddda 417 pc.printf("Boardposition \t %.2f EMG1 en EMG2 signaal = %.2f \t %.2f\n", pos_board, y1, y2);
Wallie117 10:57f090f3ddda 418 /* When both the muscles are above a value the next fase is turnt on.
Wallie117 10:57f090f3ddda 419 Some values need to be reset and the leds needs to change. */
Wallie117 10:57f090f3ddda 420 if (y1> fasecontract && y2 > fasecontract) {
Wallie117 1:0e1d91965b8e 421 motor2speed.write(relax);
Wallie117 1:0e1d91965b8e 422 fase = 2;
Wallie117 1:0e1d91965b8e 423 fase_switch_wait = true;
EmilyJCZ 5:0597358d0016 424 led_red.write(0);
EmilyJCZ 5:0597358d0016 425 led_green.write(0);
EmilyJCZ 5:0597358d0016 426 led_yellow.write(1);
Wallie117 1:0e1d91965b8e 427 j = 0;
Wallie117 1:0e1d91965b8e 428 }
Wallie117 1:0e1d91965b8e 429 break;
Wallie117 10:57f090f3ddda 430 //************************ Fase 2 ******************************************************************
Wallie117 1:0e1d91965b8e 431 case(2):
EmilyJCZ 5:0597358d0016 432 led_red.write(0);
EmilyJCZ 5:0597358d0016 433 led_green.write(0);
EmilyJCZ 5:0597358d0016 434 led_yellow.write(1);
Wallie117 1:0e1d91965b8e 435 motor1direction.write(cw);
EmilyJCZ 5:0597358d0016 436 pos_arm1 = (pos_arm - (rounds * 360));
Wallie117 10:57f090f3ddda 437 pos_arm2 = (pos_arm - (rounds * 360));
Wallie117 10:57f090f3ddda 438
Wallie117 10:57f090f3ddda 439 switch(switch_rounds) {
Wallie117 0:6616d722fed3 440 case(1):
EmilyJCZ 5:0597358d0016 441 rounds ++;
EmilyJCZ 5:0597358d0016 442 switch_rounds = 2;
Wallie117 1:0e1d91965b8e 443 break;
Wallie117 0:6616d722fed3 444 case(2):
Wallie117 10:57f090f3ddda 445 if(pos_arm1>360) {
EmilyJCZ 5:0597358d0016 446 switch_rounds = 1;
Wallie117 1:0e1d91965b8e 447 }
Wallie117 1:0e1d91965b8e 448 break;
Wallie117 0:6616d722fed3 449 }
Wallie117 1:0e1d91965b8e 450
Wallie117 10:57f090f3ddda 451 if (y2 > minimum_contract & y2 < medium_contract) {
Wallie117 10:57f090f3ddda 452 speed_control_arm = (1.0*(y2));
Wallie117 4:f9f75c913d7d 453 motor1speed.write(speed_control_arm);
Wallie117 1:0e1d91965b8e 454 }
Wallie117 10:57f090f3ddda 455 if (y2 > medium_contract) { // Hoger dan drempelwaarde = Actief
Wallie117 10:57f090f3ddda 456 speed_control_arm = (1.1*(y2));
EmilyJCZ 5:0597358d0016 457 motor1speed.write(speed_control_arm);
Wallie117 1:0e1d91965b8e 458 }
Wallie117 10:57f090f3ddda 459 if (y2 < minimum_contract) { // Lager dan drempel = Rust
Wallie117 1:0e1d91965b8e 460 motor1speed.write(relax);
Wallie117 0:6616d722fed3 461 }
Wallie117 1:0e1d91965b8e 462
Wallie117 10:57f090f3ddda 463 if(rounds == pos_armrounds_max) { // max aantal draaing gemaakt!!!!!!
Wallie117 1:0e1d91965b8e 464 problem1 = true;
Wallie117 1:0e1d91965b8e 465 problem2 = true;
Wallie117 1:0e1d91965b8e 466 motor1speed.write(relax);
EmilyJCZ 3:f9a1df2271d2 467
Wallie117 10:57f090f3ddda 468 while (problem1) {
Wallie117 1:0e1d91965b8e 469 j++;
Wallie117 1:0e1d91965b8e 470 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 471
Wallie117 10:57f090f3ddda 472 if( j > N) {
EmilyJCZ 3:f9a1df2271d2 473 problem1 = false;
Wallie117 1:0e1d91965b8e 474 }
EmilyJCZ 3:f9a1df2271d2 475 }
EmilyJCZ 3:f9a1df2271d2 476
Wallie117 1:0e1d91965b8e 477 wait(0.1);
EmilyJCZ 5:0597358d0016 478 led_red.write(0);
Wallie117 1:0e1d91965b8e 479 wait(0.1);
EmilyJCZ 5:0597358d0016 480 led_red.write(1);
Wallie117 1:0e1d91965b8e 481 wait(0.1);
EmilyJCZ 5:0597358d0016 482 led_red.write(0);
Wallie117 1:0e1d91965b8e 483 wait(0.1);
EmilyJCZ 5:0597358d0016 484 led_red.write(1);
Wallie117 1:0e1d91965b8e 485 wait(0.1);
EmilyJCZ 5:0597358d0016 486 led_red.write(0);
Wallie117 1:0e1d91965b8e 487 wait(0.1);
EmilyJCZ 5:0597358d0016 488 led_red.write(1);
Wallie117 2:de3bb38dae4e 489 wait(1.5);
Wallie117 1:0e1d91965b8e 490
Wallie117 10:57f090f3ddda 491 while(problem2) {
Wallie117 2:de3bb38dae4e 492 motor1direction.write(ccw);
Wallie117 10:57f090f3ddda 493 if(pos_arm > 260) {
Wallie117 10:57f090f3ddda 494 motor1speed.write(0.2);
Wallie117 10:57f090f3ddda 495 }
Wallie117 10:57f090f3ddda 496 if(pos_arm > 100 && pos_arm <260) {
Wallie117 10:57f090f3ddda 497 motor1speed.write(0.07);
EmilyJCZ 6:1518fccc5616 498 }
Wallie117 10:57f090f3ddda 499 if(pos_arm > 15 && pos_arm <100) {
Wallie117 10:57f090f3ddda 500 motor1speed.write(0.05);
Wallie117 10:57f090f3ddda 501 }
Wallie117 10:57f090f3ddda 502 if(pos_arm < 10) {
Wallie117 10:57f090f3ddda 503 motor1speed.write(0);
Wallie117 10:57f090f3ddda 504 }
Wallie117 10:57f090f3ddda 505 pc.printf("Positie van de arm = %.4f \n", pos_arm);
EmilyJCZ 3:f9a1df2271d2 506
Wallie117 10:57f090f3ddda 507 if (pos_arm < 10) {
Wallie117 4:f9f75c913d7d 508 flag_v_arm = false;
Wallie117 1:0e1d91965b8e 509 problem2 = false;
Wallie117 2:de3bb38dae4e 510 motor1speed.write(0);
EmilyJCZ 5:0597358d0016 511 rounds = 0;
Wallie117 1:0e1d91965b8e 512 wait(1);
Wallie117 1:0e1d91965b8e 513 }
Wallie117 1:0e1d91965b8e 514 }
Wallie117 0:6616d722fed3 515 }
Wallie117 10:57f090f3ddda 516 if (pos_arm1 > minimum_throw_angle && pos_arm1 < maximum_throw_angle) {
Wallie117 10:57f090f3ddda 517 if (y1> maximum_leftbiceps) {
Wallie117 0:6616d722fed3 518 magnet.write(off);
Wallie117 0:6616d722fed3 519 motor1speed.write(relax);
Wallie117 10:57f090f3ddda 520 flag_arm_magnet = true;
Wallie117 10:57f090f3ddda 521 led_red.write(0);
Wallie117 10:57f090f3ddda 522 led_green.write(0);
Wallie117 10:57f090f3ddda 523 led_yellow.write(0);
Wallie117 10:57f090f3ddda 524 wait(0.2);
Wallie117 10:57f090f3ddda 525 led_yellow.write(1);
Wallie117 10:57f090f3ddda 526 wait(0.2);
Wallie117 10:57f090f3ddda 527 led_yellow.write(0);
Wallie117 10:57f090f3ddda 528 wait(0.2);
Wallie117 10:57f090f3ddda 529 led_yellow.write(1);
Wallie117 10:57f090f3ddda 530 wait(0.2);
Wallie117 10:57f090f3ddda 531 led_yellow.write(0);
Wallie117 10:57f090f3ddda 532 wait(0.2);
Wallie117 10:57f090f3ddda 533 led_red.write(0);
Wallie117 10:57f090f3ddda 534 led_green.write(1);
Wallie117 10:57f090f3ddda 535 led_yellow.write(1);
Wallie117 10:57f090f3ddda 536 wait(0.2);
Wallie117 0:6616d722fed3 537 pc.printf("Van fase 2 naar fase 3\n");
Wallie117 10:57f090f3ddda 538 flag_arm_magnet = true;
Wallie117 10:57f090f3ddda 539 wait(1.6);
Wallie117 0:6616d722fed3 540 }
Wallie117 1:0e1d91965b8e 541 }
Wallie117 10:57f090f3ddda 542 if(flag_arm_magnet == true) {
Wallie117 10:57f090f3ddda 543
Wallie117 10:57f090f3ddda 544 pc.printf(" \n\n arm magneet laten oppakken! \n");
Wallie117 10:57f090f3ddda 545 while(flag_arm_magnet) {
Wallie117 10:57f090f3ddda 546
Wallie117 10:57f090f3ddda 547 pos_arm2 = (pos_arm - (rounds * 360));
Wallie117 10:57f090f3ddda 548
Wallie117 10:57f090f3ddda 549 pc.printf("pos_arm = %f \n",pos_arm2);
Wallie117 10:57f090f3ddda 550 magnet = 1;
Wallie117 10:57f090f3ddda 551
Wallie117 10:57f090f3ddda 552 pos_arm2 = (pos_arm - (rounds * 360));
Wallie117 10:57f090f3ddda 553 motor1direction.write(cw);
Wallie117 10:57f090f3ddda 554 if(pos_arm2 > 0 && pos_arm2 < 200) {
Wallie117 10:57f090f3ddda 555 motor1speed.write(0.3);
Wallie117 10:57f090f3ddda 556 }
Wallie117 10:57f090f3ddda 557 if(pos_arm2 > 200 && pos_arm2 < 260) {
Wallie117 10:57f090f3ddda 558 motor1speed.write(0.15);
Wallie117 10:57f090f3ddda 559 }
Wallie117 10:57f090f3ddda 560 if(pos_arm2 > 260 && pos_arm2 < 350) {
Wallie117 10:57f090f3ddda 561 motor1speed.write(0.04);
Wallie117 10:57f090f3ddda 562 }
Wallie117 10:57f090f3ddda 563 if(pos_arm2 > 350) {
Wallie117 10:57f090f3ddda 564 motor1speed.write(0);
Wallie117 10:57f090f3ddda 565 flag_arm_magnet = false;
Wallie117 10:57f090f3ddda 566 fase = 3;
Wallie117 10:57f090f3ddda 567 j = 0;
Wallie117 10:57f090f3ddda 568 fase_switch_wait = true;
Wallie117 10:57f090f3ddda 569 wait(1.5);
Wallie117 10:57f090f3ddda 570 }
Wallie117 10:57f090f3ddda 571 }
Wallie117 10:57f090f3ddda 572 }
Wallie117 10:57f090f3ddda 573 pc.printf("positie arm = \t %f \t EMG1 en EMG2 = %f \t %f \n", pos_arm2, y1, y2);
Wallie117 0:6616d722fed3 574 break;
Wallie117 10:57f090f3ddda 575 //======================================= Reset arm om bal te pakken ===================================================================
Wallie117 10:57f090f3ddda 576
Wallie117 10:57f090f3ddda 577
Wallie117 10:57f090f3ddda 578
Wallie117 10:57f090f3ddda 579 //************************ Fase 3 ******************************************************************
Wallie117 1:0e1d91965b8e 580 case(3):
EmilyJCZ 5:0597358d0016 581 led_red.write(0);
EmilyJCZ 5:0597358d0016 582 led_green.write(1);
EmilyJCZ 5:0597358d0016 583 led_yellow.write(0);
Wallie117 10:57f090f3ddda 584 switch(switch_reset) {
Wallie117 1:0e1d91965b8e 585 case(1):
Wallie117 10:57f090f3ddda 586
Wallie117 10:57f090f3ddda 587 speedcompensation2 = (((pos_arm - reset_arm)/2300)+0.03);
Wallie117 10:57f090f3ddda 588 speedcompensation = (0.05);
Wallie117 10:57f090f3ddda 589
Wallie117 10:57f090f3ddda 590 if(pos_arm<100) {
Wallie117 10:57f090f3ddda 591 speedcompensation2 = 0.05;
Wallie117 10:57f090f3ddda 592 }
Wallie117 10:57f090f3ddda 593
Wallie117 10:57f090f3ddda 594 if(pos_arm < reset_position) { //ene kant op draaien
Wallie117 1:0e1d91965b8e 595 motor1direction.write(cw);
Wallie117 10:57f090f3ddda 596 motor1speed.write(speedcompensation2);
Wallie117 10:57f090f3ddda 597 }
Wallie117 10:57f090f3ddda 598 if(pos_arm > reset_position) { //andere kant op
Wallie117 10:57f090f3ddda 599 motor1direction.write(ccw);
Wallie117 1:0e1d91965b8e 600 motor1speed.write(speedcompensation2);
Wallie117 1:0e1d91965b8e 601 }
Wallie117 10:57f090f3ddda 602
Wallie117 10:57f090f3ddda 603 if(pos_board < reset_board) {
Wallie117 10:57f090f3ddda 604 motor2direction.write(cw);
Wallie117 10:57f090f3ddda 605 motor2speed.write(speedcompensation);
Wallie117 10:57f090f3ddda 606 }
Wallie117 10:57f090f3ddda 607
Wallie117 10:57f090f3ddda 608 if(pos_board > reset_board) {
Wallie117 10:57f090f3ddda 609 motor2direction.write(ccw);
Wallie117 10:57f090f3ddda 610 motor2speed.write(speedcompensation);
EmilyJCZ 3:f9a1df2271d2 611 }
Wallie117 10:57f090f3ddda 612
Wallie117 10:57f090f3ddda 613 if(pos_arm < reset_position+4 && pos_arm > reset_position-4) { // Dit gaat niet goed komen, omdat het precies die waarde moet zijn
EmilyJCZ 3:f9a1df2271d2 614 motor1speed.write(0);
Wallie117 10:57f090f3ddda 615 }
Wallie117 10:57f090f3ddda 616
Wallie117 10:57f090f3ddda 617 if(pos_board < (reset_board+0.2) && pos_board > (reset_board-0.2)) {
Wallie117 10:57f090f3ddda 618 motor2speed.write(0);
EmilyJCZ 3:f9a1df2271d2 619 }
Wallie117 10:57f090f3ddda 620
Wallie117 10:57f090f3ddda 621 if( pos_arm < reset_position+4 && pos_arm > reset_position-4 && pos_board < (reset_board+0.2) && pos_board > (reset_board-0.2)) {
Wallie117 10:57f090f3ddda 622 switch_reset = 2;
Wallie117 10:57f090f3ddda 623 wait(0.5);
Wallie117 10:57f090f3ddda 624 }
Wallie117 10:57f090f3ddda 625
Wallie117 10:57f090f3ddda 626 pc.printf("Positie_arm = %.2f \t \t snelheid = %.2f \n",pos_arm, speedcompensation2);
Wallie117 1:0e1d91965b8e 627 wait(0.0001);
Wallie117 1:0e1d91965b8e 628 break;
Wallie117 1:0e1d91965b8e 629
Wallie117 1:0e1d91965b8e 630 case(2):
Wallie117 1:0e1d91965b8e 631 pc.printf("\t switch magneet automatisch \n");
Wallie117 2:de3bb38dae4e 632 wait(0.2);
Wallie117 1:0e1d91965b8e 633 magnet.write(on);
Wallie117 2:de3bb38dae4e 634 wait(2);
Wallie117 1:0e1d91965b8e 635 switch_reset = 3;
Wallie117 1:0e1d91965b8e 636 break;
EmilyJCZ 3:f9a1df2271d2 637
Wallie117 1:0e1d91965b8e 638 case(3):
Wallie117 10:57f090f3ddda 639 speedcompensation2 = 0.05;
Wallie117 0:6616d722fed3 640
Wallie117 10:57f090f3ddda 641 if(pos_arm < reset_position) { //ene kant op draaien
Wallie117 10:57f090f3ddda 642 motor1direction.write(cw);
Wallie117 10:57f090f3ddda 643 motor1speed.write(speedcompensation2);
Wallie117 0:6616d722fed3 644 }
Wallie117 10:57f090f3ddda 645 if(pos_arm > reset_position) { //andere kant op
Wallie117 10:57f090f3ddda 646 motor1direction.write(ccw);
Wallie117 10:57f090f3ddda 647 motor1speed.write(speedcompensation2);
Wallie117 10:57f090f3ddda 648 }
Wallie117 10:57f090f3ddda 649 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
Wallie117 10:57f090f3ddda 650 motor1speed.write(0);
EmilyJCZ 5:0597358d0016 651 board = true;
Wallie117 10:57f090f3ddda 652 switch_reset = 1;
Wallie117 0:6616d722fed3 653 }
Wallie117 10:57f090f3ddda 654 if(board) {
Wallie117 1:0e1d91965b8e 655 pc.printf("fase switch naar 1\n\n");
EmilyJCZ 5:0597358d0016 656 board = false;
Wallie117 1:0e1d91965b8e 657 wait(2);
Wallie117 1:0e1d91965b8e 658 games_played ++;
Wallie117 10:57f090f3ddda 659 games_played1 = games_played - (3*calibration_measurements - 3);
Wallie117 1:0e1d91965b8e 660 pc.printf("Games played total count = %i and loop %i\n",games_played,games_played1);
Wallie117 1:0e1d91965b8e 661
Wallie117 10:57f090f3ddda 662 if(games_played1 == 3) {
Wallie117 10:57f090f3ddda 663 pc.printf("Choose option calibration = left \t or go with game = right \n");
Wallie117 10:57f090f3ddda 664 wait(2);
Wallie117 10:57f090f3ddda 665 reset_case3 = true;
Wallie117 10:57f090f3ddda 666 while(reset_case3) {
Wallie117 10:57f090f3ddda 667 wait(0.05);
Wallie117 10:57f090f3ddda 668 // flag_calibration_LED = true;
Wallie117 10:57f090f3ddda 669 pc.printf("y1 = %.3f \t y2 = %.3f \n", y1, y2);
Wallie117 10:57f090f3ddda 670 if(y1 > 0.6) {
Wallie117 10:57f090f3ddda 671 flag_calibration_LED = false;
Wallie117 10:57f090f3ddda 672 pc.printf("Calibration is active");
Wallie117 10:57f090f3ddda 673 while(t) {
Wallie117 10:57f090f3ddda 674 pc.printf("\tCalibratie begint over %i \n",t);
Wallie117 10:57f090f3ddda 675 t--;
Wallie117 10:57f090f3ddda 676 led_yellow.write(1);
Wallie117 10:57f090f3ddda 677 wait(0.5);
Wallie117 10:57f090f3ddda 678 led_yellow.write(0);
Wallie117 10:57f090f3ddda 679 wait(0.5);
Wallie117 10:57f090f3ddda 680 if(t == 0) {
Wallie117 10:57f090f3ddda 681 flag_calibration = true;
Wallie117 10:57f090f3ddda 682 reset_case3 = false;
Wallie117 10:57f090f3ddda 683 }
Wallie117 10:57f090f3ddda 684 }
Wallie117 10:57f090f3ddda 685 }
Wallie117 10:57f090f3ddda 686 if(y2 > 0.6) {
Wallie117 10:57f090f3ddda 687 flag_calibration_LED = false;
Wallie117 10:57f090f3ddda 688 reset_case3 = false;
Wallie117 10:57f090f3ddda 689 pc.printf("The game is on!!");
Wallie117 10:57f090f3ddda 690 while(t) {
Wallie117 10:57f090f3ddda 691 pc.printf("\tNieuw spel begint over %i \n",t);
Wallie117 10:57f090f3ddda 692 t--;
Wallie117 10:57f090f3ddda 693 led_yellow.write(1);
Wallie117 10:57f090f3ddda 694 wait(0.5);
Wallie117 10:57f090f3ddda 695 led_yellow.write(0);
Wallie117 10:57f090f3ddda 696 wait(0.5);
Wallie117 10:57f090f3ddda 697 }
Wallie117 10:57f090f3ddda 698 }
Wallie117 1:0e1d91965b8e 699 }
Wallie117 1:0e1d91965b8e 700 }
Wallie117 10:57f090f3ddda 701 while(t) {
Wallie117 1:0e1d91965b8e 702 pc.printf("\tNieuw spel begint over %i \n",t);
Wallie117 1:0e1d91965b8e 703 t--;
EmilyJCZ 5:0597358d0016 704 led_yellow.write(1);
Wallie117 1:0e1d91965b8e 705 wait(0.5);
EmilyJCZ 5:0597358d0016 706 led_yellow.write(0);
Wallie117 1:0e1d91965b8e 707 wait(0.5);
Wallie117 1:0e1d91965b8e 708 }
Wallie117 1:0e1d91965b8e 709
Wallie117 0:6616d722fed3 710 fase = 1; // Terug naar fase 1
Wallie117 0:6616d722fed3 711 switch_reset = 1; // De switch op orginele locatie zetten
Wallie117 0:6616d722fed3 712 t = 5;
Wallie117 10:57f090f3ddda 713 rounds = 0;
Wallie117 1:0e1d91965b8e 714 }
Wallie117 1:0e1d91965b8e 715
Wallie117 10:57f090f3ddda 716 pc.printf("Positie_board = %.2f \t \t snelheid = %.2f \n",pos_board, v_arm);
Wallie117 1:0e1d91965b8e 717 wait(0.0001);
Wallie117 1:0e1d91965b8e 718 break;
Wallie117 0:6616d722fed3 719 }
Wallie117 1:0e1d91965b8e 720 break;
EmilyJCZ 9:a5c2ddf2cb8e 721 //================================================================================================== END SCRIPT ===============================================================================================================
Wallie117 0:6616d722fed3 722 }
Wallie117 1:0e1d91965b8e 723 }
Wallie117 0:6616d722fed3 724 }
Wallie117 0:6616d722fed3 725 }