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@2:de3bb38dae4e, 2015-10-21 (annotated)

Committer:
Wallie117
Date:
Wed Oct 21 11:32:48 2015 +0000
Revision:
2:de3bb38dae4e
Parent:
1:0e1d91965b8e
Child:
3:f9a1df2271d2
Dit is het script van de robot, er zijn problemen gekomen door while loops in te zetten. Dit verstoord het lezen van het EMG signaal. Het EMG signaal en Encoder signaal moet in de Tickers komen!!

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