Biorobotics10 / Mbed 2 deprecated Ball-E_script

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of Samenvoegen_7_5 by Biorobotics10

main.cpp@1:0e1d91965b8e, 2015-10-20 (annotated)

Committer:
Wallie117
Date:
Tue Oct 20 09:29:32 2015 +0000
Revision:
1:0e1d91965b8e
Parent:
0:6616d722fed3
Child:
2:de3bb38dae4e
Bezig met lampjes toevoegen en case(1) van reset bezig met gemiddelde nemen van 5 seconden iets aflezen door arrays te gebruiken. Probleem misschien nog bij calibratie

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 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 0:6616d722fed3 36 DigitalOut motor2direction(D4); // D4 en D5 zijn motor 2 (op het motorshield)
Wallie117 0:6616d722fed3 37 PwmOut motor2speed(D5);
Wallie117 0:6616d722fed3 38 DigitalOut motor1direction(D7); // D6 en D7 zijn motor 1 (op het motorshield)
Wallie117 0:6616d722fed3 39 PwmOut motor1speed(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 0:6616d722fed3 104 float Encoderread2 = 0;
Wallie117 0:6616d722fed3 105
Wallie117 0:6616d722fed3 106
Wallie117 0:6616d722fed3 107 //********* VARIABLES FOR CONTROL 3 ************
Wallie117 0:6616d722fed3 108 const float minimum_contract = 0.2; // Certain levels for muscle activity to activate motor
Wallie117 0:6616d722fed3 109 const float medium_contract = 0.5; // "Medium contract muscle"
Wallie117 0:6616d722fed3 110 const float maximum_leftbiceps = 0.8; // "Maximum contract muscle"
Wallie117 0:6616d722fed3 111 const float on = 1.0;
Wallie117 0:6616d722fed3 112 const float off = 0.0;
Wallie117 0:6616d722fed3 113 const float minimum_throw_angle = -5;
Wallie117 0:6616d722fed3 114 const float maximum_throw_angle = 110;
Wallie117 0:6616d722fed3 115 float pos_arm = 0;
Wallie117 0:6616d722fed3 116 int pos_arm1 = 0;
Wallie117 0:6616d722fed3 117 float Encoderread1 = 0;
Wallie117 0:6616d722fed3 118 int switch_rondjes = 2;
Wallie117 0:6616d722fed3 119 int rondjes = 0;
Wallie117 1:0e1d91965b8e 120 float pos_armrondjes_max = 5;
Wallie117 0:6616d722fed3 121
Wallie117 1:0e1d91965b8e 122 bool problem1 = false;
Wallie117 1:0e1d91965b8e 123 bool problem2 = false;
Wallie117 1:0e1d91965b8e 124 float problem_velocity = 0;
Wallie117 1:0e1d91965b8e 125 //********* VARIABLES FOR CONTROL 4 ************
Wallie117 0:6616d722fed3 126
Wallie117 0:6616d722fed3 127 float reset_positie = 0; // Belangrijk
Wallie117 0:6616d722fed3 128 float reset_board = 0;
Wallie117 0:6616d722fed3 129 float speedcompensation;
Wallie117 0:6616d722fed3 130 float speedcompensation2;
Wallie117 0:6616d722fed3 131 int t = 5; // aftellen naar nieuw spel
Wallie117 0:6616d722fed3 132 int switch_reset = 1;
Wallie117 0:6616d722fed3 133 bool arm = false;
Wallie117 0:6616d722fed3 134 bool board1 = false;
Wallie117 0:6616d722fed3 135
Wallie117 0:6616d722fed3 136
Wallie117 0:6616d722fed3 137 // **************************************** Tickers *****************************************
Wallie117 0:6616d722fed3 138 Ticker Sensor_control; // Adds ticker function for the variable function : Sensors
Wallie117 1:0e1d91965b8e 139 Ticker Motor_control;
Wallie117 0:6616d722fed3 140 Ticker EMG_Control;
Wallie117 0:6616d722fed3 141
Wallie117 0:6616d722fed3 142 //=============================================================================================== SUB LOOPS ==================================================================================================================
Wallie117 0:6616d722fed3 143 //**************************** CONTROL 1-EMG CALIBRATION ***********************************
Wallie117 0:6616d722fed3 144
Wallie117 0:6616d722fed3 145 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 146 biquadFilter highpass2(high_a2, high_a3, high_b1, high_b2, high_b3);
Wallie117 0:6616d722fed3 147 biquadFilter lowpass1(low_a2, low_a3, low_b1, low_b2, low_b3);
Wallie117 0:6616d722fed3 148 biquadFilter lowpass2(low_a2, low_a3, low_b1, low_b2, low_b3);
Wallie117 0:6616d722fed3 149
Wallie117 1:0e1d91965b8e 150
Wallie117 0:6616d722fed3 151 double cali_fact1 = 0.9; // calibration factor to normalize filter output to a scale of 0 - 1
Wallie117 0:6616d722fed3 152 double cali_fact2 = 0.9;
Wallie117 0:6616d722fed3 153 double cali_array1[2560] = {}; // array to store values in
Wallie117 1:0e1d91965b8e 154 double cali_array2[2560] = {};
Wallie117 0:6616d722fed3 155
Wallie117 1:0e1d91965b8e 156 void hoog_laag_filter()
Wallie117 0:6616d722fed3 157 {
Wallie117 1:0e1d91965b8e 158 u1 = EMG1;
Wallie117 0:6616d722fed3 159 u2 = EMG2;
Wallie117 1:0e1d91965b8e 160 y1 = highpass1.step(u1);
Wallie117 0:6616d722fed3 161 y2 = highpass2.step(u2); // filter order is: high-pass --> rectify --> low-pass
Wallie117 1:0e1d91965b8e 162 y1 = fabs(y1);
Wallie117 1:0e1d91965b8e 163 y2 = fabs(y2);
Wallie117 0:6616d722fed3 164 y1 = lowpass1.step(y1)*cali_fact1;
Wallie117 0:6616d722fed3 165 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 166 }
Wallie117 1:0e1d91965b8e 167
Wallie117 0:6616d722fed3 168
Wallie117 0:6616d722fed3 169 //======================================= TICKER LOOPS ==========================================
Wallie117 0:6616d722fed3 170 void SENSOR_LOOP()
Wallie117 0:6616d722fed3 171 {
Wallie117 0:6616d722fed3 172 flag_s = true;
Wallie117 0:6616d722fed3 173 }
Wallie117 0:6616d722fed3 174
Wallie117 0:6616d722fed3 175 void MOTOR_LOOP()
Wallie117 0:6616d722fed3 176 {
Wallie117 0:6616d722fed3 177 flag_m = true;
Wallie117 0:6616d722fed3 178 }
Wallie117 0:6616d722fed3 179
Wallie117 1:0e1d91965b8e 180 void samplego() // ticker function, set flag to true every sample interval
Wallie117 1:0e1d91965b8e 181 {
Wallie117 0:6616d722fed3 182 sample_go = 1;
Wallie117 0:6616d722fed3 183 }
Wallie117 0:6616d722fed3 184
Wallie117 0:6616d722fed3 185
Wallie117 0:6616d722fed3 186 //================================================================================================== HEAD LOOP ================================================================================================================
Wallie117 1:0e1d91965b8e 187 int main()
Wallie117 0:6616d722fed3 188 {
Wallie117 1:0e1d91965b8e 189
Wallie117 0:6616d722fed3 190 pc.baud(115200);
Wallie117 0:6616d722fed3 191 Sensor_control.attach(&SENSOR_LOOP, 0.01); // TICKER FUNCTION
Wallie117 0:6616d722fed3 192 Motor_control.attach(&MOTOR_LOOP, 0.01);
Wallie117 0:6616d722fed3 193 EMG_Control.attach(&samplego, (float)1/Fs);
Wallie117 1:0e1d91965b8e 194
Wallie117 1:0e1d91965b8e 195 led_groen.write(0);
Wallie117 1:0e1d91965b8e 196 led_geel.write(0);
Wallie117 1:0e1d91965b8e 197 led_rood.write(0);
Wallie117 1:0e1d91965b8e 198
Wallie117 0:6616d722fed3 199 pc.printf("===============================================================\n");
Wallie117 0:6616d722fed3 200 pc.printf(" \t\t\t Ball-E\n");
Wallie117 0:6616d722fed3 201 pc.printf("In the module Biorobotics on the University of Twente is this script created\n");
Wallie117 0:6616d722fed3 202 pc.printf("Autors:\tE. Velu, L. Verhoeven, R. v/d Wal, T. v/d Wal, E. Zoetbrood\n\n");
Wallie117 0:6616d722fed3 203 wait(3);
Wallie117 0:6616d722fed3 204 pc.printf("The script will begin with a short calibration\n\n");
Wallie117 0:6616d722fed3 205 wait(2.5);
Wallie117 0:6616d722fed3 206 pc.printf("===============================================================\n");
Wallie117 1:0e1d91965b8e 207
Wallie117 0:6616d722fed3 208 //************************* CONTROL 1-EMG CALIBRATION ****************************
Wallie117 1:0e1d91965b8e 209 while(1) {
Wallie117 1:0e1d91965b8e 210
Wallie117 1:0e1d91965b8e 211 if(sample_go) {
Wallie117 0:6616d722fed3 212 red.write(led_off); // Toggles red calibration LED off
Wallie117 0:6616d722fed3 213 green.write(led_on); // Toggles green sampling LED on
Wallie117 0:6616d722fed3 214 hoog_laag_filter();
Wallie117 0:6616d722fed3 215 sample_go = 0;
Wallie117 1:0e1d91965b8e 216 }
Wallie117 1:0e1d91965b8e 217
Wallie117 1:0e1d91965b8e 218 if (flag_calibration) {
Wallie117 1:0e1d91965b8e 219 // 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 220 calibratie_metingen ++;
Wallie117 1:0e1d91965b8e 221 cali_fact1 = 0.9; // calibration factor to normalize filter output to a scale of 0 - 1
Wallie117 1:0e1d91965b8e 222 cali_fact2 = 0.9;
Wallie117 1:0e1d91965b8e 223 double cali_max1 = 0; // declare max y1
Wallie117 1:0e1d91965b8e 224 double cali_max2 = 0; //declare max y2
Wallie117 1:0e1d91965b8e 225 pc.printf(" \n\n EMG Signal starting up Calibration measurement........... \n");
Wallie117 1:0e1d91965b8e 226 wait(2);
Wallie117 1:0e1d91965b8e 227 led_rood.write(0);
Wallie117 1:0e1d91965b8e 228 wait(0.2);
Wallie117 1:0e1d91965b8e 229 led_rood.write(1); //Toggles red calibration LED on
Wallie117 1:0e1d91965b8e 230 wait(0.2);
Wallie117 1:0e1d91965b8e 231 led_rood.write(0);
Wallie117 1:0e1d91965b8e 232 wait(0.2);
Wallie117 1:0e1d91965b8e 233 led_rood.write(1);
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(1);
Wallie117 0:6616d722fed3 239
Wallie117 1:0e1d91965b8e 240 pc.printf("\t.....Calibrating Signal of EMG 1 and 2 .......\n");
Wallie117 1:0e1d91965b8e 241 led_rood.write(0);
Wallie117 1:0e1d91965b8e 242 led_geel.write(1);
Wallie117 1:0e1d91965b8e 243 wait(0.5);
Wallie117 1:0e1d91965b8e 244 pc.printf("\t......contract muscles..... \n");
Wallie117 1:0e1d91965b8e 245
Wallie117 1:0e1d91965b8e 246 for(int cali_index1 = 0; cali_index1 < 2560; cali_index1++) { // Records 5 seconds of y1
Wallie117 1:0e1d91965b8e 247 hoog_laag_filter();
Wallie117 1:0e1d91965b8e 248 cali_array1[cali_index1] = y1;
Wallie117 1:0e1d91965b8e 249 wait((float)1/Fs);
Wallie117 1:0e1d91965b8e 250 }
Wallie117 1:0e1d91965b8e 251 for(int cali_index2 = 0; cali_index2 < 2560; cali_index2++) { // Records 5 seconds of y2
Wallie117 1:0e1d91965b8e 252 hoog_laag_filter();
Wallie117 1:0e1d91965b8e 253 cali_array2[cali_index2] = y2;
Wallie117 1:0e1d91965b8e 254 wait((float)1/Fs);
Wallie117 1:0e1d91965b8e 255 }
Wallie117 1:0e1d91965b8e 256 for(int cali_index3 = 0; cali_index3 < 2560; cali_index3++) { // Scales y1
Wallie117 1:0e1d91965b8e 257 if(cali_array1[cali_index3] > cali_max1) {
Wallie117 1:0e1d91965b8e 258 cali_max1 = cali_array1[cali_index3];
Wallie117 0:6616d722fed3 259 }
Wallie117 1:0e1d91965b8e 260 }
Wallie117 1:0e1d91965b8e 261 for(int cali_index4 = 0; cali_index4 < 2560; cali_index4++) { // Scales y2
Wallie117 1:0e1d91965b8e 262 if(cali_array2[cali_index4] > cali_max2) {
Wallie117 1:0e1d91965b8e 263 cali_max2 = cali_array2[cali_index4];
Wallie117 0:6616d722fed3 264 }
Wallie117 1:0e1d91965b8e 265 }
Wallie117 1:0e1d91965b8e 266 cali_fact1 = (double)1/cali_max1; // Calibration factor for y1
Wallie117 1:0e1d91965b8e 267 cali_fact2 = (double)1/cali_max2; // Calibration factor for y2
Wallie117 1:0e1d91965b8e 268
Wallie117 1:0e1d91965b8e 269 // Toggles green sampling LED off
Wallie117 1:0e1d91965b8e 270 led_geel.write(0);
Wallie117 1:0e1d91965b8e 271 pc.printf(" \t....... Calibration has been completed!\n");
Wallie117 1:0e1d91965b8e 272 wait(0.2);
Wallie117 1:0e1d91965b8e 273 led_groen.write(led_off);
Wallie117 1:0e1d91965b8e 274 wait(0.2);
Wallie117 1:0e1d91965b8e 275 led_groen.write(led_on);
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(4);
Wallie117 1:0e1d91965b8e 281 pc.printf("Beginning with Ball-E board settings\n");
Wallie117 1:0e1d91965b8e 282 led_groen.write(led_off);
Wallie117 1:0e1d91965b8e 283 wait(2);
Wallie117 1:0e1d91965b8e 284 y1 = 0;
Wallie117 1:0e1d91965b8e 285 y2 = 0;
Wallie117 1:0e1d91965b8e 286
Wallie117 1:0e1d91965b8e 287 j = 1; // Wachten van fase switch initialiseren
Wallie117 1:0e1d91965b8e 288 fase_switch_wait = true;
Wallie117 1:0e1d91965b8e 289 }
Wallie117 1:0e1d91965b8e 290
Wallie117 1:0e1d91965b8e 291
Wallie117 1:0e1d91965b8e 292 //************************* CONTROL MOTOR ****************************************
Wallie117 1:0e1d91965b8e 293
Wallie117 1:0e1d91965b8e 294
Wallie117 1:0e1d91965b8e 295
Wallie117 1:0e1d91965b8e 296 if (flag_s) {
Wallie117 1:0e1d91965b8e 297 Encoderread1 = wheel1.getPulses();
Wallie117 1:0e1d91965b8e 298 pos_arm = (Encoderread1/((64.0*131.0)/360.0)); // Omrekenen naar graden van arm
Wallie117 0:6616d722fed3 299
Wallie117 1:0e1d91965b8e 300 Encoderread2 = wheel2.getPulses();
Wallie117 1:0e1d91965b8e 301 pos_board = (Encoderread2/((64.0*131.0)/360.0)); // Omrekenen naar graden van board
Wallie117 1:0e1d91965b8e 302 flag_calibration = false;
Wallie117 1:0e1d91965b8e 303 }
Wallie117 1:0e1d91965b8e 304 //************************* FASE SWITCH ******************************************
Wallie117 1:0e1d91965b8e 305 //******************** Fase determination *************
Wallie117 1:0e1d91965b8e 306 if (fase_switch_wait == true) {
Wallie117 1:0e1d91965b8e 307 j++;
Wallie117 1:0e1d91965b8e 308 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 309 pc.printf("waarde j = %i \n",j);
Wallie117 1:0e1d91965b8e 310 }
Wallie117 1:0e1d91965b8e 311
Wallie117 1:0e1d91965b8e 312 if( j > N) {
Wallie117 1:0e1d91965b8e 313 fase_switch_wait = false;
Wallie117 1:0e1d91965b8e 314 switch(fase) {
Wallie117 1:0e1d91965b8e 315
Wallie117 1:0e1d91965b8e 316 //******************* Fase 1 **************************
Wallie117 1:0e1d91965b8e 317 case(1):
Wallie117 1:0e1d91965b8e 318 rondjes = 0;
Wallie117 1:0e1d91965b8e 319 if (y1> contract) {
Wallie117 1:0e1d91965b8e 320 flag_right = false;
Wallie117 1:0e1d91965b8e 321 flag_left = true;
Wallie117 1:0e1d91965b8e 322 led_rood.write(0);
Wallie117 1:0e1d91965b8e 323 led_groen.write(0);
Wallie117 1:0e1d91965b8e 324 led_geel.write(1);
Wallie117 0:6616d722fed3 325 }
Wallie117 1:0e1d91965b8e 326
Wallie117 1:0e1d91965b8e 327 if (y2 > contract) {
Wallie117 1:0e1d91965b8e 328 flag_right = true;
Wallie117 1:0e1d91965b8e 329 flag_left = false;
Wallie117 1:0e1d91965b8e 330 led_rood.write(0);
Wallie117 1:0e1d91965b8e 331 led_groen.write(0);
Wallie117 1:0e1d91965b8e 332 led_geel.write(1);
Wallie117 0:6616d722fed3 333 }
Wallie117 1:0e1d91965b8e 334
Wallie117 1:0e1d91965b8e 335 if (pos_board < maxleft) {
Wallie117 1:0e1d91965b8e 336 flag_left = false;
Wallie117 1:0e1d91965b8e 337 motor2speed.write(relax);
Wallie117 1:0e1d91965b8e 338 led_rood.write(1);
Wallie117 1:0e1d91965b8e 339 led_groen.write(0);
Wallie117 1:0e1d91965b8e 340 led_geel.write(0);
Wallie117 1:0e1d91965b8e 341 }
Wallie117 1:0e1d91965b8e 342
Wallie117 1:0e1d91965b8e 343 if (pos_board > maxright) {
Wallie117 1:0e1d91965b8e 344 flag_right = false;
Wallie117 1:0e1d91965b8e 345 motor2speed.write(relax);
Wallie117 1:0e1d91965b8e 346 led_rood.write(1);
Wallie117 1:0e1d91965b8e 347 led_groen.write(0);
Wallie117 1:0e1d91965b8e 348 led_geel.write(0);
Wallie117 0:6616d722fed3 349 }
Wallie117 1:0e1d91965b8e 350
Wallie117 1:0e1d91965b8e 351 if (flag_left == true) {
Wallie117 1:0e1d91965b8e 352 if (y1> contract) {
Wallie117 1:0e1d91965b8e 353 motor2direction.write(ccw);
Wallie117 1:0e1d91965b8e 354 motor2speed.write(speed_plate_1);
Wallie117 1:0e1d91965b8e 355 } else {
Wallie117 1:0e1d91965b8e 356 motor2speed.write(relax);
Wallie117 1:0e1d91965b8e 357 }
Wallie117 1:0e1d91965b8e 358 }
Wallie117 1:0e1d91965b8e 359
Wallie117 1:0e1d91965b8e 360 if (flag_right == true) {
Wallie117 1:0e1d91965b8e 361 if (y2 > contract) {
Wallie117 1:0e1d91965b8e 362 motor2direction.write(cw);
Wallie117 1:0e1d91965b8e 363 motor2speed.write(speed_plate_1);
Wallie117 1:0e1d91965b8e 364 } else {
Wallie117 1:0e1d91965b8e 365 motor2speed.write(relax);
Wallie117 1:0e1d91965b8e 366 }
Wallie117 0:6616d722fed3 367 }
Wallie117 1:0e1d91965b8e 368 pc.printf("Boardposition \t %f EMG1 en EMG2 signaal = %f \t %f\n", pos_board, y1, y2);
Wallie117 1:0e1d91965b8e 369 if (y1> fasecontract && y2 > fasecontract) {
Wallie117 1:0e1d91965b8e 370 motor2speed.write(relax);
Wallie117 1:0e1d91965b8e 371 fase = 2;
Wallie117 1:0e1d91965b8e 372 fase_switch_wait = true;
Wallie117 1:0e1d91965b8e 373 j = 0;
Wallie117 1:0e1d91965b8e 374 led_rood.write(0);
Wallie117 1:0e1d91965b8e 375 led_groen.write(1);
Wallie117 1:0e1d91965b8e 376 led_geel.write(0);
Wallie117 1:0e1d91965b8e 377 }
Wallie117 1:0e1d91965b8e 378 break;
Wallie117 1:0e1d91965b8e 379 //******************* Fase 2 **************************
Wallie117 1:0e1d91965b8e 380 case(2):
Wallie117 1:0e1d91965b8e 381 motor1direction.write(cw);
Wallie117 1:0e1d91965b8e 382 pos_arm1 = (pos_arm - (rondjes * 360));
Wallie117 1:0e1d91965b8e 383
Wallie117 1:0e1d91965b8e 384 switch(switch_rondjes) {
Wallie117 0:6616d722fed3 385 case(1):
Wallie117 1:0e1d91965b8e 386 rondjes ++;
Wallie117 1:0e1d91965b8e 387 switch_rondjes = 2;
Wallie117 1:0e1d91965b8e 388 break;
Wallie117 0:6616d722fed3 389 case(2):
Wallie117 1:0e1d91965b8e 390 if(pos_arm1>360 & 368<pos_arm1) {
Wallie117 1:0e1d91965b8e 391 switch_rondjes = 1;
Wallie117 1:0e1d91965b8e 392 }
Wallie117 1:0e1d91965b8e 393 break;
Wallie117 0:6616d722fed3 394 }
Wallie117 1:0e1d91965b8e 395
Wallie117 1:0e1d91965b8e 396
Wallie117 1:0e1d91965b8e 397 if (y2 > minimum_contract & y2 < medium_contract) {
Wallie117 0:6616d722fed3 398 motor1speed.write((y2 - minimum_contract)/(medium_contract - minimum_contract));
Wallie117 1:0e1d91965b8e 399 led_rood.write(0);
Wallie117 1:0e1d91965b8e 400 led_groen.write(0);
Wallie117 1:0e1d91965b8e 401 led_geel.write(1);
Wallie117 1:0e1d91965b8e 402 }
Wallie117 1:0e1d91965b8e 403 if (y2 > medium_contract) { // Hoger dan drempelwaarde = Actief
Wallie117 1:0e1d91965b8e 404 motor1speed.write(1);
Wallie117 1:0e1d91965b8e 405 led_rood.write(0);
Wallie117 1:0e1d91965b8e 406 led_groen.write(0);
Wallie117 1:0e1d91965b8e 407 led_geel.write(1);
Wallie117 1:0e1d91965b8e 408 }
Wallie117 1:0e1d91965b8e 409 if (y2 < minimum_contract) { // Lager dan drempel = Rust
Wallie117 1:0e1d91965b8e 410 motor1speed.write(relax);
Wallie117 1:0e1d91965b8e 411 led_rood.write(0);
Wallie117 1:0e1d91965b8e 412 led_groen.write(1);
Wallie117 1:0e1d91965b8e 413 led_geel.write(0);
Wallie117 0:6616d722fed3 414 }
Wallie117 1:0e1d91965b8e 415
Wallie117 1:0e1d91965b8e 416 if( rondjes == pos_armrondjes_max) { // max aantal draaing gemaakt!!!!!!
Wallie117 1:0e1d91965b8e 417 problem1 = true;
Wallie117 1:0e1d91965b8e 418 problem2 = true;
Wallie117 1:0e1d91965b8e 419 motor1speed.write(relax);
Wallie117 1:0e1d91965b8e 420 led_rood.write(1);
Wallie117 1:0e1d91965b8e 421 led_groen.write(0);
Wallie117 1:0e1d91965b8e 422 led_geel.write(0);
Wallie117 1:0e1d91965b8e 423
Wallie117 1:0e1d91965b8e 424 while (problem1) {
Wallie117 1:0e1d91965b8e 425 j++;
Wallie117 1:0e1d91965b8e 426 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 427 Encoderread1 = wheel1.getPulses();
Wallie117 1:0e1d91965b8e 428 pos_arm = (Encoderread1/((64.0*131.0)/360.0)); // Omrekenen naar graden van arm
Wallie117 1:0e1d91965b8e 429
Wallie117 1:0e1d91965b8e 430 if( j > N) {
Wallie117 1:0e1d91965b8e 431 problem1 = false;
Wallie117 1:0e1d91965b8e 432 }
Wallie117 1:0e1d91965b8e 433 }
Wallie117 1:0e1d91965b8e 434
Wallie117 1:0e1d91965b8e 435 wait(0.1);
Wallie117 1:0e1d91965b8e 436 led_rood.write(0);
Wallie117 1:0e1d91965b8e 437 wait(0.1);
Wallie117 1:0e1d91965b8e 438 led_rood.write(1);
Wallie117 1:0e1d91965b8e 439 wait(0.1);
Wallie117 1:0e1d91965b8e 440 led_rood.write(0);
Wallie117 1:0e1d91965b8e 441 wait(0.1);
Wallie117 1:0e1d91965b8e 442 led_rood.write(1);
Wallie117 1:0e1d91965b8e 443 wait(0.1);
Wallie117 1:0e1d91965b8e 444 led_rood.write(0);
Wallie117 1:0e1d91965b8e 445 wait(0.1);
Wallie117 1:0e1d91965b8e 446 led_rood.write(1);
Wallie117 0:6616d722fed3 447 }
Wallie117 1:0e1d91965b8e 448
Wallie117 1:0e1d91965b8e 449 while(problem2) {
Wallie117 1:0e1d91965b8e 450 motor1speed.write(relax);
Wallie117 1:0e1d91965b8e 451 wait(1.5);
Wallie117 1:0e1d91965b8e 452 while(1) {
Wallie117 1:0e1d91965b8e 453 led_rood.write(0);
Wallie117 1:0e1d91965b8e 454 led_groen.write(1);
Wallie117 1:0e1d91965b8e 455 led_geel.write(0);
Wallie117 1:0e1d91965b8e 456
Wallie117 1:0e1d91965b8e 457 motor1direction.write(0);
Wallie117 1:0e1d91965b8e 458 problem_velocity = 0.05 + (pos_arm/720)*0.4;
Wallie117 1:0e1d91965b8e 459 motor1speed.write(problem_velocity);
Wallie117 1:0e1d91965b8e 460
Wallie117 1:0e1d91965b8e 461 if (flag_s) {
Wallie117 1:0e1d91965b8e 462 Encoderread1 = wheel1.getPulses();
Wallie117 1:0e1d91965b8e 463 pos_arm = (Encoderread1/((64.0*131.0)/360.0)); // Omrekenen naar graden van arm
Wallie117 1:0e1d91965b8e 464 }
Wallie117 1:0e1d91965b8e 465 if (pos_arm < 0) {
Wallie117 1:0e1d91965b8e 466 led_rood.write(0);
Wallie117 1:0e1d91965b8e 467 led_groen.write(1);
Wallie117 1:0e1d91965b8e 468 led_geel.write(0);
Wallie117 1:0e1d91965b8e 469 problem2 = false;
Wallie117 1:0e1d91965b8e 470 wait(1);
Wallie117 1:0e1d91965b8e 471 }
Wallie117 1:0e1d91965b8e 472 }
Wallie117 0:6616d722fed3 473 }
Wallie117 1:0e1d91965b8e 474 if (pos_arm1 > minimum_throw_angle & pos_arm1 < maximum_throw_angle) {
Wallie117 1:0e1d91965b8e 475 if (y1> maximum_leftbiceps) {
Wallie117 0:6616d722fed3 476 magnet.write(off);
Wallie117 0:6616d722fed3 477 motor1speed.write(relax);
Wallie117 0:6616d722fed3 478 fase = 3;
Wallie117 0:6616d722fed3 479 pc.printf("Van fase 2 naar fase 3\n");
Wallie117 1:0e1d91965b8e 480
Wallie117 1:0e1d91965b8e 481 led_rood.write(0);
Wallie117 1:0e1d91965b8e 482 led_groen.write(1);
Wallie117 1:0e1d91965b8e 483 led_geel.write(0);
Wallie117 1:0e1d91965b8e 484
Wallie117 0:6616d722fed3 485 wait(2);
Wallie117 0:6616d722fed3 486 j = 0;
Wallie117 1:0e1d91965b8e 487 fase_switch_wait = true;
Wallie117 1:0e1d91965b8e 488
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):
Wallie117 1:0e1d91965b8e 495
Wallie117 1:0e1d91965b8e 496 switch(switch_reset) {
Wallie117 1:0e1d91965b8e 497 case(1):
Wallie117 1:0e1d91965b8e 498 if(pos_arm < reset_positie) { //ene kant op draaien
Wallie117 1:0e1d91965b8e 499 motor1direction.write(cw);
Wallie117 1:0e1d91965b8e 500 speedcompensation2 = ((reset_positie - pos_arm)/900.0 + (0.1));
Wallie117 1:0e1d91965b8e 501 motor1speed.write(speedcompensation2);
Wallie117 1:0e1d91965b8e 502 }
Wallie117 1:0e1d91965b8e 503 if(pos_arm > reset_positie) { //andere kant op
Wallie117 1:0e1d91965b8e 504 motor1direction.write(ccw);
Wallie117 1:0e1d91965b8e 505 speedcompensation2 = ((pos_arm - reset_positie)/500.0 + (0.1));
Wallie117 1:0e1d91965b8e 506 motor1speed.write(speedcompensation2);
Wallie117 1:0e1d91965b8e 507 }
Wallie117 1:0e1d91965b8e 508 if(pos_arm < reset_positie+3 && pos_arm > reset_positie-3) { // Dit gaat niet goed komen, omdat het precies die waarde moet zijn
Wallie117 1:0e1d91965b8e 509 for(int cali_index1 = 0; cali_index1 < 2560; cali_index1++)
Wallie117 1:0e1d91965b8e 510
Wallie117 1:0e1d91965b8e 511 motor1speed.write(0);
Wallie117 1:0e1d91965b8e 512 arm = true;
Wallie117 1:0e1d91965b8e 513 switch_reset = 2;
Wallie117 1:0e1d91965b8e 514 }
Wallie117 1:0e1d91965b8e 515 pc.printf("Positie_arm = %f \t \t snelheid = %f \n",pos_arm, speedcompensation);
Wallie117 1:0e1d91965b8e 516 wait(0.0001);
Wallie117 1:0e1d91965b8e 517 break;
Wallie117 1:0e1d91965b8e 518
Wallie117 1:0e1d91965b8e 519 case(2):
Wallie117 1:0e1d91965b8e 520 pc.printf("\t switch magneet automatisch \n");
Wallie117 1:0e1d91965b8e 521 wait(1);
Wallie117 1:0e1d91965b8e 522 magnet.write(on);
Wallie117 1:0e1d91965b8e 523 wait(0.5);
Wallie117 1:0e1d91965b8e 524 switch_reset = 3;
Wallie117 1:0e1d91965b8e 525 break;
Wallie117 1:0e1d91965b8e 526
Wallie117 1:0e1d91965b8e 527 case(3):
Wallie117 1:0e1d91965b8e 528 if(pos_board < reset_board) {
Wallie117 1:0e1d91965b8e 529 motor2direction.write(cw);
Wallie117 1:0e1d91965b8e 530 speedcompensation = ((reset_board - pos_board)/500.0 + (0.1));
Wallie117 1:0e1d91965b8e 531 motor2speed.write(speedcompensation);
Wallie117 1:0e1d91965b8e 532 }
Wallie117 0:6616d722fed3 533
Wallie117 1:0e1d91965b8e 534 if(pos_board > reset_board) {
Wallie117 1:0e1d91965b8e 535 motor2direction.write(ccw);
Wallie117 1:0e1d91965b8e 536 speedcompensation = ((pos_board - reset_board)/500.0 + (0.1));
Wallie117 1:0e1d91965b8e 537 motor2speed.write(speedcompensation);
Wallie117 0:6616d722fed3 538 }
Wallie117 1:0e1d91965b8e 539 if(pos_board < reset_board+5 && pos_board > reset_board-5) {
Wallie117 1:0e1d91965b8e 540 motor2speed.write(0);
Wallie117 1:0e1d91965b8e 541 board1 = true;
Wallie117 0:6616d722fed3 542 }
Wallie117 1:0e1d91965b8e 543 if(board1 == true) {
Wallie117 1:0e1d91965b8e 544 red=0;
Wallie117 1:0e1d91965b8e 545 pc.printf("fase switch naar 1\n\n");
Wallie117 1:0e1d91965b8e 546 board1 = false;
Wallie117 1:0e1d91965b8e 547 arm = false;
Wallie117 1:0e1d91965b8e 548 // flag_calibration = true; !!!!! Moet naar gekeken worden of we elke spel willen calibreren
Wallie117 1:0e1d91965b8e 549 wait(2);
Wallie117 1:0e1d91965b8e 550 games_played ++;
Wallie117 1:0e1d91965b8e 551 games_played1 = games_played - (3*calibratie_metingen - 3);
Wallie117 1:0e1d91965b8e 552 pc.printf("Games played total count = %i and loop %i\n",games_played,games_played1);
Wallie117 1:0e1d91965b8e 553
Wallie117 1:0e1d91965b8e 554 if(games_played1 == 3) {
Wallie117 1:0e1d91965b8e 555
Wallie117 1:0e1d91965b8e 556 flag_calibration = true;
Wallie117 1:0e1d91965b8e 557 while(t) {
Wallie117 1:0e1d91965b8e 558 pc.printf("\tCalibratie begint over %i \n",t);
Wallie117 1:0e1d91965b8e 559 t--;
Wallie117 1:0e1d91965b8e 560 led_geel.write(1);
Wallie117 1:0e1d91965b8e 561 wait(0.5);
Wallie117 1:0e1d91965b8e 562 led_geel.write(0);
Wallie117 1:0e1d91965b8e 563 wait(0.5);
Wallie117 1:0e1d91965b8e 564 }
Wallie117 1:0e1d91965b8e 565 }
Wallie117 1:0e1d91965b8e 566 while(t) {
Wallie117 1:0e1d91965b8e 567 pc.printf("\tNieuw spel begint over %i \n",t);
Wallie117 1:0e1d91965b8e 568 t--;
Wallie117 1:0e1d91965b8e 569 led_geel.write(1);
Wallie117 1:0e1d91965b8e 570 wait(0.5);
Wallie117 1:0e1d91965b8e 571 led_geel.write(0);
Wallie117 1:0e1d91965b8e 572 wait(0.5);
Wallie117 1:0e1d91965b8e 573 }
Wallie117 1:0e1d91965b8e 574
Wallie117 0:6616d722fed3 575 fase = 1; // Terug naar fase 1
Wallie117 0:6616d722fed3 576 switch_reset = 1; // De switch op orginele locatie zetten
Wallie117 0:6616d722fed3 577 t = 5;
Wallie117 1:0e1d91965b8e 578
Wallie117 1:0e1d91965b8e 579 }
Wallie117 1:0e1d91965b8e 580
Wallie117 1:0e1d91965b8e 581 pc.printf("Positie_board = %f \t \t snelheid = %f \n",pos_board, speedcompensation);
Wallie117 1:0e1d91965b8e 582 wait(0.0001);
Wallie117 1:0e1d91965b8e 583 break;
Wallie117 0:6616d722fed3 584 }
Wallie117 1:0e1d91965b8e 585 break;
Wallie117 1:0e1d91965b8e 586 //=================================================== STOP SCRIPT ============================================================
Wallie117 0:6616d722fed3 587 }
Wallie117 1:0e1d91965b8e 588 }
Wallie117 0:6616d722fed3 589 }
Wallie117 0:6616d722fed3 590 }