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@6:1518fccc5616, 2015-10-26 (annotated)

Committer:
EmilyJCZ
Date:
Mon Oct 26 11:13:37 2015 +0000
Revision:
6:1518fccc5616
Parent:
5:0597358d0016
Child:
7:d55569b92f30
jlfs

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