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@3:f9a1df2271d2, 2015-10-23 (annotated)

Committer:
EmilyJCZ
Date:
Fri Oct 23 08:24:34 2015 +0000
Revision:
3:f9a1df2271d2
Parent:
2:de3bb38dae4e
Child:
4:f9f75c913d7d
Met 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 3:f9a1df2271d2 5 In order to achieve this movement we use a ‘shoulder’ that can turn in the vertical plane and move in the horizontal plane.
Wallie117 0:6616d722fed3 6 */
Wallie117 0:6616d722fed3 7
Wallie117 0:6616d722fed3 8 //******************************************** Library DECLARATION **************************************
EmilyJCZ 3:f9a1df2271d2 9 // Libraries are files which contain standard formulas for reading surtain information. Every library contains its own information.
EmilyJCZ 3:f9a1df2271d2 10 #include "mbed.h" // Standard library. This includes the reading of AnalogIn, DigitalOut, PwmOut and other standard formules.
EmilyJCZ 3:f9a1df2271d2 11 #include "QEI.h" // This library includes the reading of of the Encoders from motors.
EmilyJCZ 3:f9a1df2271d2 12 #include "MODSERIAL.h" // MODSERIAL inherits Serial and adds extensions for buffering.
EmilyJCZ 3:f9a1df2271d2 13 #include "HIDScope.h" //
EmilyJCZ 3:f9a1df2271d2 14 #include "biquadFilter.h" //
EmilyJCZ 3:f9a1df2271d2 15 #include <cmath> //
EmilyJCZ 3:f9a1df2271d2 16 #include <stdio.h> //
Wallie117 0:6616d722fed3 17
Wallie117 0:6616d722fed3 18 //******************************************* FUNCTION DECLA *******************************************
Wallie117 0:6616d722fed3 19 //**************** INPUTS *****************
EmilyJCZ 3:f9a1df2271d2 20 AnalogIn EMG1(A0); // Input EMG
EmilyJCZ 3:f9a1df2271d2 21 AnalogIn EMG2(A1); // Input EMG
Wallie117 0:6616d722fed3 22
EmilyJCZ 3:f9a1df2271d2 23 QEI wheel1 (D10, D11, NC, 64,QEI::X4_ENCODING); // Function for Encoder1 on the motor1 to the Microcontroller
EmilyJCZ 3:f9a1df2271d2 24 QEI wheel2 (D12, D13, NC, 64,QEI::X4_ENCODING); // Function for Encoder2 on the motor2 to the Microcontroller
Wallie117 0:6616d722fed3 25
Wallie117 0:6616d722fed3 26 //**************** OUTPUTS ****************
EmilyJCZ 3:f9a1df2271d2 27 DigitalOut red(LED_RED); // LED declared for calibration
EmilyJCZ 3:f9a1df2271d2 28 DigitalOut green(LED_GREEN); // LED declared for sampling
Wallie117 1:0e1d91965b8e 29
EmilyJCZ 3:f9a1df2271d2 30 DigitalOut led_rood(D1);
EmilyJCZ 3:f9a1df2271d2 31 DigitalOut led_geel(D3);
EmilyJCZ 3:f9a1df2271d2 32 DigitalOut led_groen(D9);
Wallie117 0:6616d722fed3 33
EmilyJCZ 3:f9a1df2271d2 34 DigitalOut magnet(D2);
Wallie117 1:0e1d91965b8e 35
EmilyJCZ 3:f9a1df2271d2 36 DigitalOut motor1direction(D4); // D4 en D5 zijn motor 2 (op het motorshield)
EmilyJCZ 3:f9a1df2271d2 37 PwmOut motor1speed(D5);
EmilyJCZ 3:f9a1df2271d2 38 DigitalOut motor2direction(D7); // D6 en D7 zijn motor 1 (op het motorshield)
EmilyJCZ 3:f9a1df2271d2 39 PwmOut motor2speed(D6);
Wallie117 0:6616d722fed3 40
Wallie117 0:6616d722fed3 41 //**************** FUNCTIONS **************
Wallie117 0:6616d722fed3 42 /* HIDScope scope(4); */ // HIDScope declared with 4 scopes
EmilyJCZ 3:f9a1df2271d2 43 MODSERIAL pc(USBTX, USBRX); // Function for Serial communication with the Microcontroller to the pc.
Wallie117 0:6616d722fed3 44
Wallie117 0:6616d722fed3 45 //******************************* GLOBAL VARIABLES DECLARATION ************************************
EmilyJCZ 3:f9a1df2271d2 46 const int led_on = 0; // Needed for the LEDs to turn on and off
EmilyJCZ 3:f9a1df2271d2 47 const int led_off = 1;
EmilyJCZ 3:f9a1df2271d2 48 const int relax = 0;
EmilyJCZ 3:f9a1df2271d2 49 int games_played = -1; // -1 omdat het spel daar eerst loopt voor het spelen om alles na te checken
EmilyJCZ 3:f9a1df2271d2 50 int games_played1 = 0;
EmilyJCZ 3:f9a1df2271d2 51 bool flag_calibration = true;
Wallie117 0:6616d722fed3 52
Wallie117 1:0e1d91965b8e 53 //********* VARIABLES FOR MOTOR CONTROL ********
EmilyJCZ 3:f9a1df2271d2 54 const float cw = 1; // The number if: motor1 moves clock wise motor2 moves counterclockwise
EmilyJCZ 3:f9a1df2271d2 55 const float ccw = 0; // The number if: motor1 moves counterclock wise motor2 moves clockwise
EmilyJCZ 3:f9a1df2271d2 56 bool flag_s = false; // Var flag_s sensor ticker
EmilyJCZ 3:f9a1df2271d2 57 bool flag_m = false; // Var flag_m motor ticker
EmilyJCZ 3:f9a1df2271d2 58 float ain = 0;
Wallie117 0:6616d722fed3 59
Wallie117 0:6616d722fed3 60 //********* VARIABLES FOR CONTROL 1 ************
Wallie117 0:6616d722fed3 61 volatile bool sample_go; // Ticker EMG function
EmilyJCZ 3:f9a1df2271d2 62 int Fs = 512; // Sampling frequency
EmilyJCZ 3:f9a1df2271d2 63 int calibratie_metingen = 0;
EmilyJCZ 3:f9a1df2271d2 64 const double low_b1 = 1.480219865318266e-04; //Filter coefficients
EmilyJCZ 3:f9a1df2271d2 65 const double low_b2 = 2.960439730636533e-04;
EmilyJCZ 3:f9a1df2271d2 66 const double low_b3 = 1.480219865318266e-04;
EmilyJCZ 3:f9a1df2271d2 67 const double low_a2 = -1.965293372622690e+00; // a1 is normalized to 1
EmilyJCZ 3:f9a1df2271d2 68 const double low_a3 = 9.658854605688177e-01;
EmilyJCZ 3:f9a1df2271d2 69 const double high_b1 = 8.047897937631126e-01;
EmilyJCZ 3:f9a1df2271d2 70 const double high_b2 = -1.609579587526225e+00;
EmilyJCZ 3:f9a1df2271d2 71 const double high_b3 = 8.047897937631126e-01;
EmilyJCZ 3:f9a1df2271d2 72 const double high_a2 = -1.571102440190402e+00; // a1 is normalized to 1
EmilyJCZ 3:f9a1df2271d2 73 const double high_a3 = 6.480567348620491e-01;
EmilyJCZ 3:f9a1df2271d2 74 double u1;
EmilyJCZ 3:f9a1df2271d2 75 double y1; // Declaring the input variables
EmilyJCZ 3:f9a1df2271d2 76 double u2;
EmilyJCZ 3:f9a1df2271d2 77 double y2;
EmilyJCZ 3:f9a1df2271d2 78 double cali_fact1 = 0.9; // calibration factor to normalize filter output to a scale of 0 - 1
EmilyJCZ 3:f9a1df2271d2 79 double cali_fact2 = 0.9;
EmilyJCZ 3:f9a1df2271d2 80 double cali_array1[2560] = {}; // array to store values in
EmilyJCZ 3:f9a1df2271d2 81 double cali_array2[2560] = {};
EmilyJCZ 3:f9a1df2271d2 82 double cali_array3[512] = {};
Wallie117 0:6616d722fed3 83
Wallie117 0:6616d722fed3 84 //********* VARIABLES FOR FASE SWITCH **********
EmilyJCZ 3:f9a1df2271d2 85 bool begin = true;
EmilyJCZ 3:f9a1df2271d2 86 int fase = 3; // To switch between different phases and begins with the reset phase
EmilyJCZ 3:f9a1df2271d2 87 const float fasecontract = 0.7; // The value the EMG signal must be for fase change
EmilyJCZ 3:f9a1df2271d2 88 int reset = 0;
EmilyJCZ 3:f9a1df2271d2 89 int button_pressed = 0;
EmilyJCZ 3:f9a1df2271d2 90 int j = 1;
EmilyJCZ 3:f9a1df2271d2 91 int N = 200;
EmilyJCZ 3:f9a1df2271d2 92 bool fase_switch_wait = true;
Wallie117 0:6616d722fed3 93
Wallie117 0:6616d722fed3 94 //********* VARIABLES FOR CONTROL 2 ************
EmilyJCZ 3:f9a1df2271d2 95 const float contract = 0.5; // The minimum value for which the motor moves
EmilyJCZ 3:f9a1df2271d2 96 const float maxleft = -200; // With this angle the motor should stop moving
EmilyJCZ 3:f9a1df2271d2 97 const float maxright = 200; // With this angle the motor should stop moving
EmilyJCZ 3:f9a1df2271d2 98 const float speed_plate_1 = 0.1; // The speed of the plate in control 2
EmilyJCZ 3:f9a1df2271d2 99 bool flag_left = true;
EmilyJCZ 3:f9a1df2271d2 100 bool flag_right = true;
EmilyJCZ 3:f9a1df2271d2 101 float pos_board = 0; // The position of the board begins at zero
EmilyJCZ 3:f9a1df2271d2 102 int pos_board1 = 0;
EmilyJCZ 3:f9a1df2271d2 103 float Encoderread2 = 0;
Wallie117 0:6616d722fed3 104
Wallie117 0:6616d722fed3 105 //********* VARIABLES FOR CONTROL 3 ************
EmilyJCZ 3:f9a1df2271d2 106 const float minimum_contract = 0.4; // Certain levels for muscle activity to activate motor
EmilyJCZ 3:f9a1df2271d2 107 const float medium_contract = 0.6; // "Medium contract muscle"
EmilyJCZ 3:f9a1df2271d2 108 const float maximum_leftbiceps = 0.8; // "Maximum contract muscle"
EmilyJCZ 3:f9a1df2271d2 109 const float on = 1.0;
EmilyJCZ 3:f9a1df2271d2 110 const float off = 0.0;
EmilyJCZ 3:f9a1df2271d2 111 const float minimum_throw_angle = 20;
EmilyJCZ 3:f9a1df2271d2 112 const float maximum_throw_angle = 110;
EmilyJCZ 3:f9a1df2271d2 113 float pos_arm = 0;
EmilyJCZ 3:f9a1df2271d2 114 int pos_arm1 = 0;
EmilyJCZ 3:f9a1df2271d2 115 float Encoderread1 = 0;
EmilyJCZ 3:f9a1df2271d2 116 int switch_rondjes = 2;
EmilyJCZ 3:f9a1df2271d2 117 int rondjes = 0;
EmilyJCZ 3:f9a1df2271d2 118 float pos_armrondjes_max = 3;
EmilyJCZ 3:f9a1df2271d2 119 bool problem1 = false;
EmilyJCZ 3:f9a1df2271d2 120 bool problem2 = false;
EmilyJCZ 3:f9a1df2271d2 121 float problem_velocity = 0;
Wallie117 0:6616d722fed3 122
Wallie117 1:0e1d91965b8e 123 //********* VARIABLES FOR CONTROL 4 ************
Wallie117 0:6616d722fed3 124
EmilyJCZ 3:f9a1df2271d2 125 float reset_positie = 0;
EmilyJCZ 3:f9a1df2271d2 126 int reset_arm = 0;
EmilyJCZ 3:f9a1df2271d2 127 int reset_board = 0;
Wallie117 0:6616d722fed3 128 float speedcompensation;
Wallie117 0:6616d722fed3 129 float speedcompensation2;
EmilyJCZ 3:f9a1df2271d2 130 int t = 5; // aftellen naar nieuw spel
EmilyJCZ 3:f9a1df2271d2 131 int switch_reset = 1;
EmilyJCZ 3:f9a1df2271d2 132 bool arm = false;
EmilyJCZ 3:f9a1df2271d2 133 bool board1 = false;
Wallie117 0:6616d722fed3 134
Wallie117 0:6616d722fed3 135 // **************************************** Tickers *****************************************
EmilyJCZ 3:f9a1df2271d2 136 Ticker Sensor_control; // Adds ticker function for the variable function : Sensors
EmilyJCZ 3:f9a1df2271d2 137 Ticker Motor_control;
EmilyJCZ 3:f9a1df2271d2 138 Ticker EMG_Control;
Wallie117 0:6616d722fed3 139
Wallie117 0:6616d722fed3 140 //=============================================================================================== SUB LOOPS ==================================================================================================================
Wallie117 0:6616d722fed3 141 //**************************** CONTROL 1-EMG CALIBRATION ***********************************
Wallie117 0:6616d722fed3 142 biquadFilter highpass1(high_a2, high_a3, high_b1, high_b2, high_b3); // Different objects for different inputs, otherwise the v1 and v2 variables get fucked up
Wallie117 0:6616d722fed3 143 biquadFilter highpass2(high_a2, high_a3, high_b1, high_b2, high_b3);
Wallie117 0:6616d722fed3 144 biquadFilter lowpass1(low_a2, low_a3, low_b1, low_b2, low_b3);
Wallie117 0:6616d722fed3 145 biquadFilter lowpass2(low_a2, low_a3, low_b1, low_b2, low_b3);
Wallie117 0:6616d722fed3 146
Wallie117 1:0e1d91965b8e 147 void hoog_laag_filter()
Wallie117 0:6616d722fed3 148 {
Wallie117 1:0e1d91965b8e 149 u1 = EMG1;
Wallie117 0:6616d722fed3 150 u2 = EMG2;
Wallie117 1:0e1d91965b8e 151 y1 = highpass1.step(u1);
Wallie117 0:6616d722fed3 152 y2 = highpass2.step(u2); // filter order is: high-pass --> rectify --> low-pass
Wallie117 1:0e1d91965b8e 153 y1 = fabs(y1);
Wallie117 1:0e1d91965b8e 154 y2 = fabs(y2);
Wallie117 0:6616d722fed3 155 y1 = lowpass1.step(y1)*cali_fact1;
Wallie117 0:6616d722fed3 156 y2 = lowpass2.step(y2)*cali_fact2; // roughly normalize to a scale of 0 - 1, where 0 is minimum and 1 is roughly the maximum output.
Wallie117 0:6616d722fed3 157 }
Wallie117 1:0e1d91965b8e 158
Wallie117 0:6616d722fed3 159 //======================================= TICKER LOOPS ==========================================
Wallie117 0:6616d722fed3 160 void SENSOR_LOOP()
Wallie117 0:6616d722fed3 161 {
EmilyJCZ 3:f9a1df2271d2 162 Encoderread1 = wheel1.getPulses();
EmilyJCZ 3:f9a1df2271d2 163 pos_arm = (Encoderread1/((64.0*131.0)/360.0)); // Omrekenen naar graden van arm
EmilyJCZ 3:f9a1df2271d2 164 pos_arm1 = pos_arm;
EmilyJCZ 3:f9a1df2271d2 165
EmilyJCZ 3:f9a1df2271d2 166 Encoderread2 = wheel2.getPulses();
EmilyJCZ 3:f9a1df2271d2 167 pos_board = (Encoderread2/((64.0*131.0)/360.0)); // Omrekenen naar graden van board
EmilyJCZ 3:f9a1df2271d2 168 pos_board1 = pos_board;
EmilyJCZ 3:f9a1df2271d2 169
Wallie117 0:6616d722fed3 170 flag_s = true;
Wallie117 0:6616d722fed3 171 }
Wallie117 0:6616d722fed3 172
Wallie117 0:6616d722fed3 173 void MOTOR_LOOP()
Wallie117 0:6616d722fed3 174 {
Wallie117 0:6616d722fed3 175 flag_m = true;
Wallie117 0:6616d722fed3 176 }
Wallie117 0:6616d722fed3 177
Wallie117 1:0e1d91965b8e 178 void samplego() // ticker function, set flag to true every sample interval
Wallie117 1:0e1d91965b8e 179 {
EmilyJCZ 3:f9a1df2271d2 180 if(flag_calibration == false)
EmilyJCZ 3:f9a1df2271d2 181 {
EmilyJCZ 3:f9a1df2271d2 182 red.write(led_off); // Toggles red calibration LED off
EmilyJCZ 3:f9a1df2271d2 183 green.write(led_on); // Toggles green sampling LED on
EmilyJCZ 3:f9a1df2271d2 184 hoog_laag_filter();
EmilyJCZ 3:f9a1df2271d2 185 sample_go = 0;
EmilyJCZ 3:f9a1df2271d2 186 sample_go = 1;
EmilyJCZ 3:f9a1df2271d2 187 }
Wallie117 0:6616d722fed3 188 }
Wallie117 0:6616d722fed3 189
Wallie117 0:6616d722fed3 190
Wallie117 0:6616d722fed3 191 //================================================================================================== HEAD LOOP ================================================================================================================
Wallie117 1:0e1d91965b8e 192 int main()
Wallie117 0:6616d722fed3 193 {
Wallie117 0:6616d722fed3 194 pc.baud(115200);
Wallie117 0:6616d722fed3 195 Sensor_control.attach(&SENSOR_LOOP, 0.01); // TICKER FUNCTION
Wallie117 0:6616d722fed3 196 Motor_control.attach(&MOTOR_LOOP, 0.01);
Wallie117 0:6616d722fed3 197 EMG_Control.attach(&samplego, (float)1/Fs);
Wallie117 1:0e1d91965b8e 198
Wallie117 1:0e1d91965b8e 199 led_groen.write(0);
Wallie117 1:0e1d91965b8e 200 led_geel.write(0);
Wallie117 1:0e1d91965b8e 201 led_rood.write(0);
Wallie117 1:0e1d91965b8e 202
Wallie117 0:6616d722fed3 203 pc.printf("===============================================================\n");
Wallie117 0:6616d722fed3 204 pc.printf(" \t\t\t Ball-E\n");
Wallie117 0:6616d722fed3 205 pc.printf("In the module Biorobotics on the University of Twente is this script created\n");
Wallie117 0:6616d722fed3 206 pc.printf("Autors:\tE. Velu, L. Verhoeven, R. v/d Wal, T. v/d Wal, E. Zoetbrood\n\n");
Wallie117 0:6616d722fed3 207 wait(3);
Wallie117 0:6616d722fed3 208 pc.printf("The script will begin with a short calibration\n\n");
Wallie117 0:6616d722fed3 209 wait(2.5);
Wallie117 0:6616d722fed3 210 pc.printf("===============================================================\n");
EmilyJCZ 3:f9a1df2271d2 211
Wallie117 0:6616d722fed3 212 //************************* CONTROL 1-EMG CALIBRATION ****************************
EmilyJCZ 3:f9a1df2271d2 213 while(1)
EmilyJCZ 3:f9a1df2271d2 214 {
EmilyJCZ 3:f9a1df2271d2 215 if(sample_go)
EmilyJCZ 3:f9a1df2271d2 216 {
Wallie117 0:6616d722fed3 217 sample_go = 0;
Wallie117 1:0e1d91965b8e 218 }
Wallie117 1:0e1d91965b8e 219
EmilyJCZ 3:f9a1df2271d2 220 if (flag_calibration) // function to calibrate the emg signals from the user. It takes 5 seconds of measurements of maximum output, then takes the max and normalizes to that.
EmilyJCZ 3:f9a1df2271d2 221 {
EmilyJCZ 3:f9a1df2271d2 222 calibratie_metingen ++;
Wallie117 1:0e1d91965b8e 223 cali_fact1 = 0.9; // calibration factor to normalize filter output to a scale of 0 - 1
Wallie117 1:0e1d91965b8e 224 cali_fact2 = 0.9;
Wallie117 1:0e1d91965b8e 225 double cali_max1 = 0; // declare max y1
Wallie117 1:0e1d91965b8e 226 double cali_max2 = 0; //declare max y2
Wallie117 1:0e1d91965b8e 227 pc.printf(" \n\n EMG Signal starting up Calibration measurement........... \n");
EmilyJCZ 3:f9a1df2271d2 228
Wallie117 1:0e1d91965b8e 229 wait(2);
Wallie117 1:0e1d91965b8e 230 led_rood.write(0);
Wallie117 1:0e1d91965b8e 231 wait(0.2);
Wallie117 1:0e1d91965b8e 232 led_rood.write(1); //Toggles red calibration LED on
Wallie117 1:0e1d91965b8e 233 wait(0.2);
Wallie117 1:0e1d91965b8e 234 led_rood.write(0);
Wallie117 1:0e1d91965b8e 235 wait(0.2);
Wallie117 1:0e1d91965b8e 236 led_rood.write(1);
Wallie117 1:0e1d91965b8e 237 wait(0.2);
Wallie117 1:0e1d91965b8e 238 led_rood.write(0);
Wallie117 1:0e1d91965b8e 239 wait(0.2);
Wallie117 1:0e1d91965b8e 240 led_rood.write(1);
Wallie117 1:0e1d91965b8e 241 wait(1);
EmilyJCZ 3:f9a1df2271d2 242
Wallie117 1:0e1d91965b8e 243 pc.printf("\t.....Calibrating Signal of EMG 1 and 2 .......\n");
Wallie117 1:0e1d91965b8e 244 led_rood.write(0);
Wallie117 1:0e1d91965b8e 245 led_geel.write(1);
Wallie117 1:0e1d91965b8e 246 wait(0.5);
Wallie117 1:0e1d91965b8e 247 pc.printf("\t......contract muscles..... \n");
Wallie117 1:0e1d91965b8e 248
EmilyJCZ 3:f9a1df2271d2 249 for(int cali_index1 = 0; cali_index1 < 2560; cali_index1++) // Records 5 seconds of y1
EmilyJCZ 3:f9a1df2271d2 250 {
Wallie117 1:0e1d91965b8e 251 hoog_laag_filter();
Wallie117 1:0e1d91965b8e 252 cali_array1[cali_index1] = y1;
Wallie117 1:0e1d91965b8e 253 wait((float)1/Fs);
Wallie117 1:0e1d91965b8e 254 }
EmilyJCZ 3:f9a1df2271d2 255 for(int cali_index2 = 0; cali_index2 < 2560; cali_index2++) // Records 5 seconds of y2
EmilyJCZ 3:f9a1df2271d2 256 {
Wallie117 1:0e1d91965b8e 257 hoog_laag_filter();
Wallie117 1:0e1d91965b8e 258 cali_array2[cali_index2] = y2;
Wallie117 1:0e1d91965b8e 259 wait((float)1/Fs);
Wallie117 1:0e1d91965b8e 260 }
EmilyJCZ 3:f9a1df2271d2 261 for(int cali_index3 = 0; cali_index3 < 2560; cali_index3++) // Scales y1
EmilyJCZ 3:f9a1df2271d2 262 {
EmilyJCZ 3:f9a1df2271d2 263 if(cali_array1[cali_index3] > cali_max1)
EmilyJCZ 3:f9a1df2271d2 264 {
Wallie117 1:0e1d91965b8e 265 cali_max1 = cali_array1[cali_index3];
Wallie117 0:6616d722fed3 266 }
Wallie117 1:0e1d91965b8e 267 }
EmilyJCZ 3:f9a1df2271d2 268 for(int cali_index4 = 0; cali_index4 < 2560; cali_index4++) // Scales y2
EmilyJCZ 3:f9a1df2271d2 269 {
Wallie117 1:0e1d91965b8e 270 if(cali_array2[cali_index4] > cali_max2) {
Wallie117 1:0e1d91965b8e 271 cali_max2 = cali_array2[cali_index4];
Wallie117 0:6616d722fed3 272 }
Wallie117 1:0e1d91965b8e 273 }
Wallie117 1:0e1d91965b8e 274 cali_fact1 = (double)1/cali_max1; // Calibration factor for y1
Wallie117 1:0e1d91965b8e 275 cali_fact2 = (double)1/cali_max2; // Calibration factor for y2
Wallie117 1:0e1d91965b8e 276
Wallie117 1:0e1d91965b8e 277 // Toggles green sampling LED off
Wallie117 1:0e1d91965b8e 278 led_geel.write(0);
Wallie117 1:0e1d91965b8e 279 pc.printf(" \t....... Calibration has been completed!\n");
Wallie117 1:0e1d91965b8e 280 wait(0.2);
Wallie117 1:0e1d91965b8e 281 led_groen.write(led_off);
Wallie117 1:0e1d91965b8e 282 wait(0.2);
Wallie117 1:0e1d91965b8e 283 led_groen.write(led_on);
Wallie117 1:0e1d91965b8e 284 wait(0.2);
Wallie117 1:0e1d91965b8e 285 led_groen.write(led_off);
Wallie117 1:0e1d91965b8e 286 wait(0.2);
Wallie117 1:0e1d91965b8e 287 led_groen.write(led_on);
Wallie117 1:0e1d91965b8e 288 wait(4);
Wallie117 1:0e1d91965b8e 289 pc.printf("Beginning with Ball-E board settings\n");
Wallie117 1:0e1d91965b8e 290 led_groen.write(led_off);
Wallie117 1:0e1d91965b8e 291 wait(2);
Wallie117 1:0e1d91965b8e 292 y1 = 0;
Wallie117 1:0e1d91965b8e 293 y2 = 0;
Wallie117 1:0e1d91965b8e 294
Wallie117 1:0e1d91965b8e 295 j = 1; // Wachten van fase switch initialiseren
Wallie117 1:0e1d91965b8e 296 fase_switch_wait = true;
EmilyJCZ 3:f9a1df2271d2 297 flag_calibration = false;
Wallie117 1:0e1d91965b8e 298 }
Wallie117 1:0e1d91965b8e 299
Wallie117 1:0e1d91965b8e 300
Wallie117 1:0e1d91965b8e 301 //************************* CONTROL MOTOR ****************************************
EmilyJCZ 3:f9a1df2271d2 302 if (flag_s)
EmilyJCZ 3:f9a1df2271d2 303 {
Wallie117 1:0e1d91965b8e 304 flag_calibration = false;
Wallie117 1:0e1d91965b8e 305 }
Wallie117 1:0e1d91965b8e 306 //************************* FASE SWITCH ******************************************
Wallie117 1:0e1d91965b8e 307 //******************** Fase determination *************
EmilyJCZ 3:f9a1df2271d2 308 if (fase_switch_wait == true)
EmilyJCZ 3:f9a1df2271d2 309 {
Wallie117 1:0e1d91965b8e 310 j++;
Wallie117 1:0e1d91965b8e 311 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 312 pc.printf("waarde j = %i \n",j);
Wallie117 2:de3bb38dae4e 313 led_rood.write(0);
Wallie117 2:de3bb38dae4e 314 led_groen.write(1);
Wallie117 2:de3bb38dae4e 315 led_geel.write(0);
Wallie117 1:0e1d91965b8e 316 }
Wallie117 1:0e1d91965b8e 317
EmilyJCZ 3:f9a1df2271d2 318 if( j > N)
EmilyJCZ 3:f9a1df2271d2 319 {
Wallie117 1:0e1d91965b8e 320 fase_switch_wait = false;
EmilyJCZ 3:f9a1df2271d2 321 switch(fase)
EmilyJCZ 3:f9a1df2271d2 322 {
Wallie117 1:0e1d91965b8e 323 //******************* Fase 1 **************************
Wallie117 1:0e1d91965b8e 324 case(1):
EmilyJCZ 3:f9a1df2271d2 325 led_rood.write(1);
EmilyJCZ 3:f9a1df2271d2 326 led_groen.write(0);
EmilyJCZ 3:f9a1df2271d2 327 led_geel.write(0);
Wallie117 1:0e1d91965b8e 328 rondjes = 0;
EmilyJCZ 3:f9a1df2271d2 329 if (y1> contract)
EmilyJCZ 3:f9a1df2271d2 330 {
Wallie117 1:0e1d91965b8e 331 flag_right = false;
Wallie117 1:0e1d91965b8e 332 flag_left = true;
Wallie117 0:6616d722fed3 333 }
Wallie117 1:0e1d91965b8e 334
EmilyJCZ 3:f9a1df2271d2 335 if (y2 > contract)
EmilyJCZ 3:f9a1df2271d2 336 {
Wallie117 1:0e1d91965b8e 337 flag_right = true;
Wallie117 1:0e1d91965b8e 338 flag_left = false;
Wallie117 0:6616d722fed3 339 }
Wallie117 1:0e1d91965b8e 340
EmilyJCZ 3:f9a1df2271d2 341 if (pos_board < maxleft)
EmilyJCZ 3:f9a1df2271d2 342 {
Wallie117 1:0e1d91965b8e 343 flag_left = false;
Wallie117 1:0e1d91965b8e 344 motor2speed.write(relax);
Wallie117 1:0e1d91965b8e 345 }
Wallie117 1:0e1d91965b8e 346
EmilyJCZ 3:f9a1df2271d2 347 if (pos_board > maxright)
EmilyJCZ 3:f9a1df2271d2 348 {
Wallie117 1:0e1d91965b8e 349 flag_right = false;
Wallie117 1:0e1d91965b8e 350 motor2speed.write(relax);
Wallie117 0:6616d722fed3 351 }
Wallie117 1:0e1d91965b8e 352
EmilyJCZ 3:f9a1df2271d2 353 if (flag_left == true)
EmilyJCZ 3:f9a1df2271d2 354 {
EmilyJCZ 3:f9a1df2271d2 355 if (y1> contract)
EmilyJCZ 3:f9a1df2271d2 356 {
Wallie117 1:0e1d91965b8e 357 motor2direction.write(ccw);
Wallie117 1:0e1d91965b8e 358 motor2speed.write(speed_plate_1);
EmilyJCZ 3:f9a1df2271d2 359 }
EmilyJCZ 3:f9a1df2271d2 360 else
EmilyJCZ 3:f9a1df2271d2 361 {
Wallie117 1:0e1d91965b8e 362 motor2speed.write(relax);
Wallie117 1:0e1d91965b8e 363 }
Wallie117 1:0e1d91965b8e 364 }
Wallie117 1:0e1d91965b8e 365
EmilyJCZ 3:f9a1df2271d2 366 if (flag_right == true)
EmilyJCZ 3:f9a1df2271d2 367 {
EmilyJCZ 3:f9a1df2271d2 368 if (y2 > contract)
EmilyJCZ 3:f9a1df2271d2 369 {
Wallie117 1:0e1d91965b8e 370 motor2direction.write(cw);
Wallie117 1:0e1d91965b8e 371 motor2speed.write(speed_plate_1);
EmilyJCZ 3:f9a1df2271d2 372 }
EmilyJCZ 3:f9a1df2271d2 373 else
EmilyJCZ 3:f9a1df2271d2 374 {
Wallie117 1:0e1d91965b8e 375 motor2speed.write(relax);
Wallie117 1:0e1d91965b8e 376 }
Wallie117 0:6616d722fed3 377 }
Wallie117 1:0e1d91965b8e 378 pc.printf("Boardposition \t %f EMG1 en EMG2 signaal = %f \t %f\n", pos_board, y1, y2);
EmilyJCZ 3:f9a1df2271d2 379 if (y1> fasecontract && y2 > fasecontract)
EmilyJCZ 3:f9a1df2271d2 380 {
Wallie117 1:0e1d91965b8e 381 motor2speed.write(relax);
Wallie117 1:0e1d91965b8e 382 fase = 2;
Wallie117 1:0e1d91965b8e 383 fase_switch_wait = true;
Wallie117 2:de3bb38dae4e 384 led_rood.write(0);
Wallie117 2:de3bb38dae4e 385 led_groen.write(0);
Wallie117 2:de3bb38dae4e 386 led_geel.write(1);
Wallie117 1:0e1d91965b8e 387 j = 0;
Wallie117 1:0e1d91965b8e 388 }
Wallie117 1:0e1d91965b8e 389 break;
Wallie117 1:0e1d91965b8e 390 //******************* Fase 2 **************************
Wallie117 1:0e1d91965b8e 391 case(2):
EmilyJCZ 3:f9a1df2271d2 392 led_rood.write(0);
EmilyJCZ 3:f9a1df2271d2 393 led_groen.write(0);
EmilyJCZ 3:f9a1df2271d2 394 led_geel.write(1);
Wallie117 1:0e1d91965b8e 395 motor1direction.write(cw);
Wallie117 1:0e1d91965b8e 396 pos_arm1 = (pos_arm - (rondjes * 360));
Wallie117 1:0e1d91965b8e 397
EmilyJCZ 3:f9a1df2271d2 398 switch(switch_rondjes)
EmilyJCZ 3:f9a1df2271d2 399 {
Wallie117 0:6616d722fed3 400 case(1):
Wallie117 1:0e1d91965b8e 401 rondjes ++;
Wallie117 1:0e1d91965b8e 402 switch_rondjes = 2;
Wallie117 1:0e1d91965b8e 403 break;
Wallie117 0:6616d722fed3 404 case(2):
EmilyJCZ 3:f9a1df2271d2 405 if(pos_arm1>360 & 368<pos_arm1)
EmilyJCZ 3:f9a1df2271d2 406 {
Wallie117 1:0e1d91965b8e 407 switch_rondjes = 1;
Wallie117 1:0e1d91965b8e 408 }
Wallie117 1:0e1d91965b8e 409 break;
Wallie117 0:6616d722fed3 410 }
Wallie117 1:0e1d91965b8e 411
EmilyJCZ 3:f9a1df2271d2 412 if (y2 > minimum_contract & y2 < medium_contract)
EmilyJCZ 3:f9a1df2271d2 413 {
Wallie117 0:6616d722fed3 414 motor1speed.write((y2 - minimum_contract)/(medium_contract - minimum_contract));
Wallie117 1:0e1d91965b8e 415 }
EmilyJCZ 3:f9a1df2271d2 416 if (y2 > medium_contract) // Hoger dan drempelwaarde = Actief
EmilyJCZ 3:f9a1df2271d2 417 {
Wallie117 1:0e1d91965b8e 418 motor1speed.write(1);
Wallie117 1:0e1d91965b8e 419 }
EmilyJCZ 3:f9a1df2271d2 420 if (y2 < minimum_contract) // Lager dan drempel = Rust
EmilyJCZ 3:f9a1df2271d2 421 {
Wallie117 1:0e1d91965b8e 422 motor1speed.write(relax);
Wallie117 0:6616d722fed3 423 }
Wallie117 1:0e1d91965b8e 424
EmilyJCZ 3:f9a1df2271d2 425 if(rondjes == pos_armrondjes_max) // max aantal draaing gemaakt!!!!!!
EmilyJCZ 3:f9a1df2271d2 426 {
Wallie117 1:0e1d91965b8e 427 problem1 = true;
Wallie117 1:0e1d91965b8e 428 problem2 = true;
Wallie117 1:0e1d91965b8e 429 motor1speed.write(relax);
EmilyJCZ 3:f9a1df2271d2 430
EmilyJCZ 3:f9a1df2271d2 431 while (problem1)
EmilyJCZ 3:f9a1df2271d2 432 {
Wallie117 1:0e1d91965b8e 433 j++;
Wallie117 1:0e1d91965b8e 434 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 435 Encoderread1 = wheel1.getPulses();
Wallie117 1:0e1d91965b8e 436 pos_arm = (Encoderread1/((64.0*131.0)/360.0)); // Omrekenen naar graden van arm
Wallie117 1:0e1d91965b8e 437
EmilyJCZ 3:f9a1df2271d2 438 if( j > N)
EmilyJCZ 3:f9a1df2271d2 439 {
EmilyJCZ 3:f9a1df2271d2 440 problem1 = false;
Wallie117 1:0e1d91965b8e 441 }
EmilyJCZ 3:f9a1df2271d2 442 }
EmilyJCZ 3:f9a1df2271d2 443
Wallie117 1:0e1d91965b8e 444 wait(0.1);
Wallie117 1:0e1d91965b8e 445 led_rood.write(0);
Wallie117 1:0e1d91965b8e 446 wait(0.1);
Wallie117 1:0e1d91965b8e 447 led_rood.write(1);
Wallie117 1:0e1d91965b8e 448 wait(0.1);
Wallie117 1:0e1d91965b8e 449 led_rood.write(0);
Wallie117 1:0e1d91965b8e 450 wait(0.1);
Wallie117 1:0e1d91965b8e 451 led_rood.write(1);
Wallie117 1:0e1d91965b8e 452 wait(0.1);
Wallie117 1:0e1d91965b8e 453 led_rood.write(0);
Wallie117 1:0e1d91965b8e 454 wait(0.1);
Wallie117 1:0e1d91965b8e 455 led_rood.write(1);
Wallie117 2:de3bb38dae4e 456 wait(1.5);
Wallie117 1:0e1d91965b8e 457
EmilyJCZ 3:f9a1df2271d2 458 while(problem2)
EmilyJCZ 3:f9a1df2271d2 459 {
Wallie117 2:de3bb38dae4e 460 motor1direction.write(ccw);
Wallie117 2:de3bb38dae4e 461 motor1speed.write(0.02);
EmilyJCZ 3:f9a1df2271d2 462
EmilyJCZ 3:f9a1df2271d2 463 if (flag_s)
EmilyJCZ 3:f9a1df2271d2 464 {
Wallie117 1:0e1d91965b8e 465 Encoderread1 = wheel1.getPulses();
Wallie117 1:0e1d91965b8e 466 pos_arm = (Encoderread1/((64.0*131.0)/360.0)); // Omrekenen naar graden van arm
Wallie117 1:0e1d91965b8e 467 }
EmilyJCZ 3:f9a1df2271d2 468 if (pos_arm < 10)
EmilyJCZ 3:f9a1df2271d2 469 {
Wallie117 1:0e1d91965b8e 470 problem2 = false;
Wallie117 2:de3bb38dae4e 471 motor1speed.write(0);
Wallie117 2:de3bb38dae4e 472 rondjes = 0;
Wallie117 1:0e1d91965b8e 473 wait(1);
Wallie117 1:0e1d91965b8e 474 }
Wallie117 1:0e1d91965b8e 475 }
Wallie117 0:6616d722fed3 476 }
EmilyJCZ 3:f9a1df2271d2 477 if (pos_arm1 > minimum_throw_angle & pos_arm1 < maximum_throw_angle)
EmilyJCZ 3:f9a1df2271d2 478 {
EmilyJCZ 3:f9a1df2271d2 479 if (y1> maximum_leftbiceps)
EmilyJCZ 3:f9a1df2271d2 480 {
Wallie117 0:6616d722fed3 481 magnet.write(off);
Wallie117 0:6616d722fed3 482 motor1speed.write(relax);
Wallie117 0:6616d722fed3 483 fase = 3;
Wallie117 0:6616d722fed3 484 pc.printf("Van fase 2 naar fase 3\n");
EmilyJCZ 3:f9a1df2271d2 485
Wallie117 0:6616d722fed3 486 wait(2);
Wallie117 0:6616d722fed3 487 j = 0;
Wallie117 1:0e1d91965b8e 488 fase_switch_wait = true;
Wallie117 0:6616d722fed3 489 }
Wallie117 1:0e1d91965b8e 490 }
Wallie117 1:0e1d91965b8e 491 pc.printf("Armposition \t %i \t EMG1 en EMG2 = %f \t %f \n", pos_arm1, y1, y2);
Wallie117 0:6616d722fed3 492 break;
Wallie117 1:0e1d91965b8e 493 //********************************************* Fase 3 **********************************************
Wallie117 1:0e1d91965b8e 494 case(3):
EmilyJCZ 3:f9a1df2271d2 495 led_rood.write(0);
EmilyJCZ 3:f9a1df2271d2 496 led_groen.write(1);
EmilyJCZ 3:f9a1df2271d2 497 led_geel.write(0);
EmilyJCZ 3:f9a1df2271d2 498 switch(switch_reset)
EmilyJCZ 3:f9a1df2271d2 499 {
Wallie117 1:0e1d91965b8e 500 case(1):
EmilyJCZ 3:f9a1df2271d2 501 if(pos_arm < reset_positie) //ene kant op draaien
EmilyJCZ 3:f9a1df2271d2 502 {
Wallie117 1:0e1d91965b8e 503 motor1direction.write(cw);
EmilyJCZ 3:f9a1df2271d2 504 speedcompensation2 = 0.05; //((reset_arm - pos_arm1)/900.0 + (0.02));
Wallie117 1:0e1d91965b8e 505 motor1speed.write(speedcompensation2);
Wallie117 1:0e1d91965b8e 506 }
EmilyJCZ 3:f9a1df2271d2 507 if(pos_arm > reset_positie) //andere kant op
EmilyJCZ 3:f9a1df2271d2 508 {
EmilyJCZ 3:f9a1df2271d2 509 motor1direction.write(ccw);
EmilyJCZ 3:f9a1df2271d2 510 speedcompensation2 = 0.05;//((pos_arm1 - reset_arm)/500.0 + (0.02));
EmilyJCZ 3:f9a1df2271d2 511 motor1speed.write(speedcompensation2);
EmilyJCZ 3:f9a1df2271d2 512 }
EmilyJCZ 3:f9a1df2271d2 513 if(pos_arm < reset_positie+5 && pos_arm > reset_positie-5) // Dit gaat niet goed komen, omdat het precies die waarde moet zijn
EmilyJCZ 3:f9a1df2271d2 514 {
EmilyJCZ 3:f9a1df2271d2 515 motor1speed.write(0);
EmilyJCZ 3:f9a1df2271d2 516 arm = true;
EmilyJCZ 3:f9a1df2271d2 517 switch_reset = 2;
EmilyJCZ 3:f9a1df2271d2 518 }
Wallie117 1:0e1d91965b8e 519 pc.printf("Positie_arm = %f \t \t snelheid = %f \n",pos_arm, speedcompensation);
Wallie117 1:0e1d91965b8e 520 wait(0.0001);
Wallie117 1:0e1d91965b8e 521 break;
Wallie117 1:0e1d91965b8e 522
Wallie117 1:0e1d91965b8e 523 case(2):
Wallie117 1:0e1d91965b8e 524 pc.printf("\t switch magneet automatisch \n");
Wallie117 2:de3bb38dae4e 525 wait(0.2);
Wallie117 1:0e1d91965b8e 526 magnet.write(on);
Wallie117 2:de3bb38dae4e 527 wait(2);
Wallie117 1:0e1d91965b8e 528 switch_reset = 3;
Wallie117 1:0e1d91965b8e 529 break;
EmilyJCZ 3:f9a1df2271d2 530
Wallie117 1:0e1d91965b8e 531 case(3):
EmilyJCZ 3:f9a1df2271d2 532 if(pos_board < reset_board)
EmilyJCZ 3:f9a1df2271d2 533 {
Wallie117 1:0e1d91965b8e 534 motor2direction.write(cw);
Wallie117 2:de3bb38dae4e 535 speedcompensation = 0.05; //((reset_board - pos_board1)/500.0 + (0.1));
Wallie117 1:0e1d91965b8e 536 motor2speed.write(speedcompensation);
Wallie117 1:0e1d91965b8e 537 }
Wallie117 0:6616d722fed3 538
EmilyJCZ 3:f9a1df2271d2 539 if(pos_board > reset_board)
EmilyJCZ 3:f9a1df2271d2 540 {
Wallie117 1:0e1d91965b8e 541 motor2direction.write(ccw);
Wallie117 2:de3bb38dae4e 542 speedcompensation = 0.05;//((pos_board1 - reset_board)/500.0 + (0.05));
Wallie117 1:0e1d91965b8e 543 motor2speed.write(speedcompensation);
Wallie117 0:6616d722fed3 544 }
EmilyJCZ 3:f9a1df2271d2 545
EmilyJCZ 3:f9a1df2271d2 546 if(pos_board < reset_board+5 && pos_board > reset_board-5)
EmilyJCZ 3:f9a1df2271d2 547 {
Wallie117 1:0e1d91965b8e 548 motor2speed.write(0);
Wallie117 1:0e1d91965b8e 549 board1 = true;
Wallie117 0:6616d722fed3 550 }
EmilyJCZ 3:f9a1df2271d2 551
EmilyJCZ 3:f9a1df2271d2 552 if(board1 == true)
EmilyJCZ 3:f9a1df2271d2 553 {
Wallie117 1:0e1d91965b8e 554 red=0;
Wallie117 1:0e1d91965b8e 555 pc.printf("fase switch naar 1\n\n");
Wallie117 1:0e1d91965b8e 556 board1 = false;
Wallie117 1:0e1d91965b8e 557 arm = false;
Wallie117 1:0e1d91965b8e 558 // flag_calibration = true; !!!!! Moet naar gekeken worden of we elke spel willen calibreren
Wallie117 1:0e1d91965b8e 559 wait(2);
Wallie117 1:0e1d91965b8e 560 games_played ++;
Wallie117 1:0e1d91965b8e 561 games_played1 = games_played - (3*calibratie_metingen - 3);
Wallie117 1:0e1d91965b8e 562 pc.printf("Games played total count = %i and loop %i\n",games_played,games_played1);
Wallie117 1:0e1d91965b8e 563
EmilyJCZ 3:f9a1df2271d2 564 if(games_played1 == 3)
EmilyJCZ 3:f9a1df2271d2 565 {
Wallie117 1:0e1d91965b8e 566 flag_calibration = true;
EmilyJCZ 3:f9a1df2271d2 567 while(t)
EmilyJCZ 3:f9a1df2271d2 568 {
Wallie117 1:0e1d91965b8e 569 pc.printf("\tCalibratie begint over %i \n",t);
Wallie117 1:0e1d91965b8e 570 t--;
Wallie117 1:0e1d91965b8e 571 led_geel.write(1);
Wallie117 1:0e1d91965b8e 572 wait(0.5);
Wallie117 1:0e1d91965b8e 573 led_geel.write(0);
Wallie117 1:0e1d91965b8e 574 wait(0.5);
Wallie117 1:0e1d91965b8e 575 }
Wallie117 1:0e1d91965b8e 576 }
EmilyJCZ 3:f9a1df2271d2 577 while(t)
EmilyJCZ 3:f9a1df2271d2 578 {
Wallie117 1:0e1d91965b8e 579 pc.printf("\tNieuw spel begint over %i \n",t);
Wallie117 1:0e1d91965b8e 580 t--;
Wallie117 1:0e1d91965b8e 581 led_geel.write(1);
Wallie117 1:0e1d91965b8e 582 wait(0.5);
Wallie117 1:0e1d91965b8e 583 led_geel.write(0);
Wallie117 1:0e1d91965b8e 584 wait(0.5);
Wallie117 1:0e1d91965b8e 585 }
Wallie117 1:0e1d91965b8e 586
Wallie117 0:6616d722fed3 587 fase = 1; // Terug naar fase 1
Wallie117 0:6616d722fed3 588 switch_reset = 1; // De switch op orginele locatie zetten
Wallie117 0:6616d722fed3 589 t = 5;
Wallie117 1:0e1d91965b8e 590
Wallie117 1:0e1d91965b8e 591 }
Wallie117 1:0e1d91965b8e 592
Wallie117 1:0e1d91965b8e 593 pc.printf("Positie_board = %f \t \t snelheid = %f \n",pos_board, speedcompensation);
Wallie117 1:0e1d91965b8e 594 wait(0.0001);
Wallie117 1:0e1d91965b8e 595 break;
Wallie117 0:6616d722fed3 596 }
Wallie117 1:0e1d91965b8e 597 break;
Wallie117 1:0e1d91965b8e 598 //=================================================== STOP SCRIPT ============================================================
Wallie117 0:6616d722fed3 599 }
Wallie117 1:0e1d91965b8e 600 }
Wallie117 0:6616d722fed3 601 }
Wallie117 0:6616d722fed3 602 }