Thomas Burgers / Mbed 2 deprecated ZZ-TheChenneRobot

The Chenne Robot

Dependencies:   Encoder HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@110:58c6a32659d3, 2015-11-03 (annotated)

Committer:
ThomasBNL
Date:
Tue Nov 03 15:55:41 2015 +0000
Revision:
110:58c6a32659d3
Parent:
108:873e56f92691 
Final Version Robot

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ThomasBNL 46:9279c7a725bf 1 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 2 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 3 ///// //// ////// ////// /////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 4 ///// //// ////// ////// /////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 5 //////////// //////////// ////// ////// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 6 //////////// //////////// ////// ////// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 7 //////////// //////////// ////// /////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 8 //////////// //////////// ////// /////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 9 //////////// //////////// ////// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 10 //////////// //////////// ////// ////// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 11 //////////// //////////// ////// ////// /////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 12 //////////// //////////// ////// ////// /////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 13 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 14 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 15 /////// ////// ////// ////// ////// /////// ////// /////// ////// /////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 16 /////// ////// ////// ////// ////// ///// ////// ///// ////// /////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 17 /////// ///////////////// ////// ////// //////////// //// ////// //// ////// ///////////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 18 /////// ///////////////// ////// ////// //////////// /// ////// /// ////// ///////////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 19 /////// ///////////////// ////// ////// // ////// // ////// /////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 20 /////// ///////////////// ////// ////// // ////// // ////// /////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 21 /////// ///////////////// ////// //////////// /// ////// /// ////// ///////////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 22 /////// ///////////////// ////// ////// //////////// //// ////// //// ////// ///////////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 23 /////// ////// ////// ////// ////// ///// ////// ///// ////// /////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 24 /////// ////// ////// ////// ////// ////// ////// ////// ////// /////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 25 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 26 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 27 /////// /////////////// ///////////// /////// ////// /////////////////////////////////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 28 /////// ///////////// //////////// ///// ////// ////////////////////////////////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 29 /////// ////// /////////// /////////// //// ////// ///// ///////////////////////////////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 30 /////// ////// ////////// // ////////// /// ////// ///// ///////////////////////////////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 31 /////// ///////// //// ///////// // ////// ///// ///////////////////////////////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 32 /////// //////// ////// //////// // ////// ///// ///////////////////////////////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 33 /////// ////// /////// /////// /// ////// ///// ///////////////////////////////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 34 /////// ////// ////// ////// //// ////// ///// ///////////////////////////////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 35 /////// ////// //////////// ///// ///// ////// ////////////////////////////////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 36 /////// ////// ////////////// //// ////// ////// /////////////////////////////////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 37 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 38 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 46:9279c7a725bf 39 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 110:58c6a32659d3 40 // Groep 12: The Chenne Band ///
ThomasBNL 47:c61873a0b646 41
ThomasBNL 46:9279c7a725bf 42 //============================================================================================================================
ThomasBNL 43:fb69ef657f30 43 // ___________________________
ThomasBNL 43:fb69ef657f30 44 // // \\
ThomasBNL 43:fb69ef657f30 45 // || [Libraries] ||
ThomasBNL 43:fb69ef657f30 46 // \\___________________________//
ThomasBNL 43:fb69ef657f30 47 //
ThomasBNL 0:40052f5ca77b 48 #include "mbed.h"
ThomasBNL 21:c75210216204 49 #include "HIDScope.h"
ThomasBNL 0:40052f5ca77b 50 #include "QEI.h"
ThomasBNL 0:40052f5ca77b 51 #include "MODSERIAL.h"
ThomasBNL 8:50d6e2323d3b 52 #include "biquadFilter.h"
ThomasBNL 0:40052f5ca77b 53 #include "encoder.h"
ThomasBNL 0:40052f5ca77b 54
ThomasBNL 46:9279c7a725bf 55 //============================================================================================================================
ThomasBNL 34:c672f5c0763f 56 // ___________________________
ThomasBNL 34:c672f5c0763f 57 // // \\
ThomasBNL 78:bcf0b5627b47 58 // || [Tuning Device] ||
ThomasBNL 78:bcf0b5627b47 59 // \\___________________________//
ThomasBNL 78:bcf0b5627b47 60 //
ThomasBNL 105:ee681c33a900 61 // Calculated PID values (MATLAB)
ThomasBNL 108:873e56f92691 62 const double tuning1=0.0041211; // P_gain_strike value
ThomasBNL 108:873e56f92691 63 const double tuning2=0.039481; // I_gain_strike value
ThomasBNL 108:873e56f92691 64 const double tuning3=0; // D_gain_strike value
ThomasBNL 97:c23dd6dacb68 65
ThomasBNL 108:873e56f92691 66 const double tuning4=0.041211; // P_gain_turn value
ThomasBNL 108:873e56f92691 67 const double tuning5=0.39481; // I_gain_turn value
ThomasBNL 108:873e56f92691 68 const double tuning6=0; // D_gain_turn value
ThomasBNL 97:c23dd6dacb68 69
ThomasBNL 97:c23dd6dacb68 70 // FILTER: D-action
ThomasBNL 108:873e56f92691 71 const double a0 = 0.000021080713160785432, a1 = 0.000042161426321570865, a2 = 0.000021080713160785432, b1 = -1.990754082898736, b2 = 0.9908384057513788; //(0.75Hz Lowpass filter)
ThomasBNL 108:873e56f92691 72 biquadFilter Lowpassfilter_derivative(b1,b2,a0,a1,a2); // BiquadFilter used for the filtering of the Derivative action of the PID-action
ThomasBNL 97:c23dd6dacb68 73
ThomasBNL 97:c23dd6dacb68 74
ThomasBNL 78:bcf0b5627b47 75 // MOVEMENT ROBOT TUNING
ThomasBNL 108:873e56f92691 76 const double change_one_bottle=25; // Number of degrees between two bottles
ThomasBNL 108:873e56f92691 77 const double Hit=25; // Number of ccw degrees in comparison to starting position when bottle is hit
ThomasBNL 78:bcf0b5627b47 78
ThomasBNL 103:80b7824787e8 79 const double tuning24=0.10; // if <smaller than tuning18 than execute tuning 24 to prevent being stuck inside this loop (minimum pwm)
ThomasBNL 103:80b7824787e8 80 const double tuning18=0.1, tuning25=0.15; // First value: How much muscle percentage force is required (between 0-1)
ThomasBNL 103:80b7824787e8 81 const double tuning19=0.2, tuning26=0.21; // Second value: If after measurement period this value is the last (than what speed (pwm) does the motor get)
ThomasBNL 103:80b7824787e8 82 const double tuning20=0.3, tuning27=0.28;
ThomasBNL 103:80b7824787e8 83 const double tuning21=0.5, tuning28=0.38;
ThomasBNL 103:80b7824787e8 84 const double tuning22=0.7, tuning29=0.55;
ThomasBNL 102:9153868ce3d3 85 const double tuning23=0.9, tuning30=0.65;
ThomasBNL 78:bcf0b5627b47 86
ThomasBNL 105:ee681c33a900 87 // TURN LEFT
ThomasBNL 108:873e56f92691 88 const double tuning13=1.75; // error turn average smaller than (ENTRY) degrees than continue (BOTH FOR TURN LEFT AND TURN ACTION!
ThomasBNL 108:873e56f92691 89 const double tuning14=150; // how many samples at least before strike error is small enough (BOTH FOR TURN LEFT AND TURN ACTION!
ThomasBNL 78:bcf0b5627b47 90
ThomasBNL 78:bcf0b5627b47 91 //============================================================================================================================
ThomasBNL 78:bcf0b5627b47 92 // ___________________________
ThomasBNL 78:bcf0b5627b47 93 // // \\
ThomasBNL 46:9279c7a725bf 94 // || [FLOW AND DEBUGGING TOOLS] ||
ThomasBNL 34:c672f5c0763f 95 // \\___________________________//
ThomasBNL 66:04a203e43510 96
ThomasBNL 67:65750d716788 97 //HIDScope scope(1); // DEBUG : HIDSCOPE has the ability to display signals over time and can be used to monitor signals
ThomasBNL 46:9279c7a725bf 98
ThomasBNL 67:65750d716788 99 MODSERIAL pc(USBTX,USBRX); // MODSERIAL : makes it possible to send messages to the computer (eg. inside Putty)
ThomasBNL 34:c672f5c0763f 100
ThomasBNL 67:65750d716788 101 DigitalOut debug_led_red(LED_RED) , debug_led_green(LED_GREEN) , debug_led_blue(LED_BLUE); // DEBUG : Red, Blue and Green LED
ThomasBNL 66:04a203e43510 102 DigitalIn buttonL1(PTC6) , buttonL2(PTA4) , buttonH1(D2) , buttonH2(D1); // DEBUG/CALIBRATION: 4 buttons for calibration and debugging
ThomasBNL 67:65750d716788 103 Ticker looptimer; // FLOW : Ticker called looptimer to set a looptimerflag that puts the volatile bool control_go to true every sample
ThomasBNL 46:9279c7a725bf 104
ThomasBNL 77:ac32d64602a5 105 DigitalOut ledgreen1(D0), ledgreen2(D3), ledyellow1(PTC12), ledyellow2(D11), ledred1(D14), ledred2(D15);
ThomasBNL 76:09ace7f0a0bf 106
ThomasBNL 67:65750d716788 107 volatile bool control_go = false, sample = false, sample_error, sample_error_strike; // EMG : booleans for controlling sample time of moving average and emg signal
ThomasBNL 67:65750d716788 108 volatile bool looptimerflag; // CONTROLLER : boolean that controls the sample time of the whole controller
ThomasBNL 66:04a203e43510 109
ThomasBNL 67:65750d716788 110 double e30, e29, e28, e27, e26, e25, e24, e23, e22, e21, e20, // ACTION : in the action mechanism these variables calculate a current moving average error
ThomasBNL 67:65750d716788 111 e19, e18, e17, e16, e15, e14, e13, e12, e11, e10, e9,
ThomasBNL 67:65750d716788 112 e8, e7, e6, e5, e4, e3, e2, e1, er, error_count, error_turn_average, error_strike_average;
ThomasBNL 46:9279c7a725bf 113
ThomasBNL 67:65750d716788 114 AnalogIn input1(A0), input2(A1); // EMG : Two AnalogIn EMG inputs, input1 (Left bicep), input2 (Right bicep)
ThomasBNL 44:5dd0a3d24662 115
ThomasBNL 67:65750d716788 116 double Sample_EMG_L_1, Sample_EMG_L_2, Sample_EMG_L_3, Sample_EMG_L_4, Sample_EMG_L_5, Sample_EMG_L_6,// EMG : Left/Right bicep moving average memory variables (moving average is calculated over ten values)
ThomasBNL 66:04a203e43510 117 Sample_EMG_L_7, Sample_EMG_L_8, Sample_EMG_L_9, Sample_EMG_L_10, moving_average_left;
ThomasBNL 66:04a203e43510 118 double Sample_EMG_R_1, Sample_EMG_R_2, Sample_EMG_R_3, Sample_EMG_R_4, Sample_EMG_R_5, Sample_EMG_R_6,
ThomasBNL 66:04a203e43510 119 Sample_EMG_R_7, Sample_EMG_R_8, Sample_EMG_R_9, Sample_EMG_R_10, moving_average_right;
ThomasBNL 46:9279c7a725bf 120
ThomasBNL 66:04a203e43510 121 double minimum_L, maximum_L, EMG_L_min, EMG_L_max; // EMG CALIBRATION: variables that are used during the EMG calibration
ThomasBNL 66:04a203e43510 122 double minimum_R, maximum_R, EMG_R_min, EMG_R_max;
ThomasBNL 66:04a203e43510 123
ThomasBNL 67:65750d716788 124 double EMG_left_Bicep, EMG_Left_Bicep_filtered,
ThomasBNL 67:65750d716788 125 EMG_Left_Bicep_filtered_notch_1, EMG_Right_Bicep_filtered_notch_1;
ThomasBNL 67:65750d716788 126 double EMG_Right_Bicep, EMG_Left_Bicep_filtered_notch_2,
ThomasBNL 67:65750d716788 127 EMG_Right_Bicep_filtered_notch_2, EMG_Right_Bicep_filtered;
ThomasBNL 46:9279c7a725bf 128
ThomasBNL 86:4351907387ea 129 double Threshold_Bicep_Left_1, Threshold_Bicep_Left_2, // EMG ACTION: variables to witch the threshold values calculated after the get asigned to
ThomasBNL 67:65750d716788 130 Threshold_Bicep_Right_1, Threshold_Bicep_Right_2;
ThomasBNL 67:65750d716788 131
ThomasBNL 105:ee681c33a900 132 double n=0; double c=0; double k=0; double p=0; double a=0; int HA=0; double g=0; // FLOW : these flow variables are assigned to certain values through out the script values in order to regulate the flow of the script
ThomasBNL 54:9eb449571f4f 133
ThomasBNL 96:94f44c06f31a 134 // FILTERS EMG
ThomasBNL 96:94f44c06f31a 135 const double low_b0 = 0.05892937945281792 , low_b1 = 0.11785875890563584 , low_b2 = 0.05892937945281792, // Notch 1 LOW : VIA online biquad calculator Lowpass 520-48-0.7071-6
ThomasBNL 96:94f44c06f31a 136 low_a1 = -1.205716572226748 , low_a2 = 0.44143409003801976 ;
ThomasBNL 105:ee681c33a900 137 const double high_b0 = 0.6389437261127494 , high_b1 = -1.2778874522254988 , high_b2 = 0.6389437261127494, // Notch 2 HIGH: VIA online biquad calculator Highpass 520-52-0.7071-6
ThomasBNL 96:94f44c06f31a 138 high_a1 = -1.1429772843080923 , high_a2 = 0.41279762014290533 ;
ThomasBNL 105:ee681c33a900 139 const double high_b0_HP = 0.84855234544818812 , high_b1_HP = -1.6970469089637623 , high_b2_HP = 0.8485234544818812, // HIGHPASS : >25Hz sample rate 512Hz
ThomasBNL 96:94f44c06f31a 140 high_a1_HP = -1.6564788473046559 , high_a2_HP = 0.7376149706228688 ;
ThomasBNL 105:ee681c33a900 141 const double low_b0_LP = 0.0013067079602315681, low_b1_LP = 0.0026134159204631363, low_b2_LP = 0.0013067079602315681, // LOWPASS : <5-10 Hz sample rate 512Hz
ThomasBNL 96:94f44c06f31a 142 low_a1_LP = -1.9238184798980429 , low_a2_LP = 0.9290453117389691 ;
ThomasBNL 96:94f44c06f31a 143
ThomasBNL 96:94f44c06f31a 144 //Left bicep Filters
ThomasBNL 105:ee681c33a900 145 biquadFilter highpassfilter_1(high_a1_HP, high_a2_HP, high_b0_HP, high_b1_HP, high_b2_HP); // EMG :
ThomasBNL 105:ee681c33a900 146 biquadFilter notchL1(high_a1, high_a2, high_b0, high_b1, high_b2); // EMG :
ThomasBNL 105:ee681c33a900 147 biquadFilter notchL2(low_a1, low_a2, low_b0, low_b1, low_b2); // EMG :
ThomasBNL 96:94f44c06f31a 148 biquadFilter lowpassfilter_1(low_a1_LP, low_a2_LP, low_b0_LP, low_b1_LP, low_b2_LP);
ThomasBNL 96:94f44c06f31a 149
ThomasBNL 96:94f44c06f31a 150 // Right bicep Filters
ThomasBNL 105:ee681c33a900 151 biquadFilter highpassfilter_2(high_a1_HP, high_a2_HP, high_b0_HP, high_b1_HP, high_b2_HP); // EMG :
ThomasBNL 105:ee681c33a900 152 biquadFilter notchR1(high_a1, high_a2, high_b0, high_b1, high_b2); // EMG :
ThomasBNL 105:ee681c33a900 153 biquadFilter notchR2(low_a1, low_a2, low_b0, low_b1, low_b2); // EMG :
ThomasBNL 105:ee681c33a900 154 biquadFilter lowpassfilter_2(low_a1_LP, low_a2_LP, low_b0_LP, low_b1_LP, low_b2_LP); // EMG :
ThomasBNL 46:9279c7a725bf 155
ThomasBNL 66:04a203e43510 156 // MOTORS
ThomasBNL 67:65750d716788 157 QEI motor_turn(D12,D13,NC,32); QEI motor_strike(D9,D10,NC,32); // TURN - STRIKE : Encoder for motor
ThomasBNL 67:65750d716788 158 PwmOut pwm_motor_turn(D5); PwmOut pwm_motor_strike(D6); // TURN - STRIKE : Pwm for motor
ThomasBNL 67:65750d716788 159 DigitalOut motordirection_turn(D4); DigitalOut motordirection_strike(D7); // TURN - STRIKE : Direction of the motor
ThomasBNL 66:04a203e43510 160
ThomasBNL 67:65750d716788 161 double integrate_error_turn=0, previous_error_turn=0; double integrate_error_strike=0, previous_error_strike=0; // TURN - STRIKE : previous error and integration error motor
ThomasBNL 66:04a203e43510 162
ThomasBNL 66:04a203e43510 163 double position_turn, error_turn, reference_turn; double position_strike, error_strike, reference_strike;
ThomasBNL 67:65750d716788 164 double P_gain_turn; double I_gain_turn; double D_gain_turn; double P_gain_strike; double I_gain_strike; double D_gain_strike; // TURN - STRIKE : these gains get multiplied with the error inside the PID controller // TURN: these gains get multiplied with the error inside the PID controller
ThomasBNL 67:65750d716788 165 double pwm_motor_turn_P, pwm_motor_turn_I, pwm_motor_turn_D; double pwm_motor_strike_P, pwm_motor_strike_I, pwm_motor_strike_D; // TURN - STRIKE : variables that store the P, I and D action part of the error
ThomasBNL 67:65750d716788 166 double pwm_to_motor_turn; double pwm_to_motor_strike; // TURN - STRIKE : variable that is constructed by summing the values of the P, I and D action
ThomasBNL 46:9279c7a725bf 167
ThomasBNL 46:9279c7a725bf 168
ThomasBNL 66:04a203e43510 169 const double cw=0; // MOTOR: turn direction zero is clockwise (front view)
ThomasBNL 66:04a203e43510 170 const double ccw=1; // MOTOR: turn direction one is counterclockwise (front view)
ThomasBNL 43:fb69ef657f30 171 const double off=1; // Led off
ThomasBNL 43:fb69ef657f30 172 const double on=0; // Led on
ThomasBNL 43:fb69ef657f30 173 const int Fs = 512; // sampling frequency (512 Hz)
ThomasBNL 43:fb69ef657f30 174 const double sample_time = 0.001953125; // duration of one sample
ThomasBNL 44:5dd0a3d24662 175 double conversion_counts_to_degrees=0.085877862594198; // Calculation conversion counts to degrees
ThomasBNL 108:873e56f92691 176 // gear ratio motor = 131 ;;; ticks per magnet rotation = 32 (X2 Encoder) ;;; One revolution = 360 degrees
ThomasBNL 44:5dd0a3d24662 177 // degrees_per_encoder_tick = 360/(gear_ratio*ticks_per_magnet_rotation)=360/131*32=0.085877862594198
ThomasBNL 78:bcf0b5627b47 178 double smp, f, pwm_strike;
ThomasBNL 46:9279c7a725bf 179 //============================================================================================================================
ThomasBNL 34:c672f5c0763f 180 // ___________________________
ThomasBNL 34:c672f5c0763f 181 // // \\
ThomasBNL 34:c672f5c0763f 182 // || [FUNCTIONS USED] ||
ThomasBNL 34:c672f5c0763f 183 // \\___________________________//
ThomasBNL 67:65750d716788 184 void execute_plant_turn (); // TURN : Check error -> execute PID controller -> write pwm and direction to motor
ThomasBNL 66:04a203e43510 185 void execute_plant_strike ();
ThomasBNL 54:9eb449571f4f 186 double PID_control (double reference, double position, double &integrate_error,
ThomasBNL 54:9eb449571f4f 187 double sample_time, double &previous_error,
ThomasBNL 54:9eb449571f4f 188 double P_gain, double I_gain, double D_gain);
ThomasBNL 66:04a203e43510 189
ThomasBNL 46:9279c7a725bf 190 void keep_in_range (double * in, double min, double max); // Put in a value and makes sure that the value stays between assigned boundary's
ThomasBNL 46:9279c7a725bf 191 void setlooptimerflag (void); // Sets looptimerflag volatile bool to true
ThomasBNL 8:50d6e2323d3b 192
ThomasBNL 46:9279c7a725bf 193 void deactivate_PID_Controller (double& P_gain, double& I_gain, double& D_gain); // STRIKE/TURN: Deactivate PID Controller
ThomasBNL 46:9279c7a725bf 194
ThomasBNL 46:9279c7a725bf 195 void activate_PID_Controller_strike (double& P_gain, double& I_gain, double& D_gain); // STRIKE: Activate PID Controller
ThomasBNL 67:65750d716788 196 void activate_PID_Controller_turn (double& P_gain, double& I_gain, double& D_gain); // TURN : Activate PID Controller
ThomasBNL 46:9279c7a725bf 197
ThomasBNL 67:65750d716788 198 void Change_Turn_Position_Left (double& reference, double change_one_bottle); // TURN : Change Reference position one bottle to the left
ThomasBNL 67:65750d716788 199 void Change_Turn_Position_Right (double& reference, double change_one_bottle); // TURN : Change Reference position one bottle to the right
ThomasBNL 39:104a038f7b92 200
ThomasBNL 67:65750d716788 201 void countdown_from_5(); // PUTTY : 5 second countdown inside
ThomasBNL 67:65750d716788 202 void calibrate_motor(); // MOTOR : Calibrate starting position motor
ThomasBNL 67:65750d716788 203 void calibration(); // EMG : Calibrate the EMG signal (calculate min and max signal and determine threshold values)
ThomasBNL 67:65750d716788 204 void Filter(); // EMG : Filter the incoming EMG signals
ThomasBNL 67:65750d716788 205 void sample_filter(); // EMG : Calculate moving average (10 samples, one sample per 25 samples) using sample_filter => moving average over +-0.5 seconds
ThomasBNL 67:65750d716788 206 void take_sample(); // EMG : Take a sample once every 25 samples that's used to calculate the moving average
ThomasBNL 106:8bc89c842610 207
ThomasBNL 66:04a203e43510 208 void red();void blue();void green();void white();void yellow();void cyan();void purple(); void black(); // DEBUG: Different color LEDS
ThomasBNL 39:104a038f7b92 209
ThomasBNL 68:b262b349c902 210 void calibrate_potmeter(); // DEBUG/TEST : Calibration thresholds with potmeter
ThomasBNL 105:ee681c33a900 211 void calibrate_motor_EMG();
ThomasBNL 68:b262b349c902 212
ThomasBNL 68:b262b349c902 213 void Action_Controller(); // CONTROLLER : Decides and executes if the robot needs to turn to the left, right or strike based on the currently measured EMG signal
ThomasBNL 78:bcf0b5627b47 214
ThomasBNL 105:ee681c33a900 215 void led(double led1, double led2, double led3, double led4, double led5, double led6); // LED interface function (insert 1 if light on and 0 if light of) if (0,0,0,0,0,0) all leds are turned off
ThomasBNL 105:ee681c33a900 216 void leds_down_up(), led_up_down_up(), leds_down_up_min(), leds_down_up_max(); // LED functions
ThomasBNL 46:9279c7a725bf 217 //============================================================================================================================
ThomasBNL 46:9279c7a725bf 218 ///////////////////////////////
ThomasBNL 46:9279c7a725bf 219 // //
ThomasBNL 46:9279c7a725bf 220 ///////////////////////////////////////////// [MAIN FUNCTION] /////////////////////////////////////////////////////
ThomasBNL 46:9279c7a725bf 221 // //
ThomasBNL 46:9279c7a725bf 222 ///////////////////////////////
ThomasBNL 46:9279c7a725bf 223 //============================================================================================================================
ThomasBNL 0:40052f5ca77b 224 int main() {
ThomasBNL 43:fb69ef657f30 225
ThomasBNL 67:65750d716788 226 black(); // No LED active
ThomasBNL 43:fb69ef657f30 227 pc.printf("Start of code \n\r");
ThomasBNL 91:9ffdbbc6cce5 228 led(1,1,1,1,1,1);
ThomasBNL 91:9ffdbbc6cce5 229 wait(2);
ThomasBNL 43:fb69ef657f30 230
ThomasBNL 67:65750d716788 231 pc.baud(115200); // PC contactspeed : Set the baudrate
ThomasBNL 90:2dc2a931e49f 232
ThomasBNL 90:2dc2a931e49f 233 looptimer.attach(setlooptimerflag,(float)1/Fs); // CONTROLLER FLOW : Calls the looptimer flag every sample
ThomasBNL 100:e930203df49a 234
ThomasBNL 78:bcf0b5627b47 235 blue();
ThomasBNL 0:40052f5ca77b 236
ThomasBNL 105:ee681c33a900 237 calibration(); // EMG CALIBRATE : calibrate the EMG minimum and maximum values
ThomasBNL 105:ee681c33a900 238
ThomasBNL 100:e930203df49a 239 red();
ThomasBNL 100:e930203df49a 240 calibrate_motor(); // MOTOR CALIBRATE : The motor starting position (RED LED)
ThomasBNL 100:e930203df49a 241
ThomasBNL 105:ee681c33a900 242 //calibrate_motor_EMG(); // MOTOR CALIBRATE : The motor starting position using EMG
ThomasBNL 105:ee681c33a900 243 //calibrate_potmeter(); // DEBUG/TEST : Calibration thresholds with potmeter
ThomasBNL 67:65750d716788 244
ThomasBNL 105:ee681c33a900 245 led(0,0,0,0,0,0);
ThomasBNL 67:65750d716788 246 wait (3); // REST before starting position
ThomasBNL 46:9279c7a725bf 247
ThomasBNL 67:65750d716788 248 green(); // START CONTROLLOOP (GREEN LED)
ThomasBNL 70:7e9048f6d0fe 249
ThomasBNL 68:b262b349c902 250 Action_Controller(); // CONTROLLER : Decides and executes if the robot needs to turn to the left, right or strike based on the currently measured EMG signal
ThomasBNL 68:b262b349c902 251 }
ThomasBNL 68:b262b349c902 252
ThomasBNL 68:b262b349c902 253 //============================================================================================================================
ThomasBNL 68:b262b349c902 254 ///////////////////////////////
ThomasBNL 68:b262b349c902 255 // //
ThomasBNL 68:b262b349c902 256 ///////////////////////////////////////////// [FUNCTIONS DESCRIBED] /////////////////////////////////////////////////////
ThomasBNL 68:b262b349c902 257 // //
ThomasBNL 68:b262b349c902 258 ///////////////////////////////
ThomasBNL 68:b262b349c902 259 //============================================================================================================================
ThomasBNL 68:b262b349c902 260
ThomasBNL 107:16667cf7048c 261 // ___________________________
ThomasBNL 107:16667cf7048c 262 // // \\
ThomasBNL 107:16667cf7048c 263 // || [TIME AND VALUE CONTROL] ||
ThomasBNL 107:16667cf7048c 264 // \\___________________________//
ThomasBNL 107:16667cf7048c 265 //
ThomasBNL 107:16667cf7048c 266 void setlooptimerflag(void) // Looptimerflag function
ThomasBNL 107:16667cf7048c 267 { looptimerflag = true; }
ThomasBNL 107:16667cf7048c 268
ThomasBNL 107:16667cf7048c 269 void keep_in_range(double * in, double min, double max) // Put in certain min and max values that the input needs to stay within
ThomasBNL 107:16667cf7048c 270 {
ThomasBNL 107:16667cf7048c 271 *in > min ? *in < max? : *in = max: *in = min;
ThomasBNL 107:16667cf7048c 272 }
ThomasBNL 68:b262b349c902 273
ThomasBNL 69:22bf29479473 274 // FUNCTION 1 ___________________________
ThomasBNL 69:22bf29479473 275 // // \\
ThomasBNL 69:22bf29479473 276 // || [ACTIONCONTROLLER] ||
ThomasBNL 69:22bf29479473 277 // \\___________________________//
ThomasBNL 107:16667cf7048c 278 void Action_Controller()
ThomasBNL 70:7e9048f6d0fe 279 {
ThomasBNL 107:16667cf7048c 280
ThomasBNL 107:16667cf7048c 281 while (1)
ThomasBNL 94:3316f7fa3f50 282 { // Start while loop
ThomasBNL 106:8bc89c842610 283 HA=0;
ThomasBNL 94:3316f7fa3f50 284
ThomasBNL 106:8bc89c842610 285 while(looptimerflag != true);
ThomasBNL 106:8bc89c842610 286 looptimerflag = false;
ThomasBNL 106:8bc89c842610 287
ThomasBNL 106:8bc89c842610 288 New_action: // Return here if action left, right or strike is executed
ThomasBNL 106:8bc89c842610 289
ThomasBNL 106:8bc89c842610 290 led(0,0,1,1,0,0); // Ready for new action (standby)
ThomasBNL 106:8bc89c842610 291
ThomasBNL 106:8bc89c842610 292 while(HA<175) // Measure EMG signal for 175 samples
ThomasBNL 106:8bc89c842610 293 {
ThomasBNL 106:8bc89c842610 294 if(looptimerflag==true)
ThomasBNL 106:8bc89c842610 295 {
ThomasBNL 106:8bc89c842610 296 looptimerflag=false;
ThomasBNL 106:8bc89c842610 297 sample_filter();
ThomasBNL 106:8bc89c842610 298 HA++;
ThomasBNL 106:8bc89c842610 299 }
ThomasBNL 106:8bc89c842610 300 }
ThomasBNL 106:8bc89c842610 301
ThomasBNL 106:8bc89c842610 302 HA=0;
ThomasBNL 90:2dc2a931e49f 303
ThomasBNL 46:9279c7a725bf 304 // ___________________________
ThomasBNL 106:8bc89c842610 305 // : [Action 1: Turn Strike] :
ThomasBNL 70:7e9048f6d0fe 306 // :___________________________:
ThomasBNL 106:8bc89c842610 307 //
ThomasBNL 106:8bc89c842610 308
ThomasBNL 107:16667cf7048c 309 if(moving_average_left > Threshold_Bicep_Left_1 && moving_average_right > Threshold_Bicep_Right_1) // Check if statement, if so then continue and start Strike action otherwise skip to next action
ThomasBNL 107:16667cf7048c 310 { // Statement 1 start
ThomasBNL 107:16667cf7048c 311 blue(); n=0; k=0; p=0;
ThomasBNL 107:16667cf7048c 312
ThomasBNL 107:16667cf7048c 313 while(1)
ThomasBNL 107:16667cf7048c 314 { // inf while loop strike until finished start
ThomasBNL 106:8bc89c842610 315 if (n==0) { // Change the reference point of the PID Strike controller
ThomasBNL 107:16667cf7048c 316 reference_strike=0; // Execute once (n is set to one and only gets executed if n equals zero)
ThomasBNL 107:16667cf7048c 317 n=1;
ThomasBNL 107:16667cf7048c 318 error_count=0;
ThomasBNL 107:16667cf7048c 319 }
ThomasBNL 106:8bc89c842610 320
ThomasBNL 107:16667cf7048c 321 if (looptimerflag == true) // Loop that executes the strike controller every sample (inside the controller the loudness is regulated)
ThomasBNL 107:16667cf7048c 322 {
ThomasBNL 106:8bc89c842610 323 looptimerflag=false;
ThomasBNL 106:8bc89c842610 324 activate_PID_Controller_strike(P_gain_strike, I_gain_strike, D_gain_strike);
ThomasBNL 106:8bc89c842610 325 execute_plant_strike();
ThomasBNL 107:16667cf7048c 326 }
ThomasBNL 107:16667cf7048c 327
ThomasBNL 107:16667cf7048c 328 if (fabs(position_strike)<2) // If the bottle is hit (hit if the position is greater than the set hit point) then initiate return
ThomasBNL 107:16667cf7048c 329 { // Statement Return start
ThomasBNL 107:16667cf7048c 330 while(1)
ThomasBNL 107:16667cf7048c 331 { // inf while loop return after strike start
ThomasBNL 107:16667cf7048c 332 yellow();
ThomasBNL 107:16667cf7048c 333 if(k==0) { // Change reference point of the PID Strike controller back to the original position
ThomasBNL 107:16667cf7048c 334 p=1;
ThomasBNL 107:16667cf7048c 335 reference_strike=Hit;
ThomasBNL 107:16667cf7048c 336 error_count=0;
ThomasBNL 107:16667cf7048c 337 k=1;
ThomasBNL 107:16667cf7048c 338 smp=0;
ThomasBNL 107:16667cf7048c 339 pc.printf("return \n\r");
ThomasBNL 107:16667cf7048c 340 }
ThomasBNL 106:8bc89c842610 341
ThomasBNL 107:16667cf7048c 342 if (looptimerflag == true) { // Loop that executes the strike controller every sample (loudness is deactivated by the value of p)
ThomasBNL 107:16667cf7048c 343 looptimerflag=false;
ThomasBNL 107:16667cf7048c 344 activate_PID_Controller_strike(P_gain_strike, I_gain_strike, D_gain_strike);
ThomasBNL 107:16667cf7048c 345 execute_plant_strike();
ThomasBNL 107:16667cf7048c 346 }
ThomasBNL 107:16667cf7048c 347
ThomasBNL 107:16667cf7048c 348 if (fabs(error_strike_average) < 2 && error_count>100) { // If error is small enough and at least 100 samples have passed since the return execute new action
ThomasBNL 107:16667cf7048c 349 yellow();
ThomasBNL 107:16667cf7048c 350 pc.printf("new action \n\r");
ThomasBNL 107:16667cf7048c 351 deactivate_PID_Controller(P_gain_strike, I_gain_strike, D_gain_strike);
ThomasBNL 107:16667cf7048c 352 execute_plant_strike();
ThomasBNL 107:16667cf7048c 353 Sample_EMG_L_10=0; Sample_EMG_L_9=0; Sample_EMG_L_8=0; Sample_EMG_L_7=0; Sample_EMG_L_6=0; Sample_EMG_L_5=0; Sample_EMG_L_4=0; Sample_EMG_L_3=0; Sample_EMG_L_2=0; Sample_EMG_L_1=0;
ThomasBNL 107:16667cf7048c 354 Sample_EMG_R_10=0; Sample_EMG_R_9=0; Sample_EMG_R_8=0; Sample_EMG_R_7=0; Sample_EMG_R_6=0; Sample_EMG_R_5=0; Sample_EMG_R_4=0; Sample_EMG_R_3=0; Sample_EMG_R_2=0; Sample_EMG_R_1=0;
ThomasBNL 107:16667cf7048c 355 goto New_action;
ThomasBNL 107:16667cf7048c 356 }
ThomasBNL 106:8bc89c842610 357 } // inf while loop return after strike end
ThomasBNL 106:8bc89c842610 358 } // Statement Return end
ThomasBNL 106:8bc89c842610 359 } // inf while loop strike until finished end
ThomasBNL 106:8bc89c842610 360 } // Statement 2 end
ThomasBNL 58:141787606c4a 361 // ___________________________
ThomasBNL 90:2dc2a931e49f 362 // : [Action 2: Turn Left] :
ThomasBNL 70:7e9048f6d0fe 363 // :___________________________:
ThomasBNL 70:7e9048f6d0fe 364 //
ThomasBNL 58:141787606c4a 365
ThomasBNL 95:25b36fe02ee5 366 if(moving_average_left > Threshold_Bicep_Left_2 && moving_average_right < Threshold_Bicep_Right_1)
ThomasBNL 86:4351907387ea 367 {
ThomasBNL 106:8bc89c842610 368 yellow(); n=0;
ThomasBNL 95:25b36fe02ee5 369 led(1,1,0,0,0,0); // Green
ThomasBNL 86:4351907387ea 370 while(1)
ThomasBNL 84:9ea93eb9c2ec 371 {
ThomasBNL 84:9ea93eb9c2ec 372 if (n==0)
ThomasBNL 84:9ea93eb9c2ec 373 {
ThomasBNL 84:9ea93eb9c2ec 374 Change_Turn_Position_Left(reference_turn, change_one_bottle);
ThomasBNL 84:9ea93eb9c2ec 375 n=1; error_count=0;
ThomasBNL 84:9ea93eb9c2ec 376 }
ThomasBNL 104:0788e626a769 377
ThomasBNL 84:9ea93eb9c2ec 378 if (looptimerflag == true)
ThomasBNL 84:9ea93eb9c2ec 379 {
ThomasBNL 84:9ea93eb9c2ec 380 looptimerflag=false;
ThomasBNL 84:9ea93eb9c2ec 381 activate_PID_Controller_turn(P_gain_turn, I_gain_turn, D_gain_turn);
ThomasBNL 84:9ea93eb9c2ec 382 execute_plant_turn();
ThomasBNL 84:9ea93eb9c2ec 383 }
ThomasBNL 84:9ea93eb9c2ec 384
ThomasBNL 84:9ea93eb9c2ec 385 if (fabs(error_turn_average) < 1 && error_count>250)
ThomasBNL 84:9ea93eb9c2ec 386 {
ThomasBNL 84:9ea93eb9c2ec 387 pc.printf("new action \n\r");
ThomasBNL 84:9ea93eb9c2ec 388 deactivate_PID_Controller(P_gain_turn, I_gain_turn, D_gain_turn);
ThomasBNL 84:9ea93eb9c2ec 389 execute_plant_turn();
ThomasBNL 102:9153868ce3d3 390 Sample_EMG_L_10=0; Sample_EMG_L_9=0; Sample_EMG_L_8=0; Sample_EMG_L_7=0; Sample_EMG_L_6=0; Sample_EMG_L_5=0; Sample_EMG_L_4=0; Sample_EMG_L_3=0; Sample_EMG_L_2=0; Sample_EMG_L_1=0;
ThomasBNL 104:0788e626a769 391 Sample_EMG_R_10=0; Sample_EMG_R_9=0; Sample_EMG_R_8=0; Sample_EMG_R_7=0; Sample_EMG_R_6=0; Sample_EMG_R_5=0; Sample_EMG_R_4=0; Sample_EMG_R_3=0; Sample_EMG_R_2=0; Sample_EMG_R_1=0;
ThomasBNL 106:8bc89c842610 392 goto New_action;
ThomasBNL 84:9ea93eb9c2ec 393 }
ThomasBNL 84:9ea93eb9c2ec 394 }
ThomasBNL 84:9ea93eb9c2ec 395 }
ThomasBNL 70:7e9048f6d0fe 396 // ___________________________
ThomasBNL 70:7e9048f6d0fe 397 // : [Action 3: Turn Right] :
ThomasBNL 70:7e9048f6d0fe 398 // :___________________________:
ThomasBNL 70:7e9048f6d0fe 399 //
ThomasBNL 64:97e2db3eb0eb 400 // //Purple\\
ThomasBNL 94:3316f7fa3f50 401
ThomasBNL 70:7e9048f6d0fe 402
ThomasBNL 95:25b36fe02ee5 403 if(moving_average_right > Threshold_Bicep_Right_2 && moving_average_left < Threshold_Bicep_Left_1)
ThomasBNL 59:38a302b9f7f9 404 {
ThomasBNL 106:8bc89c842610 405 purple(); n=0;
ThomasBNL 106:8bc89c842610 406 // pc.printf("Right \n\r");
ThomasBNL 95:25b36fe02ee5 407 led(0,0,0,0,1,1); // Red right
ThomasBNL 86:4351907387ea 408 while(1)
ThomasBNL 86:4351907387ea 409 {
ThomasBNL 70:7e9048f6d0fe 410 if (n==0)
ThomasBNL 70:7e9048f6d0fe 411 {
ThomasBNL 70:7e9048f6d0fe 412 Change_Turn_Position_Right(reference_turn, change_one_bottle);
ThomasBNL 70:7e9048f6d0fe 413 n=1; error_count=0;
ThomasBNL 70:7e9048f6d0fe 414 }
ThomasBNL 104:0788e626a769 415
ThomasBNL 70:7e9048f6d0fe 416 if (looptimerflag == true)
ThomasBNL 70:7e9048f6d0fe 417 {
ThomasBNL 70:7e9048f6d0fe 418 looptimerflag=false;
ThomasBNL 70:7e9048f6d0fe 419 activate_PID_Controller_turn(P_gain_turn, I_gain_turn, D_gain_turn);
ThomasBNL 70:7e9048f6d0fe 420 execute_plant_turn();
ThomasBNL 70:7e9048f6d0fe 421 }
ThomasBNL 70:7e9048f6d0fe 422
ThomasBNL 78:bcf0b5627b47 423 if (fabs(error_turn_average) < tuning13 && error_count> tuning14)
ThomasBNL 70:7e9048f6d0fe 424 {
ThomasBNL 70:7e9048f6d0fe 425 pc.printf("new action \n\r");
ThomasBNL 70:7e9048f6d0fe 426 deactivate_PID_Controller(P_gain_turn, I_gain_turn, D_gain_turn);
ThomasBNL 70:7e9048f6d0fe 427 execute_plant_turn();
ThomasBNL 94:3316f7fa3f50 428 moving_average_right=0;
ThomasBNL 94:3316f7fa3f50 429 moving_average_left=0;
ThomasBNL 102:9153868ce3d3 430 Sample_EMG_L_10=0; Sample_EMG_L_9=0; Sample_EMG_L_8=0; Sample_EMG_L_7=0; Sample_EMG_L_6=0; Sample_EMG_L_5=0; Sample_EMG_L_4=0; Sample_EMG_L_3=0; Sample_EMG_L_2=0; Sample_EMG_L_1=0;
ThomasBNL 102:9153868ce3d3 431 Sample_EMG_R_10=0; Sample_EMG_R_9=0; Sample_EMG_R_8=0; Sample_EMG_R_7=0; Sample_EMG_R_6=0; Sample_EMG_R_5=0; Sample_EMG_R_4=0; Sample_EMG_R_3=0; Sample_EMG_R_2=0; Sample_EMG_R_1=0;
ThomasBNL 106:8bc89c842610 432 goto New_action;
ThomasBNL 70:7e9048f6d0fe 433 }
ThomasBNL 59:38a302b9f7f9 434 }
ThomasBNL 59:38a302b9f7f9 435 }
ThomasBNL 106:8bc89c842610 436 }
ThomasBNL 106:8bc89c842610 437 }
ThomasBNL 67:65750d716788 438
ThomasBNL 67:65750d716788 439 // ___________________________
ThomasBNL 67:65750d716788 440 // // \\
ThomasBNL 67:65750d716788 441 // || [CALIBRATE] ||
ThomasBNL 67:65750d716788 442 // \\___________________________//
ThomasBNL 67:65750d716788 443 // Calibrate starting postion (RED LED)
ThomasBNL 67:65750d716788 444 // ___________________________
ThomasBNL 67:65750d716788 445 // : [Starting position motor] :
ThomasBNL 67:65750d716788 446 // :___________________________:
ThomasBNL 67:65750d716788 447 //
ThomasBNL 67:65750d716788 448
ThomasBNL 106:8bc89c842610 449 void calibrate_motor()
ThomasBNL 106:8bc89c842610 450 {
ThomasBNL 67:65750d716788 451 pc.printf("Button calibration \n\r");
ThomasBNL 106:8bc89c842610 452 while(1) {
ThomasBNL 106:8bc89c842610 453 red();// RED LED
ThomasBNL 106:8bc89c842610 454
ThomasBNL 106:8bc89c842610 455 if (buttonL2.read() < 0.7) {
ThomasBNL 106:8bc89c842610 456 motordirection_turn = cw;
ThomasBNL 106:8bc89c842610 457 pwm_motor_turn = 0.1;
ThomasBNL 106:8bc89c842610 458 led(0,0,0,0,1,1);
ThomasBNL 106:8bc89c842610 459 }
ThomasBNL 106:8bc89c842610 460
ThomasBNL 106:8bc89c842610 461 if (buttonL1.read() < 0.7) {
ThomasBNL 106:8bc89c842610 462 motordirection_turn = ccw;
ThomasBNL 106:8bc89c842610 463 pwm_motor_turn = 0.1;
ThomasBNL 106:8bc89c842610 464 led(1,1,0,0,0,0);
ThomasBNL 106:8bc89c842610 465 }
ThomasBNL 106:8bc89c842610 466
ThomasBNL 106:8bc89c842610 467 led(0,0,0,0,0,0);
ThomasBNL 106:8bc89c842610 468
ThomasBNL 106:8bc89c842610 469 pwm_motor_turn = 0;
ThomasBNL 106:8bc89c842610 470
ThomasBNL 106:8bc89c842610 471 if (buttonH1.read() < 0.7) {
ThomasBNL 106:8bc89c842610 472 motordirection_strike = cw;
ThomasBNL 106:8bc89c842610 473 pwm_motor_strike = 0.02;
ThomasBNL 106:8bc89c842610 474 led(0,0,0,0,1,1);
ThomasBNL 106:8bc89c842610 475 }
ThomasBNL 106:8bc89c842610 476
ThomasBNL 106:8bc89c842610 477 if (buttonH2.read() < 0.7) {
ThomasBNL 106:8bc89c842610 478 motordirection_strike = ccw;
ThomasBNL 106:8bc89c842610 479 pwm_motor_strike = 0.02;
ThomasBNL 106:8bc89c842610 480 led(1,1,0,0,0,0);
ThomasBNL 106:8bc89c842610 481 }
ThomasBNL 106:8bc89c842610 482
ThomasBNL 106:8bc89c842610 483 pwm_motor_strike = 0;
ThomasBNL 106:8bc89c842610 484
ThomasBNL 106:8bc89c842610 485 if ((buttonL2.read() < 0.8) && (buttonL1.read() < 0.8)) { // Save current TURN and STRIKE positions as starting position
ThomasBNL 106:8bc89c842610 486 motor_turn.reset(); // TURN : Starting Position
ThomasBNL 106:8bc89c842610 487 reference_turn=0; // TURN : Set reference position to zero
ThomasBNL 106:8bc89c842610 488 motor_strike.reset(); // STRIKE : Starting Position
ThomasBNL 106:8bc89c842610 489 goto calibration_starting_position_complete3; // Calibration complete (exit while loop)
ThomasBNL 106:8bc89c842610 490 }
ThomasBNL 106:8bc89c842610 491 }
ThomasBNL 100:e930203df49a 492
ThomasBNL 101:b5e968a44040 493 calibration_starting_position_complete3:;
ThomasBNL 100:e930203df49a 494
ThomasBNL 106:8bc89c842610 495 while(1) {
ThomasBNL 106:8bc89c842610 496 // go to starting position
ThomasBNL 106:8bc89c842610 497 if(k==0) { // Change reference point of the PID Strike controller back to the original position
ThomasBNL 106:8bc89c842610 498 p=1;
ThomasBNL 106:8bc89c842610 499 reference_strike=Hit;
ThomasBNL 106:8bc89c842610 500 error_count=0;
ThomasBNL 106:8bc89c842610 501 k=1;
ThomasBNL 106:8bc89c842610 502 smp=0;
ThomasBNL 106:8bc89c842610 503 led(1,1,0,0,1,1);
ThomasBNL 67:65750d716788 504 }
ThomasBNL 106:8bc89c842610 505
ThomasBNL 106:8bc89c842610 506 if (looptimerflag == true) { // Loop that executes the strike controller every sample (loudness is deactivated by the value of p)
ThomasBNL 106:8bc89c842610 507 looptimerflag=false;
ThomasBNL 106:8bc89c842610 508 activate_PID_Controller_strike(P_gain_strike, I_gain_strike, D_gain_strike);
ThomasBNL 106:8bc89c842610 509 execute_plant_strike();
ThomasBNL 106:8bc89c842610 510 }
ThomasBNL 106:8bc89c842610 511
ThomasBNL 106:8bc89c842610 512 if (fabs(error_strike_average) < 2 && error_count>100) { // If error is small enough and at least 100 samples have passed since the return execute new action
ThomasBNL 106:8bc89c842610 513 // pc.printf("starting point calibration completed \n\r");
ThomasBNL 106:8bc89c842610 514 deactivate_PID_Controller(P_gain_strike, I_gain_strike, D_gain_strike);
ThomasBNL 106:8bc89c842610 515 execute_plant_strike();
ThomasBNL 106:8bc89c842610 516 goto calibration_position_strike_complete;
ThomasBNL 106:8bc89c842610 517 }
ThomasBNL 106:8bc89c842610 518 } // inf while loop return after strike end
ThomasBNL 106:8bc89c842610 519 calibration_position_strike_complete:
ThomasBNL 106:8bc89c842610 520 ;
ThomasBNL 106:8bc89c842610 521 }
ThomasBNL 67:65750d716788 522
ThomasBNL 46:9279c7a725bf 523 // ___________________________
ThomasBNL 46:9279c7a725bf 524 // // \\
ThomasBNL 70:7e9048f6d0fe 525 // || [EMG_Calibration] ||
ThomasBNL 46:9279c7a725bf 526 // \\___________________________//
ThomasBNL 49:a8a68abf814f 527 //
ThomasBNL 70:7e9048f6d0fe 528 void calibration()
ThomasBNL 87:507c0f6dffcb 529 {
ThomasBNL 70:7e9048f6d0fe 530
ThomasBNL 87:507c0f6dffcb 531 // [MINIMUM VALUE BICEPS CALIBRATION] //
ThomasBNL 106:8bc89c842610 532
ThomasBNL 106:8bc89c842610 533 countdown_from_5();
ThomasBNL 87:507c0f6dffcb 534 c=0;
ThomasBNL 70:7e9048f6d0fe 535
ThomasBNL 106:8bc89c842610 536 while(c<2560) // 512Hz -> 2560 is equal to five seconds
ThomasBNL 87:507c0f6dffcb 537 {
ThomasBNL 94:3316f7fa3f50 538 leds_down_up_min();
ThomasBNL 106:8bc89c842610 539 Filter(); // Filter EMG signal
ThomasBNL 90:2dc2a931e49f 540 minimum_L=fabs(EMG_Left_Bicep_filtered)+minimum_L; // Take previous sample EMG_Left_Bicep_filtered and add the new value
ThomasBNL 90:2dc2a931e49f 541 minimum_R=fabs(EMG_Right_Bicep_filtered)+minimum_R;
ThomasBNL 106:8bc89c842610 542 c++; // Every sample c is increased by one until the statement c<2560 is false
ThomasBNL 106:8bc89c842610 543 wait(0.001953125); // wait one sample
ThomasBNL 87:507c0f6dffcb 544 }
ThomasBNL 70:7e9048f6d0fe 545
ThomasBNL 106:8bc89c842610 546 // pc.printf("Finished minimum calibration \n\r");
ThomasBNL 70:7e9048f6d0fe 547
ThomasBNL 106:8bc89c842610 548 EMG_L_min=minimum_L/2560; // Divide the summation by the number of measurements (2560 measurements) to get a mean value over 5 seconds
ThomasBNL 87:507c0f6dffcb 549 EMG_R_min=minimum_R/2560;
ThomasBNL 70:7e9048f6d0fe 550
ThomasBNL 106:8bc89c842610 551 // pc.printf("EMG_L_min = %f \n\r EMG_R_min = %f \n\r", EMG_L_min, EMG_R_min);
ThomasBNL 70:7e9048f6d0fe 552
ThomasBNL 106:8bc89c842610 553 led(0,0,0,0,0,0);
ThomasBNL 106:8bc89c842610 554 wait (2); //cooldown
ThomasBNL 106:8bc89c842610 555
ThomasBNL 87:507c0f6dffcb 556 // [MAXIMUM VALUE BICEPS CALIBRATION] //
ThomasBNL 87:507c0f6dffcb 557
ThomasBNL 87:507c0f6dffcb 558 countdown_from_5();
ThomasBNL 87:507c0f6dffcb 559 c=0;
ThomasBNL 87:507c0f6dffcb 560
ThomasBNL 106:8bc89c842610 561 while(c<2560) // 512Hz -> 2560 is equal to five seconds
ThomasBNL 87:507c0f6dffcb 562 {
ThomasBNL 94:3316f7fa3f50 563 leds_down_up_max();
ThomasBNL 106:8bc89c842610 564 Filter(); // Filter EMG signal
ThomasBNL 106:8bc89c842610 565 maximum_L=fabs(EMG_Left_Bicep_filtered)+maximum_L; // Take previous sample EMG_Left_Bicep_filtered and add the new value
ThomasBNL 90:2dc2a931e49f 566 maximum_R=fabs(EMG_Right_Bicep_filtered)+maximum_R;
ThomasBNL 106:8bc89c842610 567 c++; // Every sample c is increased by one until the statement c<2560 is false
ThomasBNL 87:507c0f6dffcb 568 wait(0.001953125);
ThomasBNL 87:507c0f6dffcb 569 }
ThomasBNL 87:507c0f6dffcb 570
ThomasBNL 106:8bc89c842610 571 // pc.printf("Finished minimum calibration \n\r");
ThomasBNL 87:507c0f6dffcb 572
ThomasBNL 106:8bc89c842610 573 EMG_L_max=maximum_L/2560; // Divide the summation by the number of measurements (2560 measurements) to get a mean value over 5 seconds
ThomasBNL 87:507c0f6dffcb 574 EMG_R_max=maximum_R/2560;
ThomasBNL 87:507c0f6dffcb 575
ThomasBNL 106:8bc89c842610 576 // pc.printf("EMG_L_max = %f \n\r EMG_R_max = %f \n\r", EMG_L_max, EMG_R_max);
ThomasBNL 70:7e9048f6d0fe 577
ThomasBNL 106:8bc89c842610 578 led(0,0,0,0,0,0);
ThomasBNL 106:8bc89c842610 579 wait (2); //cooldown
ThomasBNL 87:507c0f6dffcb 580
ThomasBNL 106:8bc89c842610 581 // [ THRESHOLD CALCULATION ] //
ThomasBNL 87:507c0f6dffcb 582
ThomasBNL 106:8bc89c842610 583 Threshold_Bicep_Left_1=(fabs(EMG_L_max-EMG_L_min)*0.45)+fabs(EMG_L_min); // LEFT EMG: value at which the contraction of the muscle is at 45% compared to the maximum and minimum contraction of the muscle;
ThomasBNL 106:8bc89c842610 584 Threshold_Bicep_Left_2=(fabs(EMG_L_max-EMG_L_min)*0.9)+fabs(EMG_L_min); // LEFT EMG: Threshold at 90% of the maximum contraction
ThomasBNL 106:8bc89c842610 585 Threshold_Bicep_Right_1=(fabs(EMG_R_max-EMG_R_min)*0.45)+fabs(EMG_R_min); // RIGHT EMG: Threshold at 45% of the maximum contraction
ThomasBNL 106:8bc89c842610 586 Threshold_Bicep_Right_2=(fabs(EMG_R_max-EMG_R_min)*0.9)+fabs(EMG_R_min); // RIGHT EMG: Threshold at 90% of the maximum contraction
ThomasBNL 70:7e9048f6d0fe 587
ThomasBNL 106:8bc89c842610 588 // pc.printf("left 1: %f left 2: %f right 1: %f right 2: %f \n\r", Threshold_Bicep_Left_1, Threshold_Bicep_Left_2, Threshold_Bicep_Right_1, Threshold_Bicep_Right_2);
ThomasBNL 70:7e9048f6d0fe 589 }
ThomasBNL 70:7e9048f6d0fe 590
ThomasBNL 70:7e9048f6d0fe 591 // ___________________________
ThomasBNL 70:7e9048f6d0fe 592 // // \\
ThomasBNL 70:7e9048f6d0fe 593 // || [TURN PLANT] ||
ThomasBNL 70:7e9048f6d0fe 594 // \\___________________________//
ThomasBNL 70:7e9048f6d0fe 595
ThomasBNL 54:9eb449571f4f 596 void execute_plant_turn()
ThomasBNL 55:f4ab878ae910 597 {
ThomasBNL 70:7e9048f6d0fe 598 if ((motor_turn.getPulses()>4200) || (motor_turn.getPulses()<-4200)) // If value exceeds -4200 and 4200 (number of counts equal to one revolution) than reset to zero
ThomasBNL 70:7e9048f6d0fe 599 { motor_turn.reset(); }
ThomasBNL 70:7e9048f6d0fe 600
ThomasBNL 70:7e9048f6d0fe 601 position_turn = conversion_counts_to_degrees * motor_turn.getPulses(); // Convert counts to degrees
ThomasBNL 70:7e9048f6d0fe 602
ThomasBNL 70:7e9048f6d0fe 603 double pwm_to_motor_turn = PID_control(reference_turn, position_turn, integrate_error_turn, sample_time, previous_error_turn, P_gain_turn, I_gain_turn, D_gain_turn);
ThomasBNL 70:7e9048f6d0fe 604 // Execute PID controller and calculate the pwm to put to the motor
ThomasBNL 55:f4ab878ae910 605
ThomasBNL 70:7e9048f6d0fe 606 keep_in_range(&pwm_to_motor_turn, -1,1); // Pass to the plant but make sure the max and min pwm put to the plant stays between -1 and 1
ThomasBNL 70:7e9048f6d0fe 607
ThomasBNL 77:ac32d64602a5 608 pc.printf("pwm %f \n\r", pwm_to_motor_turn); // LINE FOR TESTING
ThomasBNL 70:7e9048f6d0fe 609
ThomasBNL 70:7e9048f6d0fe 610 if(pwm_to_motor_turn > 0) // Check error and decide the direction the motor has to turn
ThomasBNL 70:7e9048f6d0fe 611 { motordirection_turn=ccw;}
ThomasBNL 70:7e9048f6d0fe 612 else
ThomasBNL 70:7e9048f6d0fe 613 { motordirection_turn=cw; }
ThomasBNL 54:9eb449571f4f 614
ThomasBNL 104:0788e626a769 615 pwm_motor_turn=(abs(pwm_to_motor_turn)); // Put the absolute value of the PID controller to the pwm (negative pwm does not work)
ThomasBNL 104:0788e626a769 616
ThomasBNL 78:bcf0b5627b47 617 sample_filter(); // What is the current EMG value
ThomasBNL 104:0788e626a769 618
ThomasBNL 78:bcf0b5627b47 619
ThomasBNL 70:7e9048f6d0fe 620 if(sample_error) // sample_error -- e10;e9;e8;e7;e6;e5:e4;e3;e2;e1 -- error_turn_average --- er
ThomasBNL 70:7e9048f6d0fe 621 {
ThomasBNL 70:7e9048f6d0fe 622 sample_error=false;
ThomasBNL 77:ac32d64602a5 623 e1 = fabs(position_turn - reference_turn);
ThomasBNL 70:7e9048f6d0fe 624 e30=e29; e29=e28 ;e28=e27; e27=e26; e26=e25; e25=e24; e24=e23; e23=e22; e22=e21; e21=e20;
ThomasBNL 70:7e9048f6d0fe 625 e20=e19; e19=e18 ;e18=e17; e17=e16; e16=e15; e15=e14; e14=e13; e13=e12; e12=e11; e11=e10;
ThomasBNL 70:7e9048f6d0fe 626 e10=e9 ;e9=e8; e8=e7; e7=e6; e6=e5; e5=e4; e4=e3; e3=e2; e2=e1;
ThomasBNL 70:7e9048f6d0fe 627 }
ThomasBNL 70:7e9048f6d0fe 628
ThomasBNL 70:7e9048f6d0fe 629 error_turn_average=(e1+e2+e3+e4+e5+e6+e7+e8+e9+e10+e11+e12+e13+e14+e15+e16+e17+e18+e19+e20+e21+e22+e23+e24+e25+e26+e27+e28+e29+e30)/30;
ThomasBNL 70:7e9048f6d0fe 630 er++;
ThomasBNL 70:7e9048f6d0fe 631 error_count++;
ThomasBNL 54:9eb449571f4f 632 }
ThomasBNL 63:d86a46c8aa0c 633
ThomasBNL 70:7e9048f6d0fe 634 // ___________________________
ThomasBNL 70:7e9048f6d0fe 635 // // \\
ThomasBNL 70:7e9048f6d0fe 636 // || [STRIKE PLANT] ||
ThomasBNL 70:7e9048f6d0fe 637 // \\___________________________//
ThomasBNL 70:7e9048f6d0fe 638 //
ThomasBNL 63:d86a46c8aa0c 639 void execute_plant_strike()
ThomasBNL 106:8bc89c842610 640 {
ThomasBNL 106:8bc89c842610 641 if ((motor_strike.getPulses()>4200) || (motor_strike.getPulses()<-4200)) // If value is outside -4200 and 4200 (number of counts equal to one revolution) reset to zero
ThomasBNL 106:8bc89c842610 642 {
ThomasBNL 106:8bc89c842610 643 motor_strike.reset();
ThomasBNL 106:8bc89c842610 644 pc.printf("RESET \n\r");
ThomasBNL 106:8bc89c842610 645 }
ThomasBNL 106:8bc89c842610 646
ThomasBNL 106:8bc89c842610 647 position_strike = conversion_counts_to_degrees * motor_strike.getPulses();
ThomasBNL 63:d86a46c8aa0c 648
ThomasBNL 106:8bc89c842610 649 double pwm_to_motor_strike=PID_control(reference_strike, position_strike, integrate_error_strike, sample_time, previous_error_strike, P_gain_strike, I_gain_strike, D_gain_strike);
ThomasBNL 106:8bc89c842610 650 keep_in_range(&pwm_to_motor_strike, -1,1); // Pass to motor controller but keep it in range!
ThomasBNL 106:8bc89c842610 651
ThomasBNL 106:8bc89c842610 652 if(pwm_to_motor_strike > 0) // Check error and decide direction to turn
ThomasBNL 106:8bc89c842610 653 { motordirection_strike=cw; }
ThomasBNL 106:8bc89c842610 654 else
ThomasBNL 106:8bc89c842610 655 { motordirection_strike=ccw; }
ThomasBNL 106:8bc89c842610 656
ThomasBNL 106:8bc89c842610 657 if(p==1)
ThomasBNL 106:8bc89c842610 658 { pwm_motor_strike=fabs(pwm_to_motor_strike); } // Return strike mechanism to original position
ThomasBNL 106:8bc89c842610 659
ThomasBNL 106:8bc89c842610 660 if(p==0) // p is put to one if the strike has been executed and the stick needs to return to the original position
ThomasBNL 84:9ea93eb9c2ec 661 { // PWM voor slaan
ThomasBNL 78:bcf0b5627b47 662
ThomasBNL 103:80b7824787e8 663 while(smp<128)
ThomasBNL 91:9ffdbbc6cce5 664 {
ThomasBNL 95:25b36fe02ee5 665 if(looptimerflag == true)
ThomasBNL 91:9ffdbbc6cce5 666 {
ThomasBNL 95:25b36fe02ee5 667 looptimerflag=false;
ThomasBNL 98:42341afc398b 668 sample_filter();
ThomasBNL 91:9ffdbbc6cce5 669
ThomasBNL 106:8bc89c842610 670 double sum_muscle_force=(moving_average_left+moving_average_right)/(EMG_L_max+EMG_R_max);
ThomasBNL 106:8bc89c842610 671 keep_in_range(&sum_muscle_force, 0,1);
ThomasBNL 106:8bc89c842610 672
ThomasBNL 106:8bc89c842610 673 if (sum_muscle_force < tuning18) { led(0,0,0,0,0,0) ; pwm_strike=tuning24; }
ThomasBNL 106:8bc89c842610 674 if (sum_muscle_force > tuning18) { led(1,0,0,0,0,0) ; pwm_strike=tuning25; }
ThomasBNL 106:8bc89c842610 675 if (sum_muscle_force > tuning19) { led(1,1,0,0,0,0) ; pwm_strike=tuning26; }
ThomasBNL 106:8bc89c842610 676 if (sum_muscle_force > tuning20) { led(1,1,1,0,0,0) ; pwm_strike=tuning27; }
ThomasBNL 106:8bc89c842610 677 if (sum_muscle_force > tuning21) { led(1,1,1,1,0,0) ; pwm_strike=tuning28; }
ThomasBNL 106:8bc89c842610 678 if (sum_muscle_force > tuning22) { led(1,1,1,1,1,0) ; pwm_strike=tuning29; }
ThomasBNL 106:8bc89c842610 679 if (sum_muscle_force > tuning23) { led(1,1,1,1,1,1) ; pwm_strike=tuning30; }
ThomasBNL 104:0788e626a769 680
ThomasBNL 106:8bc89c842610 681 smp++;
ThomasBNL 84:9ea93eb9c2ec 682
ThomasBNL 106:8bc89c842610 683 if(smp==127)
ThomasBNL 106:8bc89c842610 684 { pwm_motor_strike=abs(pwm_strike); }
ThomasBNL 106:8bc89c842610 685 }
ThomasBNL 106:8bc89c842610 686 }
ThomasBNL 106:8bc89c842610 687 }
ThomasBNL 104:0788e626a769 688
ThomasBNL 106:8bc89c842610 689 sample_filter(); // Measure current EMG
ThomasBNL 84:9ea93eb9c2ec 690
ThomasBNL 84:9ea93eb9c2ec 691 if(sample_error_strike)
ThomasBNL 84:9ea93eb9c2ec 692 {
ThomasBNL 84:9ea93eb9c2ec 693 sample_error_strike=false;
ThomasBNL 84:9ea93eb9c2ec 694 e1 = fabs(position_strike - reference_strike);
ThomasBNL 70:7e9048f6d0fe 695 e30=e29; e29=e28 ;e28=e27; e27=e26; e26=e25; e25=e24; e24=e23; e23=e22; e22=e21; e21=e20;
ThomasBNL 70:7e9048f6d0fe 696 e20=e19; e19=e18 ;e18=e17; e17=e16; e16=e15; e15=e14; e14=e13; e13=e12; e12=e11; e11=e10;
ThomasBNL 70:7e9048f6d0fe 697 e10=e9 ;e9=e8; e8=e7; e7=e6; e6=e5; e5=e4; e4=e3; e3=e2; e2=e1;
ThomasBNL 84:9ea93eb9c2ec 698 }
ThomasBNL 84:9ea93eb9c2ec 699
ThomasBNL 64:97e2db3eb0eb 700 error_strike_average=(e1+e2+e3+e4+e5+e6+e7+e8+e9+e10+e11+e12+e13+e14+e15+e16+e17+e18+e19+e20+e21+e22+e23+e24+e25+e26+e27+e28+e29+e30)/30;
ThomasBNL 63:d86a46c8aa0c 701 er++;
ThomasBNL 63:d86a46c8aa0c 702 error_count++;
ThomasBNL 63:d86a46c8aa0c 703 }
ThomasBNL 54:9eb449571f4f 704
ThomasBNL 70:7e9048f6d0fe 705 // ___________________________
ThomasBNL 70:7e9048f6d0fe 706 // // \\
ThomasBNL 70:7e9048f6d0fe 707 // || [PID CONTROLLER] ||
ThomasBNL 70:7e9048f6d0fe 708 // \\___________________________//
ThomasBNL 70:7e9048f6d0fe 709 //
ThomasBNL 49:a8a68abf814f 710 double PID_control(double reference, double position, double &integrate_error, double sample_time, double &previous_error, double P_gain, double I_gain, double D_gain)
ThomasBNL 49:a8a68abf814f 711 {
ThomasBNL 70:7e9048f6d0fe 712 double error=(reference - position); // Current error (input controller)
ThomasBNL 70:7e9048f6d0fe 713 integrate_error=integrate_error_turn + sample_time*error_turn; // Integral error output
ThomasBNL 49:a8a68abf814f 714 // overwrite previous integrate error by adding the current error
ThomasBNL 49:a8a68abf814f 715 // multiplied by the sample time.
ThomasBNL 70:7e9048f6d0fe 716
ThomasBNL 70:7e9048f6d0fe 717 double error_derivative=(error - previous_error)/sample_time; // Derivative error output
ThomasBNL 70:7e9048f6d0fe 718 error_derivative=Lowpassfilter_derivative.step(error_derivative); // Filter
ThomasBNL 52:b530adf72f79 719
ThomasBNL 70:7e9048f6d0fe 720 previous_error_turn=error_turn; // current error is saved to memory constant to be used in
ThomasBNL 70:7e9048f6d0fe 721 // the next loop for calculating the derivative error
ThomasBNL 49:a8a68abf814f 722
ThomasBNL 70:7e9048f6d0fe 723 double pwm_motor_P = error*P_gain; // Output P controller to pwm
ThomasBNL 70:7e9048f6d0fe 724 double pwm_motor_I = integrate_error*I_gain; // Output I controller to pwm
ThomasBNL 70:7e9048f6d0fe 725 double pwm_motor_D = error_derivative*D_gain; // Output D controller to pwm
ThomasBNL 49:a8a68abf814f 726
ThomasBNL 50:6060f45d343a 727 double pwm_to_motor = pwm_motor_P + pwm_motor_I + pwm_motor_D;
ThomasBNL 49:a8a68abf814f 728
ThomasBNL 49:a8a68abf814f 729 return pwm_to_motor;
ThomasBNL 49:a8a68abf814f 730 }
ThomasBNL 34:c672f5c0763f 731
ThomasBNL 70:7e9048f6d0fe 732 // ___________________________
ThomasBNL 70:7e9048f6d0fe 733 // // \\
ThomasBNL 70:7e9048f6d0fe 734 // || [SAMPLE] ||
ThomasBNL 70:7e9048f6d0fe 735 // \\___________________________//
ThomasBNL 70:7e9048f6d0fe 736 //
ThomasBNL 46:9279c7a725bf 737 void sample_filter()
ThomasBNL 46:9279c7a725bf 738 {
ThomasBNL 46:9279c7a725bf 739 Filter();
ThomasBNL 46:9279c7a725bf 740 take_sample();
ThomasBNL 46:9279c7a725bf 741 if(sample)
ThomasBNL 46:9279c7a725bf 742 {
ThomasBNL 46:9279c7a725bf 743 sample=false;
ThomasBNL 106:8bc89c842610 744 Sample_EMG_L_1 = fabs(EMG_Left_Bicep_filtered); Sample_EMG_R_1 = fabs(EMG_Right_Bicep_filtered);
ThomasBNL 46:9279c7a725bf 745
ThomasBNL 46:9279c7a725bf 746 Sample_EMG_L_10= Sample_EMG_L_9; Sample_EMG_R_10= Sample_EMG_R_9;
ThomasBNL 46:9279c7a725bf 747 Sample_EMG_L_9 = Sample_EMG_L_8; Sample_EMG_R_9 = Sample_EMG_R_8;
ThomasBNL 46:9279c7a725bf 748 Sample_EMG_L_8 = Sample_EMG_L_7; Sample_EMG_R_8 = Sample_EMG_R_7;
ThomasBNL 46:9279c7a725bf 749 Sample_EMG_L_7 = Sample_EMG_L_6; Sample_EMG_R_7 = Sample_EMG_R_6;
ThomasBNL 46:9279c7a725bf 750 Sample_EMG_L_6 = Sample_EMG_L_5; Sample_EMG_R_6 = Sample_EMG_R_5;
ThomasBNL 46:9279c7a725bf 751 Sample_EMG_L_5 = Sample_EMG_L_4; Sample_EMG_R_5 = Sample_EMG_R_4;
ThomasBNL 46:9279c7a725bf 752 Sample_EMG_L_4 = Sample_EMG_L_3; Sample_EMG_R_4 = Sample_EMG_R_3;
ThomasBNL 46:9279c7a725bf 753 Sample_EMG_L_3 = Sample_EMG_L_2; Sample_EMG_R_3 = Sample_EMG_R_2;
ThomasBNL 46:9279c7a725bf 754 Sample_EMG_L_2 = Sample_EMG_L_1; Sample_EMG_R_2 = Sample_EMG_R_1;
ThomasBNL 46:9279c7a725bf 755 }
ThomasBNL 46:9279c7a725bf 756 moving_average_left=Sample_EMG_L_1*0.1+Sample_EMG_L_2*0.1+Sample_EMG_L_3*0.1+Sample_EMG_L_4*0.1+Sample_EMG_L_5*0.1+Sample_EMG_L_6*0.1+Sample_EMG_L_7*0.1+Sample_EMG_L_8*0.1+Sample_EMG_L_9*0.1+Sample_EMG_L_10*0.1;
ThomasBNL 46:9279c7a725bf 757 moving_average_right=Sample_EMG_R_1*0.1+Sample_EMG_R_2*0.1+Sample_EMG_R_3*0.1+Sample_EMG_R_4*0.1+Sample_EMG_R_5*0.1+Sample_EMG_R_6*0.1+Sample_EMG_R_7*0.1+Sample_EMG_R_8*0.1+Sample_EMG_R_9*0.1+Sample_EMG_R_10*0.1;
ThomasBNL 46:9279c7a725bf 758 n++;
ThomasBNL 46:9279c7a725bf 759 }
ThomasBNL 46:9279c7a725bf 760
ThomasBNL 70:7e9048f6d0fe 761 // ___________________________
ThomasBNL 70:7e9048f6d0fe 762 // // \\
ThomasBNL 70:7e9048f6d0fe 763 // || [ FILTER ] ||
ThomasBNL 70:7e9048f6d0fe 764 // \\___________________________//
ThomasBNL 70:7e9048f6d0fe 765 //
ThomasBNL 70:7e9048f6d0fe 766 void Filter() // Unfiltered EMG (input) -> highpass filter -> rectify -> lowpass filter -> Filtered EMG (output)
ThomasBNL 70:7e9048f6d0fe 767 {
ThomasBNL 106:8bc89c842610 768 EMG_left_Bicep = input1; EMG_Right_Bicep = input2; // Input EMG unfiltered
ThomasBNL 87:507c0f6dffcb 769
ThomasBNL 104:0788e626a769 770 EMG_Left_Bicep_filtered = highpassfilter_1.step(EMG_left_Bicep); EMG_Right_Bicep_filtered = highpassfilter_2.step(EMG_Right_Bicep); // Highpass filter
ThomasBNL 104:0788e626a769 771 EMG_Left_Bicep_filtered = fabs(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = fabs(EMG_Right_Bicep_filtered); // Rectify
ThomasBNL 87:507c0f6dffcb 772
ThomasBNL 104:0788e626a769 773 EMG_Left_Bicep_filtered = lowpassfilter_1.step(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = lowpassfilter_2.step(EMG_Right_Bicep_filtered); // Lowpass filter
ThomasBNL 87:507c0f6dffcb 774
ThomasBNL 70:7e9048f6d0fe 775 }
ThomasBNL 70:7e9048f6d0fe 776
ThomasBNL 70:7e9048f6d0fe 777 // ___________________________
ThomasBNL 70:7e9048f6d0fe 778 // // \\
ThomasBNL 70:7e9048f6d0fe 779 // || [TAKE SAMPLE] ||
ThomasBNL 70:7e9048f6d0fe 780 // \\___________________________//
ThomasBNL 70:7e9048f6d0fe 781 //
ThomasBNL 106:8bc89c842610 782 void take_sample() // Take a sample every 4th sample for moving average, every 5th sample for average error turn and every sample for average error strike
ThomasBNL 70:7e9048f6d0fe 783 {
ThomasBNL 102:9153868ce3d3 784 if(n==4)
ThomasBNL 70:7e9048f6d0fe 785 {sample = true; n=0;}
ThomasBNL 70:7e9048f6d0fe 786
ThomasBNL 70:7e9048f6d0fe 787 if(er==5)
ThomasBNL 70:7e9048f6d0fe 788 {sample_error = true; er=0;}
ThomasBNL 70:7e9048f6d0fe 789
ThomasBNL 70:7e9048f6d0fe 790 sample_error_strike = true;
ThomasBNL 70:7e9048f6d0fe 791 }
ThomasBNL 70:7e9048f6d0fe 792
ThomasBNL 70:7e9048f6d0fe 793 // ___________________________
ThomasBNL 70:7e9048f6d0fe 794 // // \\
ThomasBNL 70:7e9048f6d0fe 795 // || [CHANGE REFERENCE] ||
ThomasBNL 70:7e9048f6d0fe 796 // \\___________________________//
ThomasBNL 55:f4ab878ae910 797 //
ThomasBNL 70:7e9048f6d0fe 798 void Change_Turn_Position_Right (double& reference, double change_one_bottle)
ThomasBNL 70:7e9048f6d0fe 799 {
ThomasBNL 77:ac32d64602a5 800 if(reference==2*change_one_bottle) // If reference value at right boundary bottle and function is executed than immediatly turn 5 bottles to the left (ref to -90)
ThomasBNL 106:8bc89c842610 801 { reference=-2*change_one_bottle; }
ThomasBNL 70:7e9048f6d0fe 802 else
ThomasBNL 70:7e9048f6d0fe 803 { reference = reference + change_one_bottle;
ThomasBNL 77:ac32d64602a5 804 keep_in_range(&reference, -2*change_one_bottle, 2*change_one_bottle); } // reference position stays between -90 and 90 degrees (IF bottles at -90, -45, 0, 45, 90 degrees)
ThomasBNL 70:7e9048f6d0fe 805 }
ThomasBNL 70:7e9048f6d0fe 806
ThomasBNL 70:7e9048f6d0fe 807 void Change_Turn_Position_Left (double& reference, double change_one_bottle)
ThomasBNL 70:7e9048f6d0fe 808 {
ThomasBNL 77:ac32d64602a5 809 if(reference==-2*change_one_bottle) // If reference value at left boundary bottle and function is executed than immediatly turn 5 bottles to the left (ref to +90)
ThomasBNL 106:8bc89c842610 810 { reference=2*change_one_bottle; }
ThomasBNL 70:7e9048f6d0fe 811 else
ThomasBNL 70:7e9048f6d0fe 812 { reference = reference - change_one_bottle;
ThomasBNL 77:ac32d64602a5 813 keep_in_range(&reference, -2*change_one_bottle, 2*change_one_bottle); }
ThomasBNL 70:7e9048f6d0fe 814 }
ThomasBNL 70:7e9048f6d0fe 815
ThomasBNL 107:16667cf7048c 816 // ____________________________
ThomasBNL 107:16667cf7048c 817 // // \\
ThomasBNL 70:7e9048f6d0fe 818 // | [(DE)ACTIVATE PID CONTROLLERS] |
ThomasBNL 107:16667cf7048c 819 // \\____________________________//
ThomasBNL 70:7e9048f6d0fe 820 //
ThomasBNL 70:7e9048f6d0fe 821 void deactivate_PID_Controller(double& P_gain, double& I_gain, double& D_gain)
ThomasBNL 70:7e9048f6d0fe 822 {
ThomasBNL 70:7e9048f6d0fe 823 P_gain=0; I_gain=0; D_gain=0; // Deactivating values of PID controller
ThomasBNL 70:7e9048f6d0fe 824 pwm_motor_turn=0; pwm_motor_strike=0;
ThomasBNL 70:7e9048f6d0fe 825 }
ThomasBNL 70:7e9048f6d0fe 826
ThomasBNL 70:7e9048f6d0fe 827 void activate_PID_Controller_turn(double& P_gain, double& I_gain, double& D_gain)
ThomasBNL 70:7e9048f6d0fe 828 {
ThomasBNL 106:8bc89c842610 829 P_gain_turn=tuning4;
ThomasBNL 78:bcf0b5627b47 830 I_gain_turn=tuning5;
ThomasBNL 106:8bc89c842610 831 D_gain_turn=tuning6;
ThomasBNL 70:7e9048f6d0fe 832 }
ThomasBNL 70:7e9048f6d0fe 833
ThomasBNL 70:7e9048f6d0fe 834 void activate_PID_Controller_strike(double& P_gain, double& I_gain, double& D_gain)
ThomasBNL 70:7e9048f6d0fe 835 {
ThomasBNL 78:bcf0b5627b47 836 P_gain_strike=tuning1;
ThomasBNL 78:bcf0b5627b47 837 I_gain_strike=tuning2;
ThomasBNL 78:bcf0b5627b47 838 D_gain_strike=tuning3;
ThomasBNL 78:bcf0b5627b47 839 }
ThomasBNL 70:7e9048f6d0fe 840
ThomasBNL 107:16667cf7048c 841 // ____________________________
ThomasBNL 107:16667cf7048c 842 // // \\
ThomasBNL 107:16667cf7048c 843 // || [LED INTERFACE FUNCTIONS] ||
ThomasBNL 107:16667cf7048c 844 // \\____________________________//
ThomasBNL 70:7e9048f6d0fe 845 //
ThomasBNL 78:bcf0b5627b47 846 void led(double led1, double led2, double led3, double led4, double led5, double led6)
ThomasBNL 78:bcf0b5627b47 847 {ledgreen1 = led1; ledgreen2 = led2; ledyellow1 = led3; ledyellow2 = led4; ledred1 = led5; ledred2 = led6;}
ThomasBNL 78:bcf0b5627b47 848
ThomasBNL 94:3316f7fa3f50 849 void leds_down_up_min()
ThomasBNL 76:09ace7f0a0bf 850 {
ThomasBNL 94:3316f7fa3f50 851 if(a==15) {led(1,1,0,0,0,0);}
ThomasBNL 94:3316f7fa3f50 852 if(a==30) {led(1,1,1,0,0,0);}
ThomasBNL 94:3316f7fa3f50 853 if(a==45) {led(1,1,0,1,0,0);}
ThomasBNL 94:3316f7fa3f50 854 if(a==60) {led(1,1,0,0,1,0);}
ThomasBNL 94:3316f7fa3f50 855 if(a==75) {led(1,1,0,0,0,1); a=0;}
ThomasBNL 94:3316f7fa3f50 856 a++;
ThomasBNL 94:3316f7fa3f50 857 }
ThomasBNL 76:09ace7f0a0bf 858
ThomasBNL 94:3316f7fa3f50 859 void leds_down_up_max()
ThomasBNL 94:3316f7fa3f50 860 {
ThomasBNL 94:3316f7fa3f50 861 if(a==15) {led(0,0,0,0,1,1);}
ThomasBNL 94:3316f7fa3f50 862 if(a==30) {led(1,0,0,0,1,1);}
ThomasBNL 94:3316f7fa3f50 863 if(a==45) {led(0,1,0,0,1,1);}
ThomasBNL 94:3316f7fa3f50 864 if(a==60) {led(0,0,1,0,1,1);}
ThomasBNL 94:3316f7fa3f50 865 if(a==75) {led(0,0,0,1,1,1); a=0;}
ThomasBNL 76:09ace7f0a0bf 866 a++;
ThomasBNL 76:09ace7f0a0bf 867 }
ThomasBNL 76:09ace7f0a0bf 868
ThomasBNL 106:8bc89c842610 869 void countdown_from_5() // Countdown from 6 till 0 with LED(interface)
ThomasBNL 106:8bc89c842610 870 {
ThomasBNL 106:8bc89c842610 871 led(1,1,1,1,1,1);
ThomasBNL 106:8bc89c842610 872 wait(1);
ThomasBNL 106:8bc89c842610 873 led(1,1,1,1,1,0);
ThomasBNL 106:8bc89c842610 874 wait(1);
ThomasBNL 106:8bc89c842610 875 led(1,1,1,1,0,0);
ThomasBNL 106:8bc89c842610 876 wait(1);
ThomasBNL 106:8bc89c842610 877 led(1,1,1,0,0,0);
ThomasBNL 106:8bc89c842610 878 wait(1);
ThomasBNL 106:8bc89c842610 879 led(1,1,0,0,0,0);
ThomasBNL 106:8bc89c842610 880 wait(1);
ThomasBNL 106:8bc89c842610 881 led(1,0,0,0,0,0);
ThomasBNL 106:8bc89c842610 882 wait(1);
ThomasBNL 106:8bc89c842610 883 }
ThomasBNL 107:16667cf7048c 884 // _______________________________________
ThomasBNL 107:16667cf7048c 885 // // \\
ThomasBNL 107:16667cf7048c 886 // || [UNUSED/TESTING/CONCEPTUAL FUNCTIONS] ||
ThomasBNL 107:16667cf7048c 887 // \\_______________________________________//
ThomasBNL 107:16667cf7048c 888 //
ThomasBNL 106:8bc89c842610 889 // UNUSED/TESTING/CONCEPTUAL FUNCTIONS
ThomasBNL 106:8bc89c842610 890 void red() { debug_led_red=on; debug_led_blue=off; debug_led_green=off; }
ThomasBNL 106:8bc89c842610 891 void blue() { debug_led_red=off; debug_led_blue=on; debug_led_green=off; }
ThomasBNL 106:8bc89c842610 892 void green() { debug_led_red=off; debug_led_blue=off; debug_led_green=on; }
ThomasBNL 106:8bc89c842610 893 void white() { debug_led_red=on; debug_led_blue=on; debug_led_green=on; }
ThomasBNL 106:8bc89c842610 894 void yellow() { debug_led_red=on; debug_led_blue=off; debug_led_green=on; }
ThomasBNL 106:8bc89c842610 895 void cyan() { debug_led_red=off; debug_led_blue=on; debug_led_green=on; }
ThomasBNL 106:8bc89c842610 896 void purple() { debug_led_red=on; debug_led_blue=on; debug_led_green=off; }
ThomasBNL 106:8bc89c842610 897 void black() { debug_led_red=off; debug_led_blue=off; debug_led_green=off; }
ThomasBNL 106:8bc89c842610 898
ThomasBNL 106:8bc89c842610 899 void calibrate_potmeter() // DEBUG/TEST: Calibration thresholds with potmeter (testing tool without EMG)
ThomasBNL 106:8bc89c842610 900 {
ThomasBNL 106:8bc89c842610 901 EMG_L_max = 100;
ThomasBNL 106:8bc89c842610 902 EMG_L_min = 0;
ThomasBNL 106:8bc89c842610 903 EMG_R_max = 100;
ThomasBNL 106:8bc89c842610 904 EMG_R_min = 0;
ThomasBNL 106:8bc89c842610 905 Threshold_Bicep_Left_1 =((EMG_L_max-EMG_L_min)*0.2)+EMG_L_min;; //(waarde waarop het gemeten EMG signaal 20% van max het maximale is); // LEFT
ThomasBNL 106:8bc89c842610 906 Threshold_Bicep_Left_2 =((EMG_L_max-EMG_L_min)*0.6)+EMG_L_min; //(waarde waarop het gemeten EMG signaal 60% van max het maximale is);
ThomasBNL 106:8bc89c842610 907 Threshold_Bicep_Right_1 =((EMG_R_max-EMG_R_min)*0.2)+EMG_R_min; //(waarde waarop het gemeten EMG signaal 20% van max het maximale is); // RIGHT
ThomasBNL 106:8bc89c842610 908 Threshold_Bicep_Right_2 =((EMG_R_max-EMG_R_min)*0.6)+EMG_R_min; //(waarde waarop het gemeten EMG signaal 60% van max het maximale is);
ThomasBNL 106:8bc89c842610 909 }
ThomasBNL 76:09ace7f0a0bf 910
ThomasBNL 105:ee681c33a900 911 // void calibrate_motor_EMG()
ThomasBNL 105:ee681c33a900 912
ThomasBNL 105:ee681c33a900 913 // // CALIBRATE STRIKE MOTOR WITH EMG
ThomasBNL 105:ee681c33a900 914 //while(1)
ThomasBNL 105:ee681c33a900 915 //{
ThomasBNL 105:ee681c33a900 916 // led(0,0,0,0,0,0);
ThomasBNL 105:ee681c33a900 917 //
ThomasBNL 105:ee681c33a900 918 // if(looptimerflag==true)
ThomasBNL 105:ee681c33a900 919 // {
ThomasBNL 105:ee681c33a900 920 // sample_filter();
ThomasBNL 105:ee681c33a900 921 // pc.printf("%f %f \n\r", moving_average_left, moving_average_right);
ThomasBNL 105:ee681c33a900 922 // pc.printf("%f \n\r", pwm_motor_strike);
ThomasBNL 105:ee681c33a900 923 //
ThomasBNL 105:ee681c33a900 924 //while (moving_average_left > Threshold_Bicep_Left_2 && moving_average_right < Threshold_Bicep_Right_1) // Links
ThomasBNL 105:ee681c33a900 925 //{
ThomasBNL 105:ee681c33a900 926 //if(looptimerflag==true)
ThomasBNL 105:ee681c33a900 927 // {
ThomasBNL 105:ee681c33a900 928 // sample_filter();
ThomasBNL 105:ee681c33a900 929 //motordirection_strike = ccw;
ThomasBNL 105:ee681c33a900 930 // pwm_motor_strike = 0.08;
ThomasBNL 105:ee681c33a900 931 // led(1,1,0,0,0,0);
ThomasBNL 105:ee681c33a900 932 // }}
ThomasBNL 105:ee681c33a900 933 //
ThomasBNL 105:ee681c33a900 934 //while (moving_average_right > Threshold_Bicep_Right_2 && moving_average_left < Threshold_Bicep_Left_1) // Rechts
ThomasBNL 105:ee681c33a900 935 //{ if(looptimerflag==true)
ThomasBNL 105:ee681c33a900 936 // {
ThomasBNL 105:ee681c33a900 937 // sample_filter();
ThomasBNL 105:ee681c33a900 938 //motordirection_strike = cw;
ThomasBNL 105:ee681c33a900 939 // pwm_motor_strike = 0.08;
ThomasBNL 105:ee681c33a900 940 // led(0,0,0,0,1,1);} }
ThomasBNL 105:ee681c33a900 941 //
ThomasBNL 105:ee681c33a900 942 // pwm_motor_strike=0;
ThomasBNL 105:ee681c33a900 943 // led(0,0,0,0,0,0);
ThomasBNL 105:ee681c33a900 944 //
ThomasBNL 105:ee681c33a900 945 //if (moving_average_right > Threshold_Bicep_Right_2 && moving_average_left > Threshold_Bicep_Left_2) // Beide
ThomasBNL 105:ee681c33a900 946 //{ motor_strike.reset();
ThomasBNL 105:ee681c33a900 947 // reference_strike=0;
ThomasBNL 105:ee681c33a900 948 // led(1,1,1,1,1,1);
ThomasBNL 105:ee681c33a900 949 // wait(1);
ThomasBNL 105:ee681c33a900 950 // moving_average_right=0;
ThomasBNL 105:ee681c33a900 951 // moving_average_left=0;
ThomasBNL 105:ee681c33a900 952 // g=0;
ThomasBNL 105:ee681c33a900 953 // goto calibration_starting_position_complete;}
ThomasBNL 105:ee681c33a900 954 // }
ThomasBNL 105:ee681c33a900 955 // calibration_starting_position_complete:;
ThomasBNL 105:ee681c33a900 956 //
ThomasBNL 105:ee681c33a900 957 // while(g<512)
ThomasBNL 105:ee681c33a900 958 // {
ThomasBNL 105:ee681c33a900 959 // if(looptimerflag==true)
ThomasBNL 105:ee681c33a900 960 // {
ThomasBNL 105:ee681c33a900 961 // sample_filter();
ThomasBNL 105:ee681c33a900 962 // g++;
ThomasBNL 105:ee681c33a900 963 // }
ThomasBNL 105:ee681c33a900 964 // }
ThomasBNL 105:ee681c33a900 965 //
ThomasBNL 105:ee681c33a900 966 //// CALIBRATE TURN MOTOR WITH EMG
ThomasBNL 105:ee681c33a900 967 // while(1)
ThomasBNL 105:ee681c33a900 968 // {
ThomasBNL 105:ee681c33a900 969 // led(0,0,0,0,0,0);
ThomasBNL 105:ee681c33a900 970 //
ThomasBNL 105:ee681c33a900 971 // if(looptimerflag==true)
ThomasBNL 105:ee681c33a900 972 // {
ThomasBNL 105:ee681c33a900 973 // sample_filter();
ThomasBNL 105:ee681c33a900 974 // pc.printf("%f %f \n\r", moving_average_left, moving_average_right);
ThomasBNL 105:ee681c33a900 975 //
ThomasBNL 105:ee681c33a900 976 //while (moving_average_left > Threshold_Bicep_Left_2 && moving_average_right < Threshold_Bicep_Right_1) // Links
ThomasBNL 105:ee681c33a900 977 //{
ThomasBNL 105:ee681c33a900 978 //if(looptimerflag==true)
ThomasBNL 105:ee681c33a900 979 // {
ThomasBNL 105:ee681c33a900 980 // sample_filter();
ThomasBNL 105:ee681c33a900 981 //motordirection_turn = ccw;
ThomasBNL 105:ee681c33a900 982 // pwm_motor_turn = 0.02;
ThomasBNL 105:ee681c33a900 983 // led(1,1,0,0,0,0);}}
ThomasBNL 105:ee681c33a900 984 //
ThomasBNL 105:ee681c33a900 985 //while (moving_average_right > Threshold_Bicep_Right_2 && moving_average_left < Threshold_Bicep_Left_1) // Rechts
ThomasBNL 105:ee681c33a900 986 //{ if(looptimerflag==true)
ThomasBNL 105:ee681c33a900 987 // {
ThomasBNL 105:ee681c33a900 988 // sample_filter();
ThomasBNL 105:ee681c33a900 989 //motordirection_turn = cw;
ThomasBNL 105:ee681c33a900 990 // pwm_motor_turn = 0.02;
ThomasBNL 105:ee681c33a900 991 // led(0,0,0,0,1,1);} }
ThomasBNL 105:ee681c33a900 992 //
ThomasBNL 105:ee681c33a900 993 // pwm_motor_strike=0;
ThomasBNL 105:ee681c33a900 994 // led(0,0,0,0,0,0);
ThomasBNL 105:ee681c33a900 995 //
ThomasBNL 105:ee681c33a900 996 //if (moving_average_right > Threshold_Bicep_Right_2 && moving_average_left > Threshold_Bicep_Left_2) // Beide
ThomasBNL 105:ee681c33a900 997 //{ motor_strike.reset();
ThomasBNL 105:ee681c33a900 998 // reference_turn=0;
ThomasBNL 105:ee681c33a900 999 // led(1,1,1,1,1,1);
ThomasBNL 105:ee681c33a900 1000 // wait(1);
ThomasBNL 105:ee681c33a900 1001 // moving_average_right=0;
ThomasBNL 105:ee681c33a900 1002 // moving_average_left=0;
ThomasBNL 105:ee681c33a900 1003 // g=0;
ThomasBNL 105:ee681c33a900 1004 // goto calibration_starting_position_complete2;}
ThomasBNL 105:ee681c33a900 1005 // }
ThomasBNL 105:ee681c33a900 1006 // }
ThomasBNL 105:ee681c33a900 1007 // }
ThomasBNL 105:ee681c33a900 1008 // calibration_starting_position_complete2:;
ThomasBNL 105:ee681c33a900 1009 //
ThomasBNL 105:ee681c33a900 1010 // while(g<512)
ThomasBNL 105:ee681c33a900 1011 // {
ThomasBNL 105:ee681c33a900 1012 // if(looptimerflag==true)
ThomasBNL 105:ee681c33a900 1013 // {
ThomasBNL 105:ee681c33a900 1014 // sample_filter();
ThomasBNL 105:ee681c33a900 1015 // g++;
ThomasBNL 105:ee681c33a900 1016 // }
ThomasBNL 105:ee681c33a900 1017 // }
ThomasBNL 105:ee681c33a900 1018 //
ThomasBNL 105:ee681c33a900 1019 // wait(2);
ThomasBNL 105:ee681c33a900 1020 // led(0,0,0,0,0,0);
ThomasBNL 105:ee681c33a900 1021 // wait(1);
ThomasBNL 105:ee681c33a900 1022 // led(1,0,1,0,1,0);
ThomasBNL 105:ee681c33a900 1023 // wait(1);