Biorobotics10 / Mbed 2 deprecated Samenvoegen6

Het script van Ball-E die werkt!

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@0:6616d722fed3, 2015-10-19 (annotated)

Committer:
Wallie117
Date:
Mon Oct 19 07:07:21 2015 +0000
Revision:
0:6616d722fed3
Het script wat Volledig klopt!!!! Alleen verbeteringen maken die versnelling en het script mooier maken

Who changed what in which revision?

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