Thomas Burgers / Mbed 2 deprecated ZZ-TheChenneRobot

Important changes to repositories hosted on mbed.com

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

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

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

Dependencies:   Encoder HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@66:04a203e43510, 2015-10-20 (annotated)

Committer:
ThomasBNL
Date:
Tue Oct 20 19:33:29 2015 +0000
Revision:
66:04a203e43510
Parent:
65:2da8cf778181
Child:
67:65750d716788
cleaned up intro script

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 47:c61873a0b646 40 // Groep 12: The Chenne Band //
ThomasBNL 47:c61873a0b646 41 // ___________________________
ThomasBNL 47:c61873a0b646 42 // // \\
ThomasBNL 47:c61873a0b646 43 // || [Table of content] ||
ThomasBNL 47:c61873a0b646 44 // \\___________________________//
ThomasBNL 47:c61873a0b646 45 //
ThomasBNL 47:c61873a0b646 46 // Introduction
ThomasBNL 47:c61873a0b646 47
ThomasBNL 47:c61873a0b646 48 // Libraries.................................................
ThomasBNL 47:c61873a0b646 49 // Flow and debugging tools .................................
ThomasBNL 47:c61873a0b646 50 // LEDS.................................................
ThomasBNL 47:c61873a0b646 51 // Buttons..............................................
ThomasBNL 47:c61873a0b646 52 // Potmeter.............................................
ThomasBNL 47:c61873a0b646 53 // Flow.................................................
ThomasBNL 47:c61873a0b646 54 // HIDscope.............................................
ThomasBNL 47:c61873a0b646 55 // EMG.......................................................
ThomasBNL 47:c61873a0b646 56 // EMG variables........................................
ThomasBNL 47:c61873a0b646 57 // EMG filter...........................................
ThomasBNL 47:c61873a0b646 58 // motors.....................................................
ThomasBNL 47:c61873a0b646 59 // motor turn...........................................
ThomasBNL 47:c61873a0b646 60 // motor strike.........................................
ThomasBNL 47:c61873a0b646 61 // d-action filter......................................
ThomasBNL 47:c61873a0b646 62 // system constants...........................................
ThomasBNL 47:c61873a0b646 63 // functions used.............................................
ThomasBNL 47:c61873a0b646 64 // motor function.......................................
ThomasBNL 47:c61873a0b646 65 // EMG functions........................................
ThomasBNL 47:c61873a0b646 66 // action controller....................................
ThomasBNL 47:c61873a0b646 67
ThomasBNL 47:c61873a0b646 68 // Main
ThomasBNL 47:c61873a0b646 69
ThomasBNL 47:c61873a0b646 70 // Start of code..............................................
ThomasBNL 48:950b1f34161b 71 // calibrate..................................................
ThomasBNL 48:950b1f34161b 72 // starting position motor.............................. (RED LED)
ThomasBNL 48:950b1f34161b 73 // EMG calibration...................................... (BLUE LED)
ThomasBNL 54:9eb449571f4f 74 // Attach Ticker.............................................. (BLUE LED)
ThomasBNL 47:c61873a0b646 75 // Start infinite loop........................................
ThomasBNL 54:9eb449571f4f 76 // Debug buttons........................................ (GREEN LED)
ThomasBNL 54:9eb449571f4f 77 // Wait for go signal................................... (GREEN LED)
ThomasBNL 54:9eb449571f4f 78 // Limit position value and convert to degrees..........
ThomasBNL 47:c61873a0b646 79 // Get current EMG values...............................
ThomasBNL 47:c61873a0b646 80 // Action controller....................................
ThomasBNL 47:c61873a0b646 81 // Strike.........................................
ThomasBNL 47:c61873a0b646 82 // Turn left......................................
ThomasBNL 47:c61873a0b646 83 // Turn right.....................................
ThomasBNL 47:c61873a0b646 84 // PID controller.......................................
ThomasBNL 47:c61873a0b646 85 // PID error -> pwm motor...............................
ThomasBNL 47:c61873a0b646 86 // pwm -> plant.........................................
ThomasBNL 47:c61873a0b646 87 // HIDscope.............................................
ThomasBNL 47:c61873a0b646 88 // Deactivate if reached position.......................
ThomasBNL 47:c61873a0b646 89
ThomasBNL 47:c61873a0b646 90 // Functions described
ThomasBNL 47:c61873a0b646 91
ThomasBNL 47:c61873a0b646 92 // Motor Functions............................................
ThomasBNL 47:c61873a0b646 93 // EMG functions..............................................
ThomasBNL 47:c61873a0b646 94 // Action controller functions................................
ThomasBNL 47:c61873a0b646 95
ThomasBNL 47:c61873a0b646 96 //
ThomasBNL 47:c61873a0b646 97 //
ThomasBNL 47:c61873a0b646 98 //
ThomasBNL 47:c61873a0b646 99 //
ThomasBNL 47:c61873a0b646 100 //
ThomasBNL 47:c61873a0b646 101
ThomasBNL 47:c61873a0b646 102
ThomasBNL 46:9279c7a725bf 103 //============================================================================================================================
ThomasBNL 43:fb69ef657f30 104 // ___________________________
ThomasBNL 43:fb69ef657f30 105 // // \\
ThomasBNL 43:fb69ef657f30 106 // || [Libraries] ||
ThomasBNL 43:fb69ef657f30 107 // \\___________________________//
ThomasBNL 43:fb69ef657f30 108 //
ThomasBNL 43:fb69ef657f30 109
ThomasBNL 0:40052f5ca77b 110 #include "mbed.h"
ThomasBNL 21:c75210216204 111 #include "HIDScope.h"
ThomasBNL 0:40052f5ca77b 112 #include "QEI.h"
ThomasBNL 0:40052f5ca77b 113 #include "MODSERIAL.h"
ThomasBNL 8:50d6e2323d3b 114 #include "biquadFilter.h"
ThomasBNL 0:40052f5ca77b 115 #include "encoder.h"
ThomasBNL 0:40052f5ca77b 116
ThomasBNL 46:9279c7a725bf 117 //============================================================================================================================
ThomasBNL 34:c672f5c0763f 118 // ___________________________
ThomasBNL 34:c672f5c0763f 119 // // \\
ThomasBNL 46:9279c7a725bf 120 // || [FLOW AND DEBUGGING TOOLS] ||
ThomasBNL 34:c672f5c0763f 121 // \\___________________________//
ThomasBNL 66:04a203e43510 122
ThomasBNL 66:04a203e43510 123 //HIDScope scope(1); // DEBUG: HIDSCOPE has the ability to display signals over time and can be used to monitor signals
ThomasBNL 46:9279c7a725bf 124
ThomasBNL 66:04a203e43510 125 MODSERIAL pc(USBTX,USBRX); // MODSERIAL: makes it possible to send messages to the computer (eg. inside Putty)
ThomasBNL 34:c672f5c0763f 126
ThomasBNL 66:04a203e43510 127 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 128 DigitalIn buttonL1(PTC6) , buttonL2(PTA4) , buttonH1(D2) , buttonH2(D1); // DEBUG/CALIBRATION: 4 buttons for calibration and debugging
ThomasBNL 66:04a203e43510 129 Ticker looptimer; // FLOW: Ticker called looptimer to set a looptimerflag that puts the volatile bool control_go to true every sample
ThomasBNL 46:9279c7a725bf 130
ThomasBNL 66:04a203e43510 131 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 66:04a203e43510 132 volatile bool looptimerflag; // CONTROLLER: boolean that controls the sample time of the whole controller
ThomasBNL 66:04a203e43510 133
ThomasBNL 66:04a203e43510 134 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 66:04a203e43510 135 e19, e18, e17, e16, e15, e14, e13, e12, e11, e10, e9,
ThomasBNL 66:04a203e43510 136 e8, e7, e6, e5, e4, e3, e2, e1, er, error_count, error_turn_average, error_strike_average;
ThomasBNL 46:9279c7a725bf 137
ThomasBNL 66:04a203e43510 138 AnalogIn input1(A0), input2(A1); // EMG: Two AnalogIn EMG inputs, input1 (Left bicep), input2 (Right bicep)
ThomasBNL 44:5dd0a3d24662 139
ThomasBNL 66:04a203e43510 140 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 141 Sample_EMG_L_7, Sample_EMG_L_8, Sample_EMG_L_9, Sample_EMG_L_10, moving_average_left;
ThomasBNL 66:04a203e43510 142 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 143 Sample_EMG_R_7, Sample_EMG_R_8, Sample_EMG_R_9, Sample_EMG_R_10, moving_average_right;
ThomasBNL 46:9279c7a725bf 144
ThomasBNL 66:04a203e43510 145 double minimum_L, maximum_L, EMG_L_min, EMG_L_max; // EMG CALIBRATION: variables that are used during the EMG calibration
ThomasBNL 66:04a203e43510 146 double minimum_R, maximum_R, EMG_R_min, EMG_R_max;
ThomasBNL 66:04a203e43510 147
ThomasBNL 66:04a203e43510 148 double EMG_left_Bicep, EMG_Left_Bicep_filtered;
ThomasBNL 66:04a203e43510 149 double EMG_Right_Bicep, EMG_Right_Bicep_filtered;
ThomasBNL 46:9279c7a725bf 150
ThomasBNL 66:04a203e43510 151 double n=0; double c=0; double k=0; double p=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 152
ThomasBNL 66:04a203e43510 153 // FILTERS EMG
ThomasBNL 66:04a203e43510 154 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 66:04a203e43510 155 low_a1 = -1.205716572226748 , low_a2 = 0.44143409003801976 ;
ThomasBNL 66:04a203e43510 156 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 66:04a203e43510 157 high_a1 = -1.1429772843080923 , high_a2 = 0.41279762014290533 ;
ThomasBNL 66:04a203e43510 158 const double high_b0_HP = 0.84855234544818812 , high_b1_HP = -1.6970469089637623 , high_b2_HP = 0.8485234544818812, // HIGHPASS : NOG OPZOEKEN!! : >25Hz? sample rate 512Hz
ThomasBNL 66:04a203e43510 159 high_a1_HP = -1.6564788473046559 , high_a2_HP = 0.7376149706228688 ;
ThomasBNL 66:04a203e43510 160 const double low_b0_LP = 0.0013067079602315681, low_b1_LP = 0.0026134159204631363, low_b2_LP = 0.0013067079602315681, // LOWPASS : NOG OPZOEKEN!! <5-10 Hz? sample rate 512Hz
ThomasBNL 66:04a203e43510 161 low_a1_LP = -1.9238184798980429 , low_a2_LP = 0.9290453117389691 ;
ThomasBNL 46:9279c7a725bf 162
ThomasBNL 66:04a203e43510 163 //Left bicep Filters
ThomasBNL 66:04a203e43510 164 biquadFilter highpassfilter_1(high_a1_HP, high_a2_HP, high_b0_HP, high_b1_HP, high_b2_HP); // EMG: moeten nog waardes voor gemaakt worden? (>25Hz doorlaten)
ThomasBNL 66:04a203e43510 165 biquadFilter notchL1(high_a1, high_a2, high_b0, high_b1, high_b2); // EMG: moeten nog waardes voor gemaakt worden (>52Hz doorlaten)
ThomasBNL 66:04a203e43510 166 biquadFilter notchL2(low_a1, low_a2, low_b0, low_b1, low_b2); // EMG: moeten nog waardes voor gemaakt worden (<48Hz doorlaten)
ThomasBNL 66:04a203e43510 167 biquadFilter lowpassfilter_1(low_a1_LP, low_a2_LP, low_b0_LP, low_b1_LP, low_b2_LP);
ThomasBNL 46:9279c7a725bf 168
ThomasBNL 66:04a203e43510 169 // Right bicep Filters
ThomasBNL 66:04a203e43510 170 biquadFilter highpassfilter_2(high_a1_HP, high_a2_HP, high_b0_HP, high_b1_HP, high_b2_HP); // EMG: moeten nog waardes voor gemaakt worden?
ThomasBNL 66:04a203e43510 171 biquadFilter notchR1(high_a1, high_a2, high_b0, high_b1, high_b2); // EMG: moeten nog waardes voor gemaakt worden
ThomasBNL 66:04a203e43510 172 biquadFilter notchR2(low_a1, low_a2, low_b0, low_b1, low_b2); // EMG: moeten nog waardes voor gemaakt worden
ThomasBNL 66:04a203e43510 173 biquadFilter lowpassfilter_2(low_a1_LP, low_a2_LP, low_b0_LP, low_b1_LP, low_b2_LP); // EMG: moeten nog waardes voor gemaakt worden
ThomasBNL 46:9279c7a725bf 174
ThomasBNL 46:9279c7a725bf 175
ThomasBNL 66:04a203e43510 176 // MOTORS
ThomasBNL 66:04a203e43510 177 QEI motor_turn(D12,D13,NC,32); QEI motor_strike(D9,D10,NC,32); // TURN - STRIKE: Encoder for motor
ThomasBNL 66:04a203e43510 178 PwmOut pwm_motor_turn(D5); PwmOut pwm_motor_strike(D6); // TURN - STRIKE: Pwm for motor
ThomasBNL 66:04a203e43510 179 DigitalOut motordirection_turn(D4); DigitalOut motordirection_strike(D7); // TURN - STRIKE: Direction of the motor
ThomasBNL 66:04a203e43510 180
ThomasBNL 66:04a203e43510 181 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 182
ThomasBNL 66:04a203e43510 183 double position_turn, error_turn, reference_turn; double position_strike, error_strike, reference_strike;
ThomasBNL 66:04a203e43510 184 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 66:04a203e43510 185 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 66:04a203e43510 186 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 187
ThomasBNL 46:9279c7a725bf 188
ThomasBNL 66:04a203e43510 189 // FILTER: D-action
ThomasBNL 66:04a203e43510 190 const double a0 = 0.000332685098529822, a1 = 0.000665370197059644, a2 = 0.000332685098529822, b1 = -1.9625271814290315, b2 = 0.9638579218231508;
ThomasBNL 66:04a203e43510 191 // const double a0 = 0.000021080713160785432, a1 = 0.000042161426321570865, a2 = 0.000021080713160785432, b1 = -1.990754082898736, b2 = 0.9908384057513788; (0.75Hz)
ThomasBNL 66:04a203e43510 192 // const double a0 = 0.003543360146633312, a1 = 0.007086720293266624, a2 = 0.003543360146633312, b1 = -1.8704759567901301, b2 = 0.8846493973766635; (10Hz)
ThomasBNL 66:04a203e43510 193 // const double a0 = 0.0009129521023626334, a1 = 0.0018259042047252668, a2 = 0.0009129521023626334, b1 = -1.9368516414202819, b2 = 0.9405034498297324; (5Hz)
ThomasBNL 64:97e2db3eb0eb 194 biquadFilter Lowpassfilter_derivative(b1,b2,a0,a1,a2); // BiquadFilter used for the filtering of the Derivative action of the PID-action
ThomasBNL 46:9279c7a725bf 195
ThomasBNL 66:04a203e43510 196 const double cw=0; // MOTOR: turn direction zero is clockwise (front view)
ThomasBNL 66:04a203e43510 197 const double ccw=1; // MOTOR: turn direction one is counterclockwise (front view)
ThomasBNL 43:fb69ef657f30 198 const double off=1; // Led off
ThomasBNL 43:fb69ef657f30 199 const double on=0; // Led on
ThomasBNL 43:fb69ef657f30 200 const int Fs = 512; // sampling frequency (512 Hz)
ThomasBNL 43:fb69ef657f30 201 const double sample_time = 0.001953125; // duration of one sample
ThomasBNL 44:5dd0a3d24662 202 double conversion_counts_to_degrees=0.085877862594198; // Calculation conversion counts to degrees
ThomasBNL 44:5dd0a3d24662 203 // gear ratio motor = 131
ThomasBNL 44:5dd0a3d24662 204 // ticks per magnet rotation = 32 (X2 Encoder)
ThomasBNL 44:5dd0a3d24662 205 // One revolution = 360 degrees
ThomasBNL 44:5dd0a3d24662 206 // degrees_per_encoder_tick = 360/(gear_ratio*ticks_per_magnet_rotation)=360/131*32=0.085877862594198
ThomasBNL 46:9279c7a725bf 207 const double change_one_bottle=45;
ThomasBNL 66:04a203e43510 208 const double Hit=60; // position when bottle is hit
ThomasBNL 55:f4ab878ae910 209
ThomasBNL 66:04a203e43510 210
ThomasBNL 46:9279c7a725bf 211 //============================================================================================================================
ThomasBNL 34:c672f5c0763f 212 // ___________________________
ThomasBNL 34:c672f5c0763f 213 // // \\
ThomasBNL 34:c672f5c0763f 214 // || [FUNCTIONS USED] ||
ThomasBNL 34:c672f5c0763f 215 // \\___________________________//
ThomasBNL 54:9eb449571f4f 216 void execute_plant_turn (); // TURN: Check error -> execute PID controller -> write pwm and direction to motor
ThomasBNL 66:04a203e43510 217 void execute_plant_strike ();
ThomasBNL 54:9eb449571f4f 218 double PID_control (double reference, double position, double &integrate_error,
ThomasBNL 54:9eb449571f4f 219 double sample_time, double &previous_error,
ThomasBNL 54:9eb449571f4f 220 double P_gain, double I_gain, double D_gain);
ThomasBNL 66:04a203e43510 221
ThomasBNL 46:9279c7a725bf 222 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 223 void setlooptimerflag (void); // Sets looptimerflag volatile bool to true
ThomasBNL 8:50d6e2323d3b 224
ThomasBNL 46:9279c7a725bf 225 void deactivate_PID_Controller (double& P_gain, double& I_gain, double& D_gain); // STRIKE/TURN: Deactivate PID Controller
ThomasBNL 46:9279c7a725bf 226
ThomasBNL 46:9279c7a725bf 227 void activate_PID_Controller_strike (double& P_gain, double& I_gain, double& D_gain); // STRIKE: Activate PID Controller
ThomasBNL 66:04a203e43510 228 void activate_PID_Controller_turn (double& P_gain, double& I_gain, double& D_gain); // TURN: Activate PID Controller
ThomasBNL 46:9279c7a725bf 229
ThomasBNL 46:9279c7a725bf 230 void Change_Turn_Position_Left (double& reference, double change_one_bottle); // TURN: Change Reference position one bottle to the left
ThomasBNL 46:9279c7a725bf 231 void Change_Turn_Position_Right (double& reference, double change_one_bottle); // TURN: Change Reference position one bottle to the right
ThomasBNL 39:104a038f7b92 232
ThomasBNL 66:04a203e43510 233 void countdown_from_5(); // PUTTY: 5 second countdown inside
ThomasBNL 66:04a203e43510 234 void calibration(); // EMG: Calibrate the EMG signal (calculate min and max signal and determine threshold values)
ThomasBNL 46:9279c7a725bf 235 void Filter(); // EMG: Filter the incoming EMG signals
ThomasBNL 46:9279c7a725bf 236 void sample_filter(); // EMG: Calculate moving average (10 samples, one sample per 25 samples) using sample_filter => moving average over +-0.5 seconds
ThomasBNL 46:9279c7a725bf 237 void take_sample(); // EMG: Take a sample once every 25 samples that's used to calculate the moving average
ThomasBNL 46:9279c7a725bf 238 void ControlGo(); // EMG: function that gets a ticker attached and sets a certain loop to true every sample
ThomasBNL 66:04a203e43510 239
ThomasBNL 66:04a203e43510 240 void red();void blue();void green();void white();void yellow();void cyan();void purple(); void black(); // DEBUG: Different color LEDS
ThomasBNL 39:104a038f7b92 241
ThomasBNL 46:9279c7a725bf 242 //============================================================================================================================
ThomasBNL 46:9279c7a725bf 243 ///////////////////////////////
ThomasBNL 46:9279c7a725bf 244 // //
ThomasBNL 46:9279c7a725bf 245 ///////////////////////////////////////////// [MAIN FUNCTION] /////////////////////////////////////////////////////
ThomasBNL 46:9279c7a725bf 246 // //
ThomasBNL 46:9279c7a725bf 247 ///////////////////////////////
ThomasBNL 46:9279c7a725bf 248 //============================================================================================================================
ThomasBNL 0:40052f5ca77b 249 int main() {
ThomasBNL 43:fb69ef657f30 250
ThomasBNL 58:141787606c4a 251 black(); // No LED active
ThomasBNL 46:9279c7a725bf 252
ThomasBNL 43:fb69ef657f30 253 // ___________________________
ThomasBNL 43:fb69ef657f30 254 // // \\
ThomasBNL 43:fb69ef657f30 255 // || [START OF CODE] ||
ThomasBNL 43:fb69ef657f30 256 // \\___________________________//
ThomasBNL 43:fb69ef657f30 257 // START OF CODE
ThomasBNL 43:fb69ef657f30 258 pc.printf("Start of code \n\r");
ThomasBNL 43:fb69ef657f30 259
ThomasBNL 43:fb69ef657f30 260 pc.baud(115200); // Set the baudrate
ThomasBNL 0:40052f5ca77b 261
ThomasBNL 43:fb69ef657f30 262 // ___________________________
ThomasBNL 43:fb69ef657f30 263 // // \\
ThomasBNL 43:fb69ef657f30 264 // || [CALIBRATE] ||
ThomasBNL 43:fb69ef657f30 265 // \\___________________________//
ThomasBNL 43:fb69ef657f30 266 // Calibrate starting postion (RED LED)
ThomasBNL 43:fb69ef657f30 267
ThomasBNL 46:9279c7a725bf 268 // ___________________________
ThomasBNL 46:9279c7a725bf 269 // : [Starting position motor] :
ThomasBNL 46:9279c7a725bf 270 // :___________________________:
ThomasBNL 46:9279c7a725bf 271 //
ThomasBNL 46:9279c7a725bf 272
ThomasBNL 43:fb69ef657f30 273 pc.printf("Button calibration \n\r");
ThomasBNL 43:fb69ef657f30 274 while(1)
ThomasBNL 43:fb69ef657f30 275 {
ThomasBNL 58:141787606c4a 276 red();// RED LED // TURN: LOW buttons
ThomasBNL 43:fb69ef657f30 277
ThomasBNL 43:fb69ef657f30 278 if (buttonL2.read() < 0.5){ // TURN: turn the motor clockwise with pwm of 0.2
ThomasBNL 43:fb69ef657f30 279 motordirection_turn = cw;
ThomasBNL 48:950b1f34161b 280 pwm_motor_turn = 0.02;
ThomasBNL 48:950b1f34161b 281 pc.printf("cw calibration \n\r");}
ThomasBNL 43:fb69ef657f30 282
ThomasBNL 43:fb69ef657f30 283 if (buttonL1.read() < 0.5){ // TURN: turn the motor counterclockwise with pwm of 0.2
ThomasBNL 43:fb69ef657f30 284 motordirection_turn = ccw;
ThomasBNL 48:950b1f34161b 285 pwm_motor_turn = 0.02;
ThomasBNL 48:950b1f34161b 286 pc.printf("ccw calibration \n\r");}
ThomasBNL 48:950b1f34161b 287
ThomasBNL 48:950b1f34161b 288 pwm_motor_turn = 0;
ThomasBNL 43:fb69ef657f30 289 // STRIKE: HIGH buttons
ThomasBNL 43:fb69ef657f30 290
ThomasBNL 49:a8a68abf814f 291 if (buttonH2.read() < 0.5){ // STRIKE: turn the motor clockwise with pwm of 0.2
ThomasBNL 64:97e2db3eb0eb 292 motordirection_strike = cw;
ThomasBNL 64:97e2db3eb0eb 293 pwm_motor_strike = 0.02;
ThomasBNL 64:97e2db3eb0eb 294 pc.printf("cw calibration \n\r");}
ThomasBNL 44:5dd0a3d24662 295 //
ThomasBNL 49:a8a68abf814f 296 if (buttonH1.read() < 0.5){ // STRIKE: turn the motor clockwise with pwm of 0.2
ThomasBNL 64:97e2db3eb0eb 297 motordirection_strike = ccw;
ThomasBNL 64:97e2db3eb0eb 298 pwm_motor_strike = 0.02;
ThomasBNL 64:97e2db3eb0eb 299 pc.printf("ccw calibration \n\r");}
ThomasBNL 48:950b1f34161b 300
ThomasBNL 64:97e2db3eb0eb 301 pwm_motor_strike = 0;
ThomasBNL 44:5dd0a3d24662 302 //
ThomasBNL 45:359df0594588 303 if ((buttonL2.read() < 0.5) && (buttonL1.read() < 0.5)) // Save current TURN and STRIKE positions as starting position
ThomasBNL 43:fb69ef657f30 304 {
ThomasBNL 46:9279c7a725bf 305 motor_turn.reset(); // TURN: Starting Position
ThomasBNL 46:9279c7a725bf 306 reference_turn=0; // TURN: Set reference position to zero
ThomasBNL 66:04a203e43510 307 motor_strike.reset(); // STRIKE: Starting Position
ThomasBNL 46:9279c7a725bf 308 goto calibration_starting_position_complete; // Calibration complete
ThomasBNL 43:fb69ef657f30 309 }
ThomasBNL 43:fb69ef657f30 310 }
ThomasBNL 43:fb69ef657f30 311
ThomasBNL 46:9279c7a725bf 312 calibration_starting_position_complete:
ThomasBNL 58:141787606c4a 313 blue(); // Calibration end => RED LED off
ThomasBNL 40:bbe7922723df 314
ThomasBNL 46:9279c7a725bf 315 // ___________________________
ThomasBNL 46:9279c7a725bf 316 // : [EMG calibration] :
ThomasBNL 46:9279c7a725bf 317 // :___________________________:
ThomasBNL 46:9279c7a725bf 318 //
ThomasBNL 55:f4ab878ae910 319 //calibration(); // EMG: Calibration
ThomasBNL 46:9279c7a725bf 320
ThomasBNL 64:97e2db3eb0eb 321 // TEMPORARY USAGE WHILE POTMETER ACTIVE
ThomasBNL 64:97e2db3eb0eb 322 EMG_L_max = 100;
ThomasBNL 64:97e2db3eb0eb 323 EMG_L_min = 0;
ThomasBNL 64:97e2db3eb0eb 324 EMG_R_max = 100;
ThomasBNL 64:97e2db3eb0eb 325 EMG_R_min = 0;
ThomasBNL 64:97e2db3eb0eb 326
ThomasBNL 64:97e2db3eb0eb 327 const double 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 64:97e2db3eb0eb 328 const double 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 64:97e2db3eb0eb 329 const double 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 64:97e2db3eb0eb 330 const double 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 45:359df0594588 331 // ___________________________
ThomasBNL 45:359df0594588 332 // // \\
ThomasBNL 45:359df0594588 333 // || [ATTACH TICKER] ||
ThomasBNL 45:359df0594588 334 // \\___________________________//
ThomasBNL 45:359df0594588 335 // Attach Ticker to looptimerflag
ThomasBNL 45:359df0594588 336
ThomasBNL 45:359df0594588 337 looptimer.attach(setlooptimerflag,(float)1/Fs); // calls the looptimer flag every sample
ThomasBNL 58:141787606c4a 338 green();
ThomasBNL 55:f4ab878ae910 339 wait (3); // Wait 5 seconds before starting system
ThomasBNL 7:ddd7fb357786 340 pc.printf("Start infinite loop \n\r");
ThomasBNL 46:9279c7a725bf 341
ThomasBNL 44:5dd0a3d24662 342 // ___________________________
ThomasBNL 44:5dd0a3d24662 343 // // \\
ThomasBNL 44:5dd0a3d24662 344 // || [START INFINTTE LOOP] ||
ThomasBNL 44:5dd0a3d24662 345 // \\___________________________//
ThomasBNL 44:5dd0a3d24662 346 //
ThomasBNL 44:5dd0a3d24662 347
ThomasBNL 5:8fb74a22fe3c 348 while(1)
ThomasBNL 64:97e2db3eb0eb 349 { // Start while loop
ThomasBNL 64:97e2db3eb0eb 350 while(looptimerflag != true);
ThomasBNL 64:97e2db3eb0eb 351 looptimerflag = false;
ThomasBNL 64:97e2db3eb0eb 352 green();
ThomasBNL 64:97e2db3eb0eb 353 execute_plant_turn();
ThomasBNL 64:97e2db3eb0eb 354
ThomasBNL 34:c672f5c0763f 355 // ___________________________
ThomasBNL 34:c672f5c0763f 356 // // \\
ThomasBNL 64:97e2db3eb0eb 357 // || [ACTION CONTROLLER] ||
ThomasBNL 34:c672f5c0763f 358 // \\___________________________//
ThomasBNL 64:97e2db3eb0eb 359 // // 3 possible actions (left, right, strike)
ThomasBNL 44:5dd0a3d24662 360
ThomasBNL 64:97e2db3eb0eb 361 Nieuwe_actie:
ThomasBNL 64:97e2db3eb0eb 362 green();
ThomasBNL 46:9279c7a725bf 363
ThomasBNL 46:9279c7a725bf 364 // ___________________________
ThomasBNL 46:9279c7a725bf 365 // // \\
ThomasBNL 46:9279c7a725bf 366 // || [GET CURRENT EMG VALUES] ||
ThomasBNL 46:9279c7a725bf 367 // \\___________________________//
ThomasBNL 64:97e2db3eb0eb 368
ThomasBNL 64:97e2db3eb0eb 369 ////// //sample_filter(); // TIJDELIJK UIT
ThomasBNL 61:5b644df6f6ab 370
ThomasBNL 64:97e2db3eb0eb 371 moving_average_left = (input1.read())*100; // EMG Right bicep (tussen nul en 100%) // DEBUG TEST TOOL: substitute EMG input for potmeter inputs
ThomasBNL 64:97e2db3eb0eb 372 moving_average_right = (input2.read())*100; // EMG Left bicep (tussen nul en 100%) // DEBUG TEST TOOL: substitute EMG input for potmeter inputs
ThomasBNL 53:05194bab0bfd 373 pc.printf("mov_r = %f, mov_l = %f \n\r", moving_average_right, moving_average_left);
ThomasBNL 60:084fd3352882 374 /// ___________________________
ThomasBNL 60:084fd3352882 375 // // \\
ThomasBNL 64:97e2db3eb0eb 376 // || [Action 1: Strike] ||
ThomasBNL 60:084fd3352882 377 // \\___________________________//
ThomasBNL 64:97e2db3eb0eb 378 // // blue \\
ThomasBNL 60:084fd3352882 379
ThomasBNL 65:2da8cf778181 380 moving_average_left = 40; // TIJDELIJK PID TEST
ThomasBNL 65:2da8cf778181 381 moving_average_right = 40; // TIJDELIJK PID TEST
ThomasBNL 60:084fd3352882 382 if (moving_average_right > Threshold_Bicep_Right_1 && moving_average_left > Threshold_Bicep_Left_1)
ThomasBNL 60:084fd3352882 383 {
ThomasBNL 60:084fd3352882 384 blue();
ThomasBNL 64:97e2db3eb0eb 385 n=0; k=0; p=0;
ThomasBNL 60:084fd3352882 386 pc.printf("Slag \n\r");
ThomasBNL 61:5b644df6f6ab 387
ThomasBNL 61:5b644df6f6ab 388 wait(0.5); // TIJDELIJK
ThomasBNL 61:5b644df6f6ab 389
ThomasBNL 64:97e2db3eb0eb 390 if(moving_average_left > Threshold_Bicep_Left_1 && moving_average_right > Threshold_Bicep_Right_1)
ThomasBNL 64:97e2db3eb0eb 391 {
ThomasBNL 64:97e2db3eb0eb 392 while(1)
ThomasBNL 60:084fd3352882 393 {
ThomasBNL 64:97e2db3eb0eb 394 if (n==0)
ThomasBNL 61:5b644df6f6ab 395 {
ThomasBNL 64:97e2db3eb0eb 396 reference_strike=90;
ThomasBNL 61:5b644df6f6ab 397 n=1;
ThomasBNL 61:5b644df6f6ab 398 error_count=0;
ThomasBNL 64:97e2db3eb0eb 399 }
ThomasBNL 63:d86a46c8aa0c 400
ThomasBNL 60:084fd3352882 401 if (looptimerflag == true)
ThomasBNL 60:084fd3352882 402 {
ThomasBNL 60:084fd3352882 403 looptimerflag=false;
ThomasBNL 64:97e2db3eb0eb 404 activate_PID_Controller_strike(P_gain_strike, I_gain_strike, D_gain_strike);
ThomasBNL 63:d86a46c8aa0c 405 execute_plant_strike();
ThomasBNL 60:084fd3352882 406 }
ThomasBNL 61:5b644df6f6ab 407
ThomasBNL 64:97e2db3eb0eb 408 if (fabs(position_strike)>90)
ThomasBNL 61:5b644df6f6ab 409 {
ThomasBNL 61:5b644df6f6ab 410 while(1)
ThomasBNL 64:97e2db3eb0eb 411 { yellow();
ThomasBNL 61:5b644df6f6ab 412 if(k==0)
ThomasBNL 61:5b644df6f6ab 413 {
ThomasBNL 64:97e2db3eb0eb 414 p=1;
ThomasBNL 64:97e2db3eb0eb 415 reference_strike=0; error_count=0; k=1;
ThomasBNL 64:97e2db3eb0eb 416 pc.printf("return \n\r");
ThomasBNL 61:5b644df6f6ab 417 }
ThomasBNL 64:97e2db3eb0eb 418
ThomasBNL 64:97e2db3eb0eb 419 //pc.printf("ref_t = %f, e_cnt= %f e_av=%f \n\r k=%f, er_cnt= %f", reference_strike, error_strike, error_strike_average, k, error_count);
ThomasBNL 61:5b644df6f6ab 420
ThomasBNL 61:5b644df6f6ab 421 if (looptimerflag == true)
ThomasBNL 61:5b644df6f6ab 422 {
ThomasBNL 61:5b644df6f6ab 423 looptimerflag=false;
ThomasBNL 64:97e2db3eb0eb 424 activate_PID_Controller_strike(P_gain_strike, I_gain_strike, D_gain_strike);
ThomasBNL 64:97e2db3eb0eb 425 //D_gain_strike=(input1.read())*0.000001;
ThomasBNL 64:97e2db3eb0eb 426 //printf("D_s = %f \n\r",D_gain_strike);
ThomasBNL 64:97e2db3eb0eb 427 execute_plant_strike();
ThomasBNL 61:5b644df6f6ab 428 }
ThomasBNL 61:5b644df6f6ab 429
ThomasBNL 64:97e2db3eb0eb 430 printf(" %f \n\r",error_strike_average);
ThomasBNL 64:97e2db3eb0eb 431
ThomasBNL 65:2da8cf778181 432 if (fabs(error_strike_average) < 0.5 && error_count>100)
ThomasBNL 64:97e2db3eb0eb 433 {
ThomasBNL 64:97e2db3eb0eb 434 yellow();
ThomasBNL 64:97e2db3eb0eb 435 pc.printf("new action \n\r");
ThomasBNL 64:97e2db3eb0eb 436 deactivate_PID_Controller(P_gain_strike, I_gain_strike, D_gain_strike);
ThomasBNL 64:97e2db3eb0eb 437 execute_plant_strike();
ThomasBNL 64:97e2db3eb0eb 438 goto Nieuwe_actie;
ThomasBNL 64:97e2db3eb0eb 439 }
ThomasBNL 64:97e2db3eb0eb 440 }
ThomasBNL 61:5b644df6f6ab 441 }
ThomasBNL 61:5b644df6f6ab 442 }
ThomasBNL 60:084fd3352882 443 }
ThomasBNL 60:084fd3352882 444 }
ThomasBNL 58:141787606c4a 445 // ___________________________
ThomasBNL 58:141787606c4a 446 // // \\
ThomasBNL 64:97e2db3eb0eb 447 // || [Action 2: TURN LEFT] ||
ThomasBNL 58:141787606c4a 448 // \\___________________________//
ThomasBNL 64:97e2db3eb0eb 449 // // yellow \\
ThomasBNL 58:141787606c4a 450
ThomasBNL 59:38a302b9f7f9 451 if (moving_average_left > Threshold_Bicep_Left_2 && moving_average_right < Threshold_Bicep_Right_1)
ThomasBNL 53:05194bab0bfd 452 {
ThomasBNL 64:97e2db3eb0eb 453 yellow();
ThomasBNL 53:05194bab0bfd 454 n=0;
ThomasBNL 64:97e2db3eb0eb 455 pc.printf("LEFT \n\r");
ThomasBNL 64:97e2db3eb0eb 456
ThomasBNL 59:38a302b9f7f9 457 wait(2); // TIJDELIJK
ThomasBNL 64:97e2db3eb0eb 458
ThomasBNL 53:05194bab0bfd 459 while(moving_average_left > Threshold_Bicep_Left_1 && moving_average_right < Threshold_Bicep_Right_1)
ThomasBNL 53:05194bab0bfd 460 {
ThomasBNL 64:97e2db3eb0eb 461 if (n==0)
ThomasBNL 55:f4ab878ae910 462 {
ThomasBNL 59:38a302b9f7f9 463 Change_Turn_Position_Left(reference_turn, change_one_bottle);
ThomasBNL 64:97e2db3eb0eb 464 n=1; error_count=0;
ThomasBNL 64:97e2db3eb0eb 465 }
ThomasBNL 64:97e2db3eb0eb 466
ThomasBNL 55:f4ab878ae910 467 pc.printf("ref_t = %f, e_cnt= %f e_av=%f \n\r", reference_turn, error_count, error_turn_average);
ThomasBNL 54:9eb449571f4f 468
ThomasBNL 54:9eb449571f4f 469 if (looptimerflag == true)
ThomasBNL 54:9eb449571f4f 470 {
ThomasBNL 55:f4ab878ae910 471 looptimerflag=false;
ThomasBNL 55:f4ab878ae910 472 activate_PID_Controller_turn(P_gain_turn, I_gain_turn, D_gain_turn);
ThomasBNL 54:9eb449571f4f 473 execute_plant_turn();
ThomasBNL 54:9eb449571f4f 474 }
ThomasBNL 54:9eb449571f4f 475
ThomasBNL 64:97e2db3eb0eb 476 if (fabs(error_turn_average) < 1 && error_count>250)
ThomasBNL 53:05194bab0bfd 477 {
ThomasBNL 55:f4ab878ae910 478 pc.printf("new action \n\r");
ThomasBNL 55:f4ab878ae910 479 deactivate_PID_Controller(P_gain_turn, I_gain_turn, D_gain_turn);
ThomasBNL 55:f4ab878ae910 480 execute_plant_turn();
ThomasBNL 64:97e2db3eb0eb 481 goto Nieuwe_actie;
ThomasBNL 53:05194bab0bfd 482 }
ThomasBNL 53:05194bab0bfd 483 }
ThomasBNL 53:05194bab0bfd 484 }
ThomasBNL 59:38a302b9f7f9 485 /// ___________________________
ThomasBNL 59:38a302b9f7f9 486 // // \\
ThomasBNL 64:97e2db3eb0eb 487 // || [Action 3: TURN RIGHT] ||
ThomasBNL 59:38a302b9f7f9 488 // \\___________________________//
ThomasBNL 64:97e2db3eb0eb 489 // //Purple\\
ThomasBNL 46:9279c7a725bf 490
ThomasBNL 59:38a302b9f7f9 491 if (moving_average_right > Threshold_Bicep_Right_2 && moving_average_left < Threshold_Bicep_Left_1)
ThomasBNL 59:38a302b9f7f9 492 {
ThomasBNL 64:97e2db3eb0eb 493 purple();
ThomasBNL 59:38a302b9f7f9 494 n=0;
ThomasBNL 59:38a302b9f7f9 495 pc.printf("Right \n\r");
ThomasBNL 64:97e2db3eb0eb 496
ThomasBNL 59:38a302b9f7f9 497 wait(2); // TIJDELIJK
ThomasBNL 64:97e2db3eb0eb 498
ThomasBNL 59:38a302b9f7f9 499 while(moving_average_right > Threshold_Bicep_Right_1 && moving_average_left < Threshold_Bicep_Left_1)
ThomasBNL 59:38a302b9f7f9 500 {
ThomasBNL 64:97e2db3eb0eb 501 if (n==0)
ThomasBNL 59:38a302b9f7f9 502 {
ThomasBNL 59:38a302b9f7f9 503 Change_Turn_Position_Right(reference_turn, change_one_bottle);
ThomasBNL 64:97e2db3eb0eb 504 n=1; error_count=0;
ThomasBNL 64:97e2db3eb0eb 505 }
ThomasBNL 64:97e2db3eb0eb 506
ThomasBNL 59:38a302b9f7f9 507 pc.printf("ref_t = %f, e_cnt= %f e_av=%f \n\r", reference_turn, error_count, error_turn_average);
ThomasBNL 59:38a302b9f7f9 508
ThomasBNL 59:38a302b9f7f9 509 if (looptimerflag == true)
ThomasBNL 59:38a302b9f7f9 510 {
ThomasBNL 59:38a302b9f7f9 511 looptimerflag=false;
ThomasBNL 59:38a302b9f7f9 512 activate_PID_Controller_turn(P_gain_turn, I_gain_turn, D_gain_turn);
ThomasBNL 59:38a302b9f7f9 513 execute_plant_turn();
ThomasBNL 59:38a302b9f7f9 514 }
ThomasBNL 59:38a302b9f7f9 515
ThomasBNL 64:97e2db3eb0eb 516 if (fabs(error_turn_average) < 1 && error_count>250)
ThomasBNL 59:38a302b9f7f9 517 {
ThomasBNL 59:38a302b9f7f9 518 pc.printf("new action \n\r");
ThomasBNL 59:38a302b9f7f9 519 deactivate_PID_Controller(P_gain_turn, I_gain_turn, D_gain_turn);
ThomasBNL 59:38a302b9f7f9 520 execute_plant_turn();
ThomasBNL 64:97e2db3eb0eb 521 goto Nieuwe_actie;
ThomasBNL 59:38a302b9f7f9 522 }
ThomasBNL 59:38a302b9f7f9 523 }
ThomasBNL 59:38a302b9f7f9 524 }
ThomasBNL 46:9279c7a725bf 525
ThomasBNL 34:c672f5c0763f 526 // ___________________________
ThomasBNL 34:c672f5c0763f 527 // // \\
ThomasBNL 34:c672f5c0763f 528 // || [HIDSCOPE] ||
ThomasBNL 34:c672f5c0763f 529 // \\___________________________//
ThomasBNL 34:c672f5c0763f 530 // // Check signals inside HIDSCOPE \\
ThomasBNL 34:c672f5c0763f 531
ThomasBNL 64:97e2db3eb0eb 532 //scope.set(0,error_turn); // HIDSCOPE channel 0 : Current Error
ThomasBNL 48:950b1f34161b 533 //scope.set(1,position_turn); // HIDSCOPE channel 1 : Position_turn
ThomasBNL 48:950b1f34161b 534 //scope.set(2,pwm_to_motor_turn); // HIDSCOPE channel 2 : Pwm_to_motor_turn
ThomasBNL 48:950b1f34161b 535 //scope.send(); // Send channel info to HIDSCOPE
ThomasBNL 40:bbe7922723df 536
ThomasBNL 0:40052f5ca77b 537 }
ThomasBNL 5:8fb74a22fe3c 538 }
ThomasBNL 0:40052f5ca77b 539
ThomasBNL 46:9279c7a725bf 540 //============================================================================================================================
ThomasBNL 46:9279c7a725bf 541 ///////////////////////////////
ThomasBNL 46:9279c7a725bf 542 // //
ThomasBNL 46:9279c7a725bf 543 ///////////////////////////////////////////// [FUNCTIONS DESCRIBED] /////////////////////////////////////////////////////
ThomasBNL 46:9279c7a725bf 544 // //
ThomasBNL 46:9279c7a725bf 545 ///////////////////////////////
ThomasBNL 46:9279c7a725bf 546 //============================================================================================================================
ThomasBNL 46:9279c7a725bf 547 // ___________________________
ThomasBNL 46:9279c7a725bf 548 // // \\
ThomasBNL 46:9279c7a725bf 549 // || [MOTOR FUCNTIONS] ||
ThomasBNL 46:9279c7a725bf 550 // \\___________________________//
ThomasBNL 49:a8a68abf814f 551 //
ThomasBNL 54:9eb449571f4f 552 // PLANT CONTROL TURN
ThomasBNL 54:9eb449571f4f 553 void execute_plant_turn()
ThomasBNL 55:f4ab878ae910 554 {
ThomasBNL 55:f4ab878ae910 555 if ((motor_turn.getPulses()>4200) || (motor_turn.getPulses()<-4200)) // If value is outside -4200 and 4200 (number of counts equal to one revolution) reset to zero
ThomasBNL 55:f4ab878ae910 556 {
ThomasBNL 55:f4ab878ae910 557 motor_turn.reset();
ThomasBNL 55:f4ab878ae910 558 pc.printf("RESET \n\r");
ThomasBNL 55:f4ab878ae910 559 }
ThomasBNL 55:f4ab878ae910 560
ThomasBNL 55:f4ab878ae910 561 position_turn = conversion_counts_to_degrees * motor_turn.getPulses();
ThomasBNL 55:f4ab878ae910 562
ThomasBNL 54:9eb449571f4f 563 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 54:9eb449571f4f 564 keep_in_range(&pwm_to_motor_turn, -1,1); // Pass to motor controller but keep it in range!
ThomasBNL 55:f4ab878ae910 565 //pc.printf("pwm %f \n\r", pwm_to_motor_turn);
ThomasBNL 54:9eb449571f4f 566
ThomasBNL 54:9eb449571f4f 567 // Check error and decide direction to turn
ThomasBNL 54:9eb449571f4f 568 if(pwm_to_motor_turn > 0)
ThomasBNL 54:9eb449571f4f 569 {
ThomasBNL 54:9eb449571f4f 570 motordirection_turn=ccw;
ThomasBNL 55:f4ab878ae910 571 //pc.printf("if loop pwm > 0 \n\r");
ThomasBNL 54:9eb449571f4f 572 }
ThomasBNL 54:9eb449571f4f 573 else
ThomasBNL 54:9eb449571f4f 574 {
ThomasBNL 54:9eb449571f4f 575 motordirection_turn=cw;
ThomasBNL 55:f4ab878ae910 576 //pc.printf("else loop pwm < 0 \n\r");
ThomasBNL 54:9eb449571f4f 577 }
ThomasBNL 54:9eb449571f4f 578 pwm_motor_turn=(abs(pwm_to_motor_turn));
ThomasBNL 55:f4ab878ae910 579
ThomasBNL 55:f4ab878ae910 580 take_sample();
ThomasBNL 64:97e2db3eb0eb 581
ThomasBNL 64:97e2db3eb0eb 582 // sample_filter();
ThomasBNL 55:f4ab878ae910 583
ThomasBNL 55:f4ab878ae910 584 if(sample_error) // sample_error -- e10;e9;e8;e7;e6;e5:e4;e3;e2;e1 -- error_turn_average --- er
ThomasBNL 55:f4ab878ae910 585 {
ThomasBNL 55:f4ab878ae910 586 sample_error=false;
ThomasBNL 55:f4ab878ae910 587 e1 = (position_turn - reference_turn);
ThomasBNL 57:8f3603cc2e71 588 e30=e29; e29=e28 ;e28=e27; e27=e26; e26=e25; e25=e24; e24=e23; e23=e22; e22=e21; e21=e20;
ThomasBNL 57:8f3603cc2e71 589 e20=e19; e19=e18 ;e18=e17; e17=e16; e16=e15; e15=e14; e14=e13; e13=e12; e12=e11; e11=e10;
ThomasBNL 55:f4ab878ae910 590 e10=e9 ;e9=e8; e8=e7; e7=e6; e6=e5; e5=e4; e4=e3; e3=e2; e2=e1;
ThomasBNL 55:f4ab878ae910 591 }
ThomasBNL 57:8f3603cc2e71 592 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 55:f4ab878ae910 593 er++;
ThomasBNL 55:f4ab878ae910 594 error_count++;
ThomasBNL 54:9eb449571f4f 595 }
ThomasBNL 63:d86a46c8aa0c 596
ThomasBNL 63:d86a46c8aa0c 597 // PLANT CONTROL STRIKE
ThomasBNL 63:d86a46c8aa0c 598 void execute_plant_strike()
ThomasBNL 63:d86a46c8aa0c 599 {
ThomasBNL 64:97e2db3eb0eb 600 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 63:d86a46c8aa0c 601 {
ThomasBNL 64:97e2db3eb0eb 602 motor_strike.reset();
ThomasBNL 63:d86a46c8aa0c 603 pc.printf("RESET \n\r");
ThomasBNL 63:d86a46c8aa0c 604 }
ThomasBNL 63:d86a46c8aa0c 605
ThomasBNL 64:97e2db3eb0eb 606 position_strike = conversion_counts_to_degrees * motor_strike.getPulses();
ThomasBNL 63:d86a46c8aa0c 607
ThomasBNL 65:2da8cf778181 608
ThomasBNL 64:97e2db3eb0eb 609 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 64:97e2db3eb0eb 610 keep_in_range(&pwm_to_motor_strike, -1,1); // Pass to motor controller but keep it in range!
ThomasBNL 63:d86a46c8aa0c 611 //pc.printf("pwm %f \n\r", pwm_to_motor_turn);
ThomasBNL 63:d86a46c8aa0c 612
ThomasBNL 63:d86a46c8aa0c 613 // Check error and decide direction to turn
ThomasBNL 64:97e2db3eb0eb 614 if(pwm_to_motor_strike > 0)
ThomasBNL 63:d86a46c8aa0c 615 {
ThomasBNL 64:97e2db3eb0eb 616 motordirection_strike=cw;
ThomasBNL 63:d86a46c8aa0c 617 //pc.printf("if loop pwm > 0 \n\r");
ThomasBNL 63:d86a46c8aa0c 618 }
ThomasBNL 63:d86a46c8aa0c 619 else
ThomasBNL 63:d86a46c8aa0c 620 {
ThomasBNL 64:97e2db3eb0eb 621 motordirection_strike=ccw;
ThomasBNL 63:d86a46c8aa0c 622 //pc.printf("else loop pwm < 0 \n\r");
ThomasBNL 63:d86a46c8aa0c 623 }
ThomasBNL 64:97e2db3eb0eb 624
ThomasBNL 64:97e2db3eb0eb 625 if(p==1)
ThomasBNL 64:97e2db3eb0eb 626 {
ThomasBNL 64:97e2db3eb0eb 627 pwm_motor_strike=(abs(pwm_to_motor_strike));
ThomasBNL 64:97e2db3eb0eb 628 }
ThomasBNL 64:97e2db3eb0eb 629
ThomasBNL 64:97e2db3eb0eb 630
ThomasBNL 63:d86a46c8aa0c 631 // TEMPORARY USAGE WHILE POTMETER ACTIVE
ThomasBNL 63:d86a46c8aa0c 632 EMG_L_max = 100; // Calibreren (max average over 5 seconde?) gemeten integraal EMG over tijd / (tijdsample stappen)=100
ThomasBNL 63:d86a46c8aa0c 633 EMG_L_min = 0;
ThomasBNL 63:d86a46c8aa0c 634 EMG_R_max = 100; // Calibreren
ThomasBNL 63:d86a46c8aa0c 635 EMG_R_min = 0;
ThomasBNL 63:d86a46c8aa0c 636
ThomasBNL 63:d86a46c8aa0c 637 const double 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 63:d86a46c8aa0c 638 const double 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 63:d86a46c8aa0c 639
ThomasBNL 65:2da8cf778181 640 moving_average_left = 40; // TIJDELIJK PID TEST
ThomasBNL 65:2da8cf778181 641 moving_average_right = 40; // TIJDELIJK PID TEST
ThomasBNL 63:d86a46c8aa0c 642 double signal_above_threshold=(moving_average_right-Threshold_Bicep_Right_1)+(moving_average_left-Threshold_Bicep_Left_1);
ThomasBNL 63:d86a46c8aa0c 643 double max_signal=(EMG_R_max-Threshold_Bicep_Right_1)+(EMG_L_max-Threshold_Bicep_Left_1);
ThomasBNL 63:d86a46c8aa0c 644 double pwm_strike=signal_above_threshold/max_signal;
ThomasBNL 63:d86a46c8aa0c 645
ThomasBNL 64:97e2db3eb0eb 646 //pc.printf("mov_r = %f, mov_l = %f, pwm_strike = %f position = %f \n\r", moving_average_right, moving_average_left, pwm_strike, position_strike);
ThomasBNL 64:97e2db3eb0eb 647 if(p==0)
ThomasBNL 64:97e2db3eb0eb 648 {
ThomasBNL 64:97e2db3eb0eb 649 pwm_motor_strike=pwm_strike;
ThomasBNL 64:97e2db3eb0eb 650 }
ThomasBNL 63:d86a46c8aa0c 651
ThomasBNL 63:d86a46c8aa0c 652 take_sample();
ThomasBNL 64:97e2db3eb0eb 653
ThomasBNL 64:97e2db3eb0eb 654 // sample_filter();
ThomasBNL 64:97e2db3eb0eb 655
ThomasBNL 64:97e2db3eb0eb 656 if(sample_error_strike)
ThomasBNL 64:97e2db3eb0eb 657 {
ThomasBNL 64:97e2db3eb0eb 658 sample_error_strike=false;
ThomasBNL 64:97e2db3eb0eb 659 e1 = fabs(position_strike - reference_strike);
ThomasBNL 63:d86a46c8aa0c 660 e30=e29; e29=e28 ;e28=e27; e27=e26; e26=e25; e25=e24; e24=e23; e23=e22; e22=e21; e21=e20;
ThomasBNL 63:d86a46c8aa0c 661 e20=e19; e19=e18 ;e18=e17; e17=e16; e16=e15; e15=e14; e14=e13; e13=e12; e12=e11; e11=e10;
ThomasBNL 63:d86a46c8aa0c 662 e10=e9 ;e9=e8; e8=e7; e7=e6; e6=e5; e5=e4; e4=e3; e3=e2; e2=e1;
ThomasBNL 64:97e2db3eb0eb 663 }
ThomasBNL 64:97e2db3eb0eb 664
ThomasBNL 64:97e2db3eb0eb 665 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 666 er++;
ThomasBNL 63:d86a46c8aa0c 667 error_count++;
ThomasBNL 63:d86a46c8aa0c 668 }
ThomasBNL 54:9eb449571f4f 669
ThomasBNL 49:a8a68abf814f 670 // PID CONTROLLER
ThomasBNL 49:a8a68abf814f 671 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 672 {
ThomasBNL 50:6060f45d343a 673 double error=(reference - position); // TURN: Current error (input controller)
ThomasBNL 49:a8a68abf814f 674 integrate_error=integrate_error_turn + sample_time*error_turn; // TURN: Integral error output
ThomasBNL 49:a8a68abf814f 675 // overwrite previous integrate error by adding the current error
ThomasBNL 49:a8a68abf814f 676 // multiplied by the sample time.
ThomasBNL 52:b530adf72f79 677
ThomasBNL 55:f4ab878ae910 678 //pc.printf("error = %f", error);
ThomasBNL 55:f4ab878ae910 679 //pc.printf("P= %f", P_gain);
ThomasBNL 55:f4ab878ae910 680 //pc.printf("I = %f", I_gain);
ThomasBNL 55:f4ab878ae910 681 //pc.printf("D = %f", D_gain);
ThomasBNL 50:6060f45d343a 682 double error_derivative=(error - previous_error)/sample_time; // TURN: Derivative error output
ThomasBNL 50:6060f45d343a 683 error_derivative=Lowpassfilter_derivative.step(error_derivative); // TURN: Filter
ThomasBNL 49:a8a68abf814f 684
ThomasBNL 49:a8a68abf814f 685 previous_error_turn=error_turn; // current error is saved to memory constant to be used in
ThomasBNL 49:a8a68abf814f 686 // the next loop for calculating the derivative error
ThomasBNL 49:a8a68abf814f 687
ThomasBNL 50:6060f45d343a 688 double pwm_motor_P = error*P_gain; // Output P controller to pwm
ThomasBNL 50:6060f45d343a 689 double pwm_motor_I = integrate_error*I_gain; // Output I controller to pwm
ThomasBNL 50:6060f45d343a 690 double pwm_motor_D = error_derivative*D_gain; // Output D controller to pwm
ThomasBNL 49:a8a68abf814f 691
ThomasBNL 50:6060f45d343a 692 double pwm_to_motor = pwm_motor_P + pwm_motor_I + pwm_motor_D;
ThomasBNL 49:a8a68abf814f 693
ThomasBNL 49:a8a68abf814f 694 return pwm_to_motor;
ThomasBNL 49:a8a68abf814f 695 }
ThomasBNL 34:c672f5c0763f 696
ThomasBNL 0:40052f5ca77b 697 // Keep in range function
ThomasBNL 0:40052f5ca77b 698 void keep_in_range(double * in, double min, double max)
ThomasBNL 0:40052f5ca77b 699 {
ThomasBNL 0:40052f5ca77b 700 *in > min ? *in < max? : *in = max: *in = min;
ThomasBNL 0:40052f5ca77b 701 }
ThomasBNL 0:40052f5ca77b 702
ThomasBNL 0:40052f5ca77b 703 // Looptimerflag function
ThomasBNL 0:40052f5ca77b 704 void setlooptimerflag(void)
ThomasBNL 0:40052f5ca77b 705 {
ThomasBNL 0:40052f5ca77b 706 looptimerflag = true;
ThomasBNL 1:dc683e88b44e 707 }
ThomasBNL 1:dc683e88b44e 708
ThomasBNL 39:104a038f7b92 709 // Change reference
ThomasBNL 58:141787606c4a 710 void Change_Turn_Position_Right (double& reference, double change_one_bottle)
ThomasBNL 39:104a038f7b92 711 {
ThomasBNL 46:9279c7a725bf 712 if(reference==90) // added reference if at boundary bottle and try to go to the side where no bottles are; than immediatly turn 5 bottles to the left
ThomasBNL 45:359df0594588 713 {
ThomasBNL 45:359df0594588 714 reference=-90;
ThomasBNL 45:359df0594588 715 }
ThomasBNL 45:359df0594588 716 else
ThomasBNL 45:359df0594588 717 {
ThomasBNL 45:359df0594588 718 reference = reference + change_one_bottle;
ThomasBNL 46:9279c7a725bf 719 keep_in_range(&reference, -90, 90); // reference position stays between -90 and 90 degrees (IF bottles at -90, -45, 0, 45, 90 degrees)
ThomasBNL 45:359df0594588 720 }
ThomasBNL 39:104a038f7b92 721 }
ThomasBNL 39:104a038f7b92 722
ThomasBNL 58:141787606c4a 723 void Change_Turn_Position_Left (double& reference, double change_one_bottle)
ThomasBNL 39:104a038f7b92 724 {
ThomasBNL 45:359df0594588 725 if(reference==-90)
ThomasBNL 45:359df0594588 726 {
ThomasBNL 45:359df0594588 727 reference=90;
ThomasBNL 45:359df0594588 728 }
ThomasBNL 45:359df0594588 729 else
ThomasBNL 45:359df0594588 730 {
ThomasBNL 45:359df0594588 731 reference = reference - change_one_bottle;
ThomasBNL 45:359df0594588 732 keep_in_range(&reference, -90, 90);
ThomasBNL 45:359df0594588 733 }
ThomasBNL 39:104a038f7b92 734 }
ThomasBNL 39:104a038f7b92 735
ThomasBNL 42:0a7898cb3e94 736 // Deactivate or Activate PID_Controller
ThomasBNL 42:0a7898cb3e94 737 void deactivate_PID_Controller(double& P_gain, double& I_gain, double& D_gain)
ThomasBNL 42:0a7898cb3e94 738 {
ThomasBNL 42:0a7898cb3e94 739 P_gain=0;
ThomasBNL 42:0a7898cb3e94 740 I_gain=0;
ThomasBNL 42:0a7898cb3e94 741 D_gain=0;
ThomasBNL 54:9eb449571f4f 742 pwm_motor_turn=0;
ThomasBNL 64:97e2db3eb0eb 743 pwm_motor_strike=0;
ThomasBNL 42:0a7898cb3e94 744 }
ThomasBNL 42:0a7898cb3e94 745
ThomasBNL 42:0a7898cb3e94 746 void activate_PID_Controller_turn(double& P_gain, double& I_gain, double& D_gain)
ThomasBNL 42:0a7898cb3e94 747 {
ThomasBNL 57:8f3603cc2e71 748 P_gain_turn=0.02; // hier waardes P,I,D veranderen (waardes bovenaan doen (tijdelijk) niks meer
ThomasBNL 57:8f3603cc2e71 749 I_gain_turn=0.1;
ThomasBNL 52:b530adf72f79 750 D_gain_turn=0;
ThomasBNL 42:0a7898cb3e94 751 }
ThomasBNL 42:0a7898cb3e94 752
ThomasBNL 42:0a7898cb3e94 753 void activate_PID_Controller_strike(double& P_gain, double& I_gain, double& D_gain)
ThomasBNL 42:0a7898cb3e94 754 {
ThomasBNL 65:2da8cf778181 755 double Ku = (input1.read())*1; // EMG Right bicep (tussen nul en 100%) // DEBUG TEST TOOL: substitute EMG input for potmeter inputs
ThomasBNL 65:2da8cf778181 756 double Pu = (input2.read())*1; // EMG Left bicep (tussen nul en 100%)
ThomasBNL 65:2da8cf778181 757
ThomasBNL 66:04a203e43510 758 P_gain_strike=0.8*Ku; // Ku=0.2 (ultimate gain Ziegler-Nichols method)
ThomasBNL 66:04a203e43510 759 // Pu=0.25 (ultimate period) (4Hz)
ThomasBNL 65:2da8cf778181 760
ThomasBNL 65:2da8cf778181 761 pc.printf("Ku=%f Pu=%f \n\r", Ku, Pu);
ThomasBNL 65:2da8cf778181 762 //0.09090909;
ThomasBNL 65:2da8cf778181 763 //PI tyreus luyben : 0.0625, 0.55;
ThomasBNL 65:2da8cf778181 764 //PID tyreus luyben : 0.09090909, 0.55, 0.0396825;
ThomasBNL 65:2da8cf778181 765 // Ku=0.2 (ultimate gain Ziegler-Nichols method)
ThomasBNL 65:2da8cf778181 766 // Pu=0.25 (ultimate period) (4Hz)
ThomasBNL 65:2da8cf778181 767 // hier waardes P,I,D veranderen (waardes bovenaan doen (tijdelijk) niks meer // 0.00045 // 0.03
ThomasBNL 65:2da8cf778181 768 I_gain_strike=0; //0.55;
ThomasBNL 66:04a203e43510 769 D_gain_strike=Pu/8; //0.0396825;
ThomasBNL 46:9279c7a725bf 770 }
ThomasBNL 46:9279c7a725bf 771
ThomasBNL 46:9279c7a725bf 772 //============================================================================================================================
ThomasBNL 46:9279c7a725bf 773 // ___________________________
ThomasBNL 46:9279c7a725bf 774 // // \\
ThomasBNL 46:9279c7a725bf 775 // || [EMG FUCNTIONS] ||
ThomasBNL 46:9279c7a725bf 776 // \\___________________________//
ThomasBNL 46:9279c7a725bf 777 //
ThomasBNL 46:9279c7a725bf 778
ThomasBNL 46:9279c7a725bf 779 // [CALIBRATION] // (blue LED)
ThomasBNL 46:9279c7a725bf 780 void calibration()
ThomasBNL 46:9279c7a725bf 781 {
ThomasBNL 46:9279c7a725bf 782
ThomasBNL 46:9279c7a725bf 783
ThomasBNL 46:9279c7a725bf 784 // [MINIMUM VALUE BICEPS CALIBRATION] //
ThomasBNL 46:9279c7a725bf 785
ThomasBNL 46:9279c7a725bf 786
ThomasBNL 46:9279c7a725bf 787 debug_led_blue=on;
ThomasBNL 46:9279c7a725bf 788 pc.printf("Start minimum calibration in 5 seconds \n\r");
ThomasBNL 46:9279c7a725bf 789 pc.printf("Keep your biceps as relaxed as possible \n\r");
ThomasBNL 46:9279c7a725bf 790
ThomasBNL 46:9279c7a725bf 791 countdown_from_5();
ThomasBNL 46:9279c7a725bf 792 c=0;
ThomasBNL 46:9279c7a725bf 793
ThomasBNL 46:9279c7a725bf 794 while(c<2560) // 512Hz -> 2560 is equal to five seconds
ThomasBNL 46:9279c7a725bf 795 {
ThomasBNL 46:9279c7a725bf 796 Filter(); // Filter EMG signal
ThomasBNL 46:9279c7a725bf 797 minimum_L=EMG_Left_Bicep_filtered+minimum_L; // Take previous sample EMG_Left_Bicep_filtered and add the new value
ThomasBNL 46:9279c7a725bf 798 minimum_R=EMG_Right_Bicep_filtered+minimum_R;
ThomasBNL 46:9279c7a725bf 799 // scope.set(0,EMG_left_Bicep);
ThomasBNL 46:9279c7a725bf 800 // scope.set(1,EMG_Left_Bicep_filtered);
ThomasBNL 46:9279c7a725bf 801 // scope.set(2,minimum_L);
ThomasBNL 46:9279c7a725bf 802 // scope.send();
ThomasBNL 46:9279c7a725bf 803 c++; // Every sample c is increased by one until the statement c<2560 is false
ThomasBNL 46:9279c7a725bf 804 wait(0.001953125); // wait one sample
ThomasBNL 46:9279c7a725bf 805 }
ThomasBNL 46:9279c7a725bf 806
ThomasBNL 46:9279c7a725bf 807 pc.printf("Finished minimum calibration \n\r");
ThomasBNL 46:9279c7a725bf 808
ThomasBNL 46:9279c7a725bf 809 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 46:9279c7a725bf 810 EMG_R_min=minimum_R/2560;
ThomasBNL 46:9279c7a725bf 811
ThomasBNL 46:9279c7a725bf 812 pc.printf("EMG_L_min = %f \n\r EMG_R_min = %f \n\r", EMG_L_min, EMG_R_min);
ThomasBNL 46:9279c7a725bf 813
ThomasBNL 46:9279c7a725bf 814 wait (3); //cooldown
ThomasBNL 46:9279c7a725bf 815
ThomasBNL 46:9279c7a725bf 816
ThomasBNL 46:9279c7a725bf 817 // [MAXIMUM VALUE BICEPS CALIBRATION] //
ThomasBNL 46:9279c7a725bf 818
ThomasBNL 46:9279c7a725bf 819
ThomasBNL 46:9279c7a725bf 820 pc.printf("start maximum calibration in 5 seconds (start contraction at 3) \n\r");
ThomasBNL 46:9279c7a725bf 821
ThomasBNL 46:9279c7a725bf 822 countdown_from_5();
ThomasBNL 46:9279c7a725bf 823 c=0;
ThomasBNL 46:9279c7a725bf 824
ThomasBNL 46:9279c7a725bf 825 while(c<2560) // 512Hz -> 2560 is equal to five seconds
ThomasBNL 46:9279c7a725bf 826 {
ThomasBNL 46:9279c7a725bf 827 Filter(); // Filter EMG signal
ThomasBNL 46:9279c7a725bf 828 maximum_L=EMG_Left_Bicep_filtered+maximum_L; // Take previous sample EMG_Left_Bicep_filtered and add the new value
ThomasBNL 46:9279c7a725bf 829 maximum_R=EMG_Right_Bicep_filtered+maximum_R;
ThomasBNL 46:9279c7a725bf 830 // scope.set(0,EMG_left_Bicep);
ThomasBNL 46:9279c7a725bf 831 // scope.set(1,EMG_Left_Bicep_filtered);
ThomasBNL 46:9279c7a725bf 832 // scope.set(2,maximum_R);
ThomasBNL 46:9279c7a725bf 833 // scope.send();
ThomasBNL 46:9279c7a725bf 834 c++; // Every sample c is increased by one until the statement c<2560 is false
ThomasBNL 50:6060f45d343a 835 wait(0.001953125);
ThomasBNL 46:9279c7a725bf 836 }
ThomasBNL 46:9279c7a725bf 837
ThomasBNL 46:9279c7a725bf 838 pc.printf("Finished minimum calibration \n\r");
ThomasBNL 46:9279c7a725bf 839
ThomasBNL 46:9279c7a725bf 840 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 46:9279c7a725bf 841 EMG_R_max=maximum_R/2560;
ThomasBNL 46:9279c7a725bf 842
ThomasBNL 46:9279c7a725bf 843 pc.printf("EMG_L_max = %f \n\r EMG_R_max = %f \n\r", EMG_L_max, EMG_R_max);
ThomasBNL 46:9279c7a725bf 844
ThomasBNL 46:9279c7a725bf 845 wait (3); //cooldown
ThomasBNL 46:9279c7a725bf 846
ThomasBNL 46:9279c7a725bf 847 debug_led_blue=off;
ThomasBNL 46:9279c7a725bf 848
ThomasBNL 46:9279c7a725bf 849
ThomasBNL 46:9279c7a725bf 850 // [MAXIMUM VALUE BICEPS CALIBRATION] //
ThomasBNL 46:9279c7a725bf 851 // Calculate threshold percentages //
ThomasBNL 46:9279c7a725bf 852
ThomasBNL 46:9279c7a725bf 853 const float 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 46:9279c7a725bf 854 const float 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 46:9279c7a725bf 855 const float 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 46:9279c7a725bf 856 const float 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 46:9279c7a725bf 857
ThomasBNL 46:9279c7a725bf 858 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 46:9279c7a725bf 859
ThomasBNL 46:9279c7a725bf 860 }
ThomasBNL 46:9279c7a725bf 861
ThomasBNL 46:9279c7a725bf 862 // [COUNTDOWN FUNCTION] //
ThomasBNL 46:9279c7a725bf 863
ThomasBNL 46:9279c7a725bf 864
ThomasBNL 46:9279c7a725bf 865 void countdown_from_5() // Countdown from 5 till 0 inside Putty (interface)
ThomasBNL 46:9279c7a725bf 866 {
ThomasBNL 46:9279c7a725bf 867 wait(1); pc.printf("5 \n\r"); wait(1); pc.printf("4 \n\r"); wait(1); pc.printf("3 \n\r"); wait(1); pc.printf("2 Ready \n\r");
ThomasBNL 46:9279c7a725bf 868 wait(1); pc.printf("1 Set \n\r"); wait(1); pc.printf("Go \n\r");
ThomasBNL 46:9279c7a725bf 869 }
ThomasBNL 46:9279c7a725bf 870
ThomasBNL 46:9279c7a725bf 871
ThomasBNL 46:9279c7a725bf 872 // [FLOW CONTROL FUNCTIONS] //
ThomasBNL 46:9279c7a725bf 873
ThomasBNL 46:9279c7a725bf 874
ThomasBNL 46:9279c7a725bf 875 void ControlGo() //Control flag
ThomasBNL 46:9279c7a725bf 876 {
ThomasBNL 46:9279c7a725bf 877 control_go = true;
ThomasBNL 46:9279c7a725bf 878 }
ThomasBNL 46:9279c7a725bf 879
ThomasBNL 46:9279c7a725bf 880 void take_sample() // Take a sample every 25th sample
ThomasBNL 46:9279c7a725bf 881 {
ThomasBNL 46:9279c7a725bf 882 if(n==25)
ThomasBNL 46:9279c7a725bf 883 {
ThomasBNL 64:97e2db3eb0eb 884 sample = true; n=0;
ThomasBNL 46:9279c7a725bf 885 }
ThomasBNL 55:f4ab878ae910 886
ThomasBNL 57:8f3603cc2e71 887 if(er==5)
ThomasBNL 55:f4ab878ae910 888 {
ThomasBNL 64:97e2db3eb0eb 889 sample_error = true; er=0;
ThomasBNL 55:f4ab878ae910 890 }
ThomasBNL 64:97e2db3eb0eb 891
ThomasBNL 64:97e2db3eb0eb 892 sample_error_strike = true;
ThomasBNL 46:9279c7a725bf 893 }
ThomasBNL 46:9279c7a725bf 894
ThomasBNL 46:9279c7a725bf 895 // [FILTER FUNCTIONS] //
ThomasBNL 46:9279c7a725bf 896 // [EMG] //
ThomasBNL 46:9279c7a725bf 897
ThomasBNL 46:9279c7a725bf 898 void Filter() // Unfiltered EMG (input) -> highpass filter -> rectify -> lowpass filter -> Filtered EMG (output)
ThomasBNL 46:9279c7a725bf 899 {
ThomasBNL 46:9279c7a725bf 900 EMG_left_Bicep = input1; EMG_Right_Bicep = input2;
ThomasBNL 46:9279c7a725bf 901
ThomasBNL 46:9279c7a725bf 902 EMG_Left_Bicep_filtered = highpassfilter_1.step(EMG_left_Bicep); EMG_Right_Bicep_filtered = highpassfilter_2.step(EMG_Right_Bicep);
ThomasBNL 46:9279c7a725bf 903 EMG_Left_Bicep_filtered = fabs(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = fabs(EMG_Right_Bicep_filtered);
ThomasBNL 46:9279c7a725bf 904 EMG_Left_Bicep_filtered = lowpassfilter_1.step(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = lowpassfilter_2.step(EMG_Right_Bicep_filtered);
ThomasBNL 46:9279c7a725bf 905 }
ThomasBNL 46:9279c7a725bf 906
ThomasBNL 46:9279c7a725bf 907 // [FILTER FUNCTIONS] //
ThomasBNL 46:9279c7a725bf 908 // [Include Moving Average] //
ThomasBNL 46:9279c7a725bf 909
ThomasBNL 46:9279c7a725bf 910 void sample_filter()
ThomasBNL 46:9279c7a725bf 911 {
ThomasBNL 46:9279c7a725bf 912 Filter();
ThomasBNL 46:9279c7a725bf 913 take_sample();
ThomasBNL 46:9279c7a725bf 914 if(sample)
ThomasBNL 46:9279c7a725bf 915 {
ThomasBNL 46:9279c7a725bf 916 sample=false;
ThomasBNL 46:9279c7a725bf 917 Sample_EMG_L_1 = EMG_Left_Bicep_filtered; Sample_EMG_R_1 = EMG_Right_Bicep_filtered;
ThomasBNL 46:9279c7a725bf 918
ThomasBNL 46:9279c7a725bf 919 Sample_EMG_L_10= Sample_EMG_L_9; Sample_EMG_R_10= Sample_EMG_R_9;
ThomasBNL 46:9279c7a725bf 920 Sample_EMG_L_9 = Sample_EMG_L_8; Sample_EMG_R_9 = Sample_EMG_R_8;
ThomasBNL 46:9279c7a725bf 921 Sample_EMG_L_8 = Sample_EMG_L_7; Sample_EMG_R_8 = Sample_EMG_R_7;
ThomasBNL 46:9279c7a725bf 922 Sample_EMG_L_7 = Sample_EMG_L_6; Sample_EMG_R_7 = Sample_EMG_R_6;
ThomasBNL 46:9279c7a725bf 923 Sample_EMG_L_6 = Sample_EMG_L_5; Sample_EMG_R_6 = Sample_EMG_R_5;
ThomasBNL 46:9279c7a725bf 924 Sample_EMG_L_5 = Sample_EMG_L_4; Sample_EMG_R_5 = Sample_EMG_R_4;
ThomasBNL 46:9279c7a725bf 925 Sample_EMG_L_4 = Sample_EMG_L_3; Sample_EMG_R_4 = Sample_EMG_R_3;
ThomasBNL 46:9279c7a725bf 926 Sample_EMG_L_3 = Sample_EMG_L_2; Sample_EMG_R_3 = Sample_EMG_R_2;
ThomasBNL 46:9279c7a725bf 927 Sample_EMG_L_2 = Sample_EMG_L_1; Sample_EMG_R_2 = Sample_EMG_R_1;
ThomasBNL 46:9279c7a725bf 928 }
ThomasBNL 46:9279c7a725bf 929 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 930 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 931 n++;
ThomasBNL 46:9279c7a725bf 932 }
ThomasBNL 46:9279c7a725bf 933
ThomasBNL 47:c61873a0b646 934 //============================================================================================================================
ThomasBNL 47:c61873a0b646 935 // ___________________________
ThomasBNL 47:c61873a0b646 936 // // \\
ThomasBNL 47:c61873a0b646 937 // || [Action Controller] ||
ThomasBNL 47:c61873a0b646 938 // \\___________________________//
ThomasBNL 55:f4ab878ae910 939 //
ThomasBNL 55:f4ab878ae910 940 void red() { debug_led_red=on; debug_led_blue=off; debug_led_green=off; }
ThomasBNL 55:f4ab878ae910 941 void blue() { debug_led_red=off; debug_led_blue=on; debug_led_green=off; }
ThomasBNL 55:f4ab878ae910 942 void green() { debug_led_red=off; debug_led_blue=off; debug_led_green=on; }
ThomasBNL 55:f4ab878ae910 943 void white() { debug_led_red=on; debug_led_blue=on; debug_led_green=on; }
ThomasBNL 55:f4ab878ae910 944 void yellow() { debug_led_red=on; debug_led_blue=off; debug_led_green=on; }
ThomasBNL 55:f4ab878ae910 945 void cyan() { debug_led_red=off; debug_led_blue=on; debug_led_green=on; }
ThomasBNL 55:f4ab878ae910 946 void purple() { debug_led_red=on; debug_led_blue=on; debug_led_green=off; }
ThomasBNL 55:f4ab878ae910 947 void black() { debug_led_red=off; debug_led_blue=off; debug_led_green=off; }