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

Committer:
Wallie117
Date:
Fri Oct 23 09:47:23 2015 +0000
Revision:
4:f9f75c913d7d
Parent:
3:f9a1df2271d2
Child:
5:0597358d0016
Er is een snelheidslimiet gemaakt waardoor de arm met een bepaalde snelheid zal bewegen.

Who changed what in which revision?

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