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@48:950b1f34161b, 2015-10-15 (annotated)

Committer:
ThomasBNL
Date:
Thu Oct 15 11:03:53 2015 +0000
Revision:
48:950b1f34161b
Parent:
47:c61873a0b646
Child:
49:a8a68abf814f
Calibration turn motor set position works (line 403)

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 48:950b1f34161b 74 // Attach Ticker..............................................
ThomasBNL 47:c61873a0b646 75 // Start infinite loop........................................
ThomasBNL 47:c61873a0b646 76 // Debug buttons........................................
ThomasBNL 47:c61873a0b646 77 // Wait for go signal...................................
ThomasBNL 47:c61873a0b646 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 46:9279c7a725bf 122 // _________________________
ThomasBNL 46:9279c7a725bf 123 // : [LEDS] :
ThomasBNL 46:9279c7a725bf 124 // :__________________________:
ThomasBNL 34:c672f5c0763f 125 //
ThomasBNL 34:c672f5c0763f 126 DigitalOut debug_led_red(LED_RED); // Debug LED
ThomasBNL 34:c672f5c0763f 127 DigitalOut debug_led_green(LED_GREEN); // Debug LED
ThomasBNL 34:c672f5c0763f 128 DigitalOut debug_led_blue(LED_BLUE); // Debug LED
ThomasBNL 46:9279c7a725bf 129 // __________________________
ThomasBNL 46:9279c7a725bf 130 // : [Buttons] :
ThomasBNL 46:9279c7a725bf 131 // :__________________________:
ThomasBNL 46:9279c7a725bf 132 //
ThomasBNL 43:fb69ef657f30 133 DigitalIn buttonL1(PTC6); // Button 1 (low on k64f) for testing at the lower board
ThomasBNL 43:fb69ef657f30 134 DigitalIn buttonL2(PTA4); // Button 2 (low on k64f) for testing at the lower board
ThomasBNL 43:fb69ef657f30 135 DigitalIn buttonH1(D2); // Button 3 (high on k64f) for testing at the top board
ThomasBNL 46:9279c7a725bf 136 DigitalIn buttonH2(D6); // Button 4 (high on k64f) for testing at the top board
ThomasBNL 46:9279c7a725bf 137
ThomasBNL 46:9279c7a725bf 138 // __________________________
ThomasBNL 46:9279c7a725bf 139 // : [Potmeter] :
ThomasBNL 46:9279c7a725bf 140 // :__________________________:
ThomasBNL 46:9279c7a725bf 141 // DEBUGGING / TESTING
ThomasBNL 48:950b1f34161b 142 //moving_average_right = (input1.read())*100; // EMG Right bicep (tussen nul en 100%)
ThomasBNL 48:950b1f34161b 143 //moving_average_left = (input2.read())*100; // EMG Left bicep (tussen nul en 100%)
ThomasBNL 34:c672f5c0763f 144
ThomasBNL 46:9279c7a725bf 145 // __________________________
ThomasBNL 46:9279c7a725bf 146 // : [FLOW] :
ThomasBNL 46:9279c7a725bf 147 // :__________________________:
ThomasBNL 46:9279c7a725bf 148 //
ThomasBNL 46:9279c7a725bf 149 Ticker looptimer; // Ticker called looptimer to set a looptimerflag
ThomasBNL 34:c672f5c0763f 150
ThomasBNL 46:9279c7a725bf 151 // __________________________
ThomasBNL 46:9279c7a725bf 152 // : [HIDSCOPE] :
ThomasBNL 46:9279c7a725bf 153 // :__________________________:
ThomasBNL 46:9279c7a725bf 154 //
ThomasBNL 46:9279c7a725bf 155
ThomasBNL 48:950b1f34161b 156 //HIDScope scope(3); // HIDSCOPE declared
ThomasBNL 46:9279c7a725bf 157 MODSERIAL pc(USBTX,USBRX);
ThomasBNL 46:9279c7a725bf 158
ThomasBNL 46:9279c7a725bf 159 //============================================================================================================================
ThomasBNL 44:5dd0a3d24662 160
ThomasBNL 34:c672f5c0763f 161 // ___________________________
ThomasBNL 34:c672f5c0763f 162 // // \\
ThomasBNL 46:9279c7a725bf 163 // || [EMG] ||
ThomasBNL 34:c672f5c0763f 164 // \\___________________________//
ThomasBNL 34:c672f5c0763f 165 //
ThomasBNL 34:c672f5c0763f 166
ThomasBNL 46:9279c7a725bf 167 AnalogIn input1(A0); // EMG: Two AnalogIn EMG inputs
ThomasBNL 46:9279c7a725bf 168 AnalogIn input2(A1); // EMG: Two AnalogIn EMG inputs
ThomasBNL 46:9279c7a725bf 169
ThomasBNL 46:9279c7a725bf 170 // __________________________
ThomasBNL 46:9279c7a725bf 171 // : [EMG variables] :
ThomasBNL 46:9279c7a725bf 172 // :__________________________:
ThomasBNL 46:9279c7a725bf 173 //
ThomasBNL 46:9279c7a725bf 174
ThomasBNL 46:9279c7a725bf 175 volatile bool control_go = false; // EMG:
ThomasBNL 46:9279c7a725bf 176 volatile bool sample = false; // EMG:
ThomasBNL 46:9279c7a725bf 177 //
ThomasBNL 46:9279c7a725bf 178 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, Sample_EMG_L_7, Sample_EMG_L_8, Sample_EMG_L_9, Sample_EMG_L_10, moving_average_left; // EMG:
ThomasBNL 46:9279c7a725bf 179 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, Sample_EMG_R_7, Sample_EMG_R_8, Sample_EMG_R_9, Sample_EMG_R_10, moving_average_right; // EMG:
ThomasBNL 46:9279c7a725bf 180 double minimum_L; // EMG:
ThomasBNL 46:9279c7a725bf 181 double maximum_L; // EMG:
ThomasBNL 46:9279c7a725bf 182 double EMG_L_min; // EMG:
ThomasBNL 46:9279c7a725bf 183 double EMG_L_max; // EMG:
ThomasBNL 46:9279c7a725bf 184 double minimum_R; // EMG:
ThomasBNL 46:9279c7a725bf 185 double maximum_R; // EMG:
ThomasBNL 46:9279c7a725bf 186 double EMG_R_min; // EMG:
ThomasBNL 46:9279c7a725bf 187 double EMG_R_max; // EMG:
ThomasBNL 46:9279c7a725bf 188 double n=0; // EMG:
ThomasBNL 46:9279c7a725bf 189 double c=0; // EMG:
ThomasBNL 46:9279c7a725bf 190
ThomasBNL 46:9279c7a725bf 191 // __________________________
ThomasBNL 46:9279c7a725bf 192 // : [EMG Filter] :
ThomasBNL 46:9279c7a725bf 193 // :__________________________:
ThomasBNL 46:9279c7a725bf 194 // Biquad filters
ThomasBNL 46:9279c7a725bf 195
ThomasBNL 46:9279c7a725bf 196 const double low_b0 = 0.05892937945281792;
ThomasBNL 46:9279c7a725bf 197 const double low_b1 = 0.11785875890563584;
ThomasBNL 46:9279c7a725bf 198 const double low_b2 = 0.05892937945281792;
ThomasBNL 46:9279c7a725bf 199 const double low_a1 = -1.205716572226748;
ThomasBNL 46:9279c7a725bf 200 const double low_a2 = 0.44143409003801976; // VIA online biquad calculator Lowpas 520-48-0.7071-6
ThomasBNL 46:9279c7a725bf 201
ThomasBNL 46:9279c7a725bf 202 const double high_b0 = 0.6389437261127494;
ThomasBNL 46:9279c7a725bf 203 const double high_b1 = -1.2778874522254988;
ThomasBNL 46:9279c7a725bf 204 const double high_b2 = 0.6389437261127494;
ThomasBNL 46:9279c7a725bf 205 const double high_a1 = -1.1429772843080923;
ThomasBNL 46:9279c7a725bf 206 const double high_a2 = 0.41279762014290533; // VIA online biquad calculator Highpas 520-52-0.7071-6
ThomasBNL 46:9279c7a725bf 207
ThomasBNL 46:9279c7a725bf 208 biquadFilter highpassfilter_1(high_a1, high_a2, high_b0, high_b1, high_b2);
ThomasBNL 46:9279c7a725bf 209 biquadFilter highpassfilter_2(high_a1, high_a2, high_b0, high_b1, high_b2);
ThomasBNL 46:9279c7a725bf 210 biquadFilter lowpassfilter_1(low_a1, low_a2, low_b0, low_b1, low_b2);
ThomasBNL 46:9279c7a725bf 211 biquadFilter lowpassfilter_2(low_a1, low_a2, low_b0, low_b1, low_b2);
ThomasBNL 46:9279c7a725bf 212
ThomasBNL 46:9279c7a725bf 213 double EMG_left_Bicep; double EMG_Right_Bicep; // input variables
ThomasBNL 46:9279c7a725bf 214 double EMG_Left_Bicep_filtered; double EMG_Right_Bicep_filtered; // output variables
ThomasBNL 46:9279c7a725bf 215
ThomasBNL 46:9279c7a725bf 216 //============================================================================================================================
ThomasBNL 46:9279c7a725bf 217 // ___________________________
ThomasBNL 46:9279c7a725bf 218 // // \\
ThomasBNL 46:9279c7a725bf 219 // || [MOTORS] ||
ThomasBNL 46:9279c7a725bf 220 // \\___________________________//
ThomasBNL 46:9279c7a725bf 221 //
ThomasBNL 46:9279c7a725bf 222 // __________________________
ThomasBNL 46:9279c7a725bf 223 // : [Motor TURN] :
ThomasBNL 46:9279c7a725bf 224 // :__________________________:
ThomasBNL 46:9279c7a725bf 225 //
ThomasBNL 46:9279c7a725bf 226
ThomasBNL 43:fb69ef657f30 227 QEI motor_turn(D12,D13,NC,32); // TURN: Encoder for motor
ThomasBNL 43:fb69ef657f30 228 PwmOut pwm_motor_turn(D5); // TURN: Pwm for motor
ThomasBNL 43:fb69ef657f30 229 DigitalOut motordirection_turn(D4); // TURN: Direction of the motor
ThomasBNL 43:fb69ef657f30 230
ThomasBNL 43:fb69ef657f30 231 // PID motor constants
ThomasBNL 43:fb69ef657f30 232
ThomasBNL 46:9279c7a725bf 233 double integrate_error_turn=0; // TURN: integration error turn motor
ThomasBNL 46:9279c7a725bf 234 double previous_error_turn=0; // TURN: previous error turn motor
ThomasBNL 43:fb69ef657f30 235
ThomasBNL 46:9279c7a725bf 236 double position_turn; // TURN: Set variable to store current position of the motor
ThomasBNL 46:9279c7a725bf 237 double error_turn; // TURN: Set variable to store the current error of the motor
ThomasBNL 46:9279c7a725bf 238
ThomasBNL 46:9279c7a725bf 239 double pwm_motor_turn_P; // TURN: variable that stores the P action part of the error
ThomasBNL 46:9279c7a725bf 240 double pwm_motor_turn_I; // TURN: variable that stores the I action part of the error
ThomasBNL 46:9279c7a725bf 241 double pwm_motor_turn_D; // TURN: variable that stores the D action part of the error
ThomasBNL 46:9279c7a725bf 242 double pwm_to_motor_turn; // TURN: variable that is constructed by summing the values of the P, I and D action
ThomasBNL 46:9279c7a725bf 243
ThomasBNL 46:9279c7a725bf 244 double P_gain_turn; // TURN: this gain gets multiplied with the error inside the P action of the PID controller
ThomasBNL 46:9279c7a725bf 245 double I_gain_turn; // TURN: this gain gets multiplied with the error inside the I action of the PID controller
ThomasBNL 46:9279c7a725bf 246 double D_gain_turn; // TURN: this gain gets multiplied with the error inside the D action of the PID controller
ThomasBNL 46:9279c7a725bf 247
ThomasBNL 46:9279c7a725bf 248 double reference_turn; // TURN: reference position of the motor (position the motor 'likes' to get to)
ThomasBNL 46:9279c7a725bf 249
ThomasBNL 46:9279c7a725bf 250
ThomasBNL 46:9279c7a725bf 251 // __________________________
ThomasBNL 46:9279c7a725bf 252 // : [Motor STRIKE] :
ThomasBNL 46:9279c7a725bf 253 // :__________________________:
ThomasBNL 46:9279c7a725bf 254 //
ThomasBNL 43:fb69ef657f30 255
ThomasBNL 43:fb69ef657f30 256 //QEI motor_strike(XX,XX,NC,32); // STRIKE: Encoder for motor Turn
ThomasBNL 43:fb69ef657f30 257 //PwmOut pwm_motor_strike(XX); // STRIKE: Pwm for motor Turn
ThomasBNL 43:fb69ef657f30 258 //DigitalOut motordirection_strike(XX); // STRIKE: Direction of the motor Turn
ThomasBNL 43:fb69ef657f30 259
ThomasBNL 43:fb69ef657f30 260 // PID motor constants
ThomasBNL 46:9279c7a725bf 261
ThomasBNL 46:9279c7a725bf 262 //double integrate_error_strike=0; // STRIKE: integration error turn motor
ThomasBNL 46:9279c7a725bf 263 //double previous_error_strike=0; // STRIKE: previous error turn motor
ThomasBNL 46:9279c7a725bf 264
ThomasBNL 46:9279c7a725bf 265 //double position_strike; // STRIKE: Set variable to store current position of the motor
ThomasBNL 46:9279c7a725bf 266 //double error_strike; // STRIKE: Set variable to store the current error of the motor
ThomasBNL 34:c672f5c0763f 267
ThomasBNL 46:9279c7a725bf 268 //double pwm_motor_strike_P; // STRIKE: variable that stores the P action part of the error
ThomasBNL 46:9279c7a725bf 269 //double pwm_motor_strike_I; // STRIKE: variable that stores the I action part of the error
ThomasBNL 46:9279c7a725bf 270 //double pwm_motor_strike_D; // STRIKE: variable that stores the D action part of the error
ThomasBNL 46:9279c7a725bf 271 //double pwm_to_motor_strike; // STRIKE: variable that is constructed by summing the values of the P, I and D action
ThomasBNL 46:9279c7a725bf 272 //double reference_strike;
ThomasBNL 46:9279c7a725bf 273
ThomasBNL 46:9279c7a725bf 274 //double P_gain_turn; // STRIKE: this gain gets multiplied with the error inside the P action of the PID controller
ThomasBNL 46:9279c7a725bf 275 //double I_gain_turn; // STRIKE: this gain gets multiplied with the error inside the I action of the PID controller
ThomasBNL 46:9279c7a725bf 276 //double D_gain_turn; // STRIKE: this gain gets multiplied with the error inside the D action of the PID controller
ThomasBNL 46:9279c7a725bf 277
ThomasBNL 46:9279c7a725bf 278 //double reference_strike; // STRIKE: reference position of the motor (position the motor 'likes' to get to)
ThomasBNL 46:9279c7a725bf 279
ThomasBNL 46:9279c7a725bf 280 // __________________________
ThomasBNL 46:9279c7a725bf 281 // : [D-action filter] :
ThomasBNL 46:9279c7a725bf 282 // :__________________________:
ThomasBNL 46:9279c7a725bf 283 // Applicable for both motors
ThomasBNL 46:9279c7a725bf 284
ThomasBNL 46:9279c7a725bf 285 const double mT_f_a1=-1.965293372622690e+00; // Motor Turn derivative filter constants
ThomasBNL 46:9279c7a725bf 286 const double mT_f_a2=9.658854605688177e-01;
ThomasBNL 46:9279c7a725bf 287 const double mT_f_b0=1.480219865318266e-04;
ThomasBNL 46:9279c7a725bf 288 const double mT_f_b1=2.960439730636533e-04;
ThomasBNL 46:9279c7a725bf 289 const double mT_f_b2=1.480219865318266e-04;
ThomasBNL 46:9279c7a725bf 290
ThomasBNL 46:9279c7a725bf 291 biquadFilter Lowpassfilter_derivative(mT_f_a1,mT_f_a2,mT_f_b0,mT_f_b1,mT_f_b2); // BiquadFilter used for the filtering of the Derivative action of the PID-action
ThomasBNL 46:9279c7a725bf 292
ThomasBNL 46:9279c7a725bf 293
ThomasBNL 46:9279c7a725bf 294 //============================================================================================================================
ThomasBNL 34:c672f5c0763f 295 // ___________________________
ThomasBNL 34:c672f5c0763f 296 // // \\
ThomasBNL 46:9279c7a725bf 297 // || [SYSTEM CONSTANTS] ||
ThomasBNL 34:c672f5c0763f 298 // \\___________________________//
ThomasBNL 34:c672f5c0763f 299 //
ThomasBNL 0:40052f5ca77b 300
ThomasBNL 0:40052f5ca77b 301 volatile bool looptimerflag;
ThomasBNL 43:fb69ef657f30 302 const double cw=0; // zero is clockwise (front view)
ThomasBNL 43:fb69ef657f30 303 const double ccw=1; // one is counterclockwise (front view)
ThomasBNL 43:fb69ef657f30 304 const double off=1; // Led off
ThomasBNL 43:fb69ef657f30 305 const double on=0; // Led on
ThomasBNL 43:fb69ef657f30 306 const int Fs = 512; // sampling frequency (512 Hz)
ThomasBNL 43:fb69ef657f30 307 const double sample_time = 0.001953125; // duration of one sample
ThomasBNL 44:5dd0a3d24662 308 double conversion_counts_to_degrees=0.085877862594198; // Calculation conversion counts to degrees
ThomasBNL 44:5dd0a3d24662 309 // gear ratio motor = 131
ThomasBNL 44:5dd0a3d24662 310 // ticks per magnet rotation = 32 (X2 Encoder)
ThomasBNL 44:5dd0a3d24662 311 // One revolution = 360 degrees
ThomasBNL 44:5dd0a3d24662 312 // degrees_per_encoder_tick = 360/(gear_ratio*ticks_per_magnet_rotation)=360/131*32=0.085877862594198
ThomasBNL 46:9279c7a725bf 313 const double change_one_bottle=45;
ThomasBNL 46:9279c7a725bf 314
ThomasBNL 46:9279c7a725bf 315 //============================================================================================================================
ThomasBNL 34:c672f5c0763f 316 // ___________________________
ThomasBNL 34:c672f5c0763f 317 // // \\
ThomasBNL 34:c672f5c0763f 318 // || [FUNCTIONS USED] ||
ThomasBNL 34:c672f5c0763f 319 // \\___________________________//
ThomasBNL 46:9279c7a725bf 320 // __________________________
ThomasBNL 46:9279c7a725bf 321 // : [Motor Functions] :
ThomasBNL 46:9279c7a725bf 322 // :__________________________:
ThomasBNL 46:9279c7a725bf 323 // Applicable for both motors
ThomasBNL 46:9279c7a725bf 324
ThomasBNL 46:9279c7a725bf 325 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 326 void setlooptimerflag (void); // Sets looptimerflag volatile bool to true
ThomasBNL 8:50d6e2323d3b 327
ThomasBNL 46:9279c7a725bf 328 void deactivate_PID_Controller (double& P_gain, double& I_gain, double& D_gain); // STRIKE/TURN: Deactivate PID Controller
ThomasBNL 46:9279c7a725bf 329
ThomasBNL 46:9279c7a725bf 330 void activate_PID_Controller_strike (double& P_gain, double& I_gain, double& D_gain); // STRIKE: Activate PID Controller
ThomasBNL 46:9279c7a725bf 331
ThomasBNL 46:9279c7a725bf 332 void activate_PID_Controller_turn (double& P_gain, double& I_gain, double& D_gain); // TURN: Activate PID Controller
ThomasBNL 46:9279c7a725bf 333 void Change_Turn_Position_Left (double& reference, double change_one_bottle); // TURN: Change Reference position one bottle to the left
ThomasBNL 46:9279c7a725bf 334 void Change_Turn_Position_Right (double& reference, double change_one_bottle); // TURN: Change Reference position one bottle to the right
ThomasBNL 39:104a038f7b92 335
ThomasBNL 46:9279c7a725bf 336 // __________________________
ThomasBNL 46:9279c7a725bf 337 // : [EMG Functions] :
ThomasBNL 46:9279c7a725bf 338 // :__________________________:
ThomasBNL 46:9279c7a725bf 339 // Applicable for both biceps
ThomasBNL 8:50d6e2323d3b 340
ThomasBNL 46:9279c7a725bf 341 void calibration(); // EMG: Calibrate the EMG signal (calculate min and max signal and determine threshold values)
ThomasBNL 46:9279c7a725bf 342 void countdown_from_5(); // PUTTY: 5 second countdown inside
ThomasBNL 46:9279c7a725bf 343 void Filter(); // EMG: Filter the incoming EMG signals
ThomasBNL 46:9279c7a725bf 344 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 345 void take_sample(); // EMG: Take a sample once every 25 samples that's used to calculate the moving average
ThomasBNL 46:9279c7a725bf 346 void ControlGo(); // EMG: function that gets a ticker attached and sets a certain loop to true every sample
ThomasBNL 46:9279c7a725bf 347
ThomasBNL 46:9279c7a725bf 348 // __________________________
ThomasBNL 46:9279c7a725bf 349 // : [Action Controller] :
ThomasBNL 46:9279c7a725bf 350 // :__________________________:
ThomasBNL 46:9279c7a725bf 351 //
ThomasBNL 0:40052f5ca77b 352
ThomasBNL 39:104a038f7b92 353
ThomasBNL 46:9279c7a725bf 354 //============================================================================================================================
ThomasBNL 46:9279c7a725bf 355 ///////////////////////////////
ThomasBNL 46:9279c7a725bf 356 // //
ThomasBNL 46:9279c7a725bf 357 ///////////////////////////////////////////// [MAIN FUNCTION] /////////////////////////////////////////////////////
ThomasBNL 46:9279c7a725bf 358 // //
ThomasBNL 46:9279c7a725bf 359 ///////////////////////////////
ThomasBNL 46:9279c7a725bf 360 //============================================================================================================================
ThomasBNL 0:40052f5ca77b 361 int main() {
ThomasBNL 43:fb69ef657f30 362
ThomasBNL 36:da07b5c2984d 363 debug_led_red=off;
ThomasBNL 36:da07b5c2984d 364 debug_led_blue=off;
ThomasBNL 36:da07b5c2984d 365 debug_led_green=off;
ThomasBNL 46:9279c7a725bf 366
ThomasBNL 43:fb69ef657f30 367 // ___________________________
ThomasBNL 43:fb69ef657f30 368 // // \\
ThomasBNL 43:fb69ef657f30 369 // || [START OF CODE] ||
ThomasBNL 43:fb69ef657f30 370 // \\___________________________//
ThomasBNL 43:fb69ef657f30 371 // START OF CODE
ThomasBNL 43:fb69ef657f30 372 pc.printf("Start of code \n\r");
ThomasBNL 43:fb69ef657f30 373
ThomasBNL 43:fb69ef657f30 374 pc.baud(115200); // Set the baudrate
ThomasBNL 0:40052f5ca77b 375
ThomasBNL 43:fb69ef657f30 376 // ___________________________
ThomasBNL 43:fb69ef657f30 377 // // \\
ThomasBNL 43:fb69ef657f30 378 // || [CALIBRATE] ||
ThomasBNL 43:fb69ef657f30 379 // \\___________________________//
ThomasBNL 43:fb69ef657f30 380 // Calibrate starting postion (RED LED)
ThomasBNL 43:fb69ef657f30 381
ThomasBNL 46:9279c7a725bf 382 // ___________________________
ThomasBNL 46:9279c7a725bf 383 // : [Starting position motor] :
ThomasBNL 46:9279c7a725bf 384 // :___________________________:
ThomasBNL 46:9279c7a725bf 385 //
ThomasBNL 46:9279c7a725bf 386
ThomasBNL 43:fb69ef657f30 387 pc.printf("Button calibration \n\r");
ThomasBNL 43:fb69ef657f30 388 while(1)
ThomasBNL 43:fb69ef657f30 389 {
ThomasBNL 43:fb69ef657f30 390 debug_led_red=on; // TURN: LOW buttons
ThomasBNL 43:fb69ef657f30 391
ThomasBNL 43:fb69ef657f30 392 if (buttonL2.read() < 0.5){ // TURN: turn the motor clockwise with pwm of 0.2
ThomasBNL 43:fb69ef657f30 393 motordirection_turn = cw;
ThomasBNL 48:950b1f34161b 394 pwm_motor_turn = 0.02;
ThomasBNL 48:950b1f34161b 395 pc.printf("cw calibration \n\r");}
ThomasBNL 43:fb69ef657f30 396
ThomasBNL 43:fb69ef657f30 397 if (buttonL1.read() < 0.5){ // TURN: turn the motor counterclockwise with pwm of 0.2
ThomasBNL 43:fb69ef657f30 398 motordirection_turn = ccw;
ThomasBNL 48:950b1f34161b 399 pwm_motor_turn = 0.02;
ThomasBNL 48:950b1f34161b 400 pc.printf("ccw calibration \n\r");}
ThomasBNL 48:950b1f34161b 401
ThomasBNL 48:950b1f34161b 402 pwm_motor_turn = 0;
ThomasBNL 43:fb69ef657f30 403 // STRIKE: HIGH buttons
ThomasBNL 43:fb69ef657f30 404
ThomasBNL 44:5dd0a3d24662 405 // if (buttonH2.read() < 0.5){ // STRIKE: turn the motor clockwise with pwm of 0.2
ThomasBNL 44:5dd0a3d24662 406 // motordirection_strike = cw;
ThomasBNL 48:950b1f34161b 407 // pwm_motor_strike = 0.02;}
ThomasBNL 44:5dd0a3d24662 408 //
ThomasBNL 44:5dd0a3d24662 409 // if (buttonH1.read() < 0.5){ // STRIKE: turn the motor clockwise with pwm of 0.2
ThomasBNL 44:5dd0a3d24662 410 // motordirection_strike = ccw;
ThomasBNL 48:950b1f34161b 411 // pwm_motor_strike = 0.02;}
ThomasBNL 48:950b1f34161b 412
ThomasBNL 48:950b1f34161b 413 // pwm_motor_strike = 0;
ThomasBNL 44:5dd0a3d24662 414 //
ThomasBNL 45:359df0594588 415 if ((buttonL2.read() < 0.5) && (buttonL1.read() < 0.5)) // Save current TURN and STRIKE positions as starting position
ThomasBNL 43:fb69ef657f30 416 {
ThomasBNL 46:9279c7a725bf 417 motor_turn.reset(); // TURN: Starting Position
ThomasBNL 46:9279c7a725bf 418 reference_turn=0; // TURN: Set reference position to zero
ThomasBNL 46:9279c7a725bf 419 // motor_strike.reset(); // STRIKE: Starting Position
ThomasBNL 46:9279c7a725bf 420 // double reference_strike=0; // STRIKE: Set reference position to zero
ThomasBNL 46:9279c7a725bf 421 goto calibration_starting_position_complete; // Calibration complete
ThomasBNL 43:fb69ef657f30 422 }
ThomasBNL 43:fb69ef657f30 423 }
ThomasBNL 43:fb69ef657f30 424
ThomasBNL 46:9279c7a725bf 425 calibration_starting_position_complete:
ThomasBNL 44:5dd0a3d24662 426 debug_led_red=off; // Calibration end => RED LED off
ThomasBNL 40:bbe7922723df 427
ThomasBNL 46:9279c7a725bf 428 // ___________________________
ThomasBNL 46:9279c7a725bf 429 // : [EMG calibration] :
ThomasBNL 46:9279c7a725bf 430 // :___________________________:
ThomasBNL 46:9279c7a725bf 431 //
ThomasBNL 46:9279c7a725bf 432 calibration(); // EMG: Calibration
ThomasBNL 46:9279c7a725bf 433
ThomasBNL 45:359df0594588 434 // ___________________________
ThomasBNL 45:359df0594588 435 // // \\
ThomasBNL 45:359df0594588 436 // || [ATTACH TICKER] ||
ThomasBNL 45:359df0594588 437 // \\___________________________//
ThomasBNL 45:359df0594588 438 // Attach Ticker to looptimerflag
ThomasBNL 45:359df0594588 439
ThomasBNL 46:9279c7a725bf 440 // EMG_ticker.attach(&ControlGo, (float)1/Fs); // EMG: Ticker
ThomasBNL 45:359df0594588 441 looptimer.attach(setlooptimerflag,(float)1/Fs); // calls the looptimer flag every sample
ThomasBNL 7:ddd7fb357786 442 pc.printf("Start infinite loop \n\r");
ThomasBNL 45:359df0594588 443
ThomasBNL 45:359df0594588 444 wait (5); // Wait 5 seconds before starting system
ThomasBNL 0:40052f5ca77b 445
ThomasBNL 44:5dd0a3d24662 446 activate_PID_Controller_strike(P_gain_turn, I_gain_turn, D_gain_turn);
ThomasBNL 34:c672f5c0763f 447
ThomasBNL 46:9279c7a725bf 448
ThomasBNL 46:9279c7a725bf 449
ThomasBNL 44:5dd0a3d24662 450 // ___________________________
ThomasBNL 44:5dd0a3d24662 451 // // \\
ThomasBNL 44:5dd0a3d24662 452 // || [START INFINTTE LOOP] ||
ThomasBNL 44:5dd0a3d24662 453 // \\___________________________//
ThomasBNL 44:5dd0a3d24662 454 //
ThomasBNL 44:5dd0a3d24662 455
ThomasBNL 5:8fb74a22fe3c 456 while(1)
ThomasBNL 8:50d6e2323d3b 457 { // Start while loop
ThomasBNL 34:c672f5c0763f 458
ThomasBNL 34:c672f5c0763f 459 // ___________________________
ThomasBNL 34:c672f5c0763f 460 // // \\
ThomasBNL 34:c672f5c0763f 461 // || [DEBUG BUTTONS] ||
ThomasBNL 34:c672f5c0763f 462 // \\___________________________//
ThomasBNL 34:c672f5c0763f 463 // interrupt button Disbalances the current motor position
ThomasBNL 34:c672f5c0763f 464
ThomasBNL 44:5dd0a3d24662 465 //if button L2 pressed => disbalance motor
ThomasBNL 34:c672f5c0763f 466 if (buttonL2.read() < 0.5){
ThomasBNL 8:50d6e2323d3b 467 motordirection_turn = cw;
ThomasBNL 34:c672f5c0763f 468 pwm_motor_turn = 0.5f;
ThomasBNL 8:50d6e2323d3b 469 pc.printf("positie = %d \r\n", motor_turn.getPulses()); }
ThomasBNL 34:c672f5c0763f 470
ThomasBNL 8:50d6e2323d3b 471
ThomasBNL 44:5dd0a3d24662 472 // if button L1 pressed => shut down motor for 1000 seconds
ThomasBNL 34:c672f5c0763f 473 if (buttonL1.read() < 0.5){
ThomasBNL 22:eaf4cbd1dcec 474 motordirection_turn = cw;
ThomasBNL 22:eaf4cbd1dcec 475 pwm_motor_turn = 0;
ThomasBNL 22:eaf4cbd1dcec 476 wait(1000);
ThomasBNL 22:eaf4cbd1dcec 477 pc.printf("positie = %d \r\n", motor_turn.getPulses()); }
ThomasBNL 17:aa167ab3cf75 478
ThomasBNL 34:c672f5c0763f 479 else
ThomasBNL 8:50d6e2323d3b 480 {
ThomasBNL 34:c672f5c0763f 481 // ___________________________
ThomasBNL 34:c672f5c0763f 482 // // \\
ThomasBNL 34:c672f5c0763f 483 // || [Wait for go signal] ||
ThomasBNL 34:c672f5c0763f 484 // \\___________________________//
ThomasBNL 36:da07b5c2984d 485 // // Wait until looptimer flag is true then execute PID controller loop.
ThomasBNL 34:c672f5c0763f 486
ThomasBNL 0:40052f5ca77b 487 while(looptimerflag != true);
ThomasBNL 0:40052f5ca77b 488
ThomasBNL 0:40052f5ca77b 489 looptimerflag = false;
ThomasBNL 44:5dd0a3d24662 490
ThomasBNL 44:5dd0a3d24662 491
ThomasBNL 47:c61873a0b646 492 // __________________________________________________
ThomasBNL 47:c61873a0b646 493 // // \\
ThomasBNL 47:c61873a0b646 494 // || [Limit position value and convert to degrees] ||
ThomasBNL 47:c61873a0b646 495 // \\__________________________________________________//
ThomasBNL 47:c61873a0b646 496 // // Keep motor position between -4200 and 4200 counts
ThomasBNL 34:c672f5c0763f 497
ThomasBNL 8:50d6e2323d3b 498 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 3:11a7da46e093 499 {
ThomasBNL 8:50d6e2323d3b 500 motor_turn.reset();
ThomasBNL 3:11a7da46e093 501 pc.printf("RESET \n\r");
ThomasBNL 3:11a7da46e093 502 }
ThomasBNL 3:11a7da46e093 503
ThomasBNL 34:c672f5c0763f 504 // Convert position to degrees \\
ThomasBNL 34:c672f5c0763f 505
ThomasBNL 8:50d6e2323d3b 506 position_turn = conversion_counts_to_degrees * motor_turn.getPulses();
ThomasBNL 0:40052f5ca77b 507
ThomasBNL 8:50d6e2323d3b 508 pc.printf("calibrated setpoint: %f, calibrated position motor %i, position %f \n\r", reference_turn, motor_turn.getPulses(), position_turn);
ThomasBNL 46:9279c7a725bf 509
ThomasBNL 46:9279c7a725bf 510 // ___________________________
ThomasBNL 46:9279c7a725bf 511 // // \\
ThomasBNL 46:9279c7a725bf 512 // || [GET CURRENT EMG VALUES] ||
ThomasBNL 46:9279c7a725bf 513 // \\___________________________//
ThomasBNL 46:9279c7a725bf 514 //
ThomasBNL 46:9279c7a725bf 515 debug_led_green = on; //
ThomasBNL 48:950b1f34161b 516 //sample_filter();
ThomasBNL 48:950b1f34161b 517 moving_average_right = (input1.read())*100; // EMG Right bicep (tussen nul en 100%) // DEBUG TEST TOOL: substitute EMG input for potmeter inputs
ThomasBNL 48:950b1f34161b 518 moving_average_left = (input2.read())*100; // EMG Left bicep (tussen nul en 100%) // DEBUG TEST TOOL: substitute EMG input for potmeter inputs
ThomasBNL 48:950b1f34161b 519 pc.printf("mov_r = %f, mov_r = %f \n\r", moving_average_right, moving_average_left);
ThomasBNL 48:950b1f34161b 520
ThomasBNL 46:9279c7a725bf 521 // scope.set(0,EMG_left_Bicep);
ThomasBNL 46:9279c7a725bf 522 // scope.set(1,EMG_Left_Bicep_filtered);
ThomasBNL 46:9279c7a725bf 523 // scope.set(2,moving_average_left);
ThomasBNL 46:9279c7a725bf 524 // scope.send(); //
ThomasBNL 3:11a7da46e093 525
ThomasBNL 0:40052f5ca77b 526
ThomasBNL 34:c672f5c0763f 527 // ___________________________
ThomasBNL 34:c672f5c0763f 528 // // \\
ThomasBNL 46:9279c7a725bf 529 // || [ACTION CONTROLLER] ||
ThomasBNL 46:9279c7a725bf 530 // \\___________________________//
ThomasBNL 46:9279c7a725bf 531 //
ThomasBNL 46:9279c7a725bf 532
ThomasBNL 46:9279c7a725bf 533 // DO STUFF / DECIDE WHETHER TO DO SOMETHING OR NOT ==> TICKER loop moet dan wel uit (TICKER VOOR SAMPLE FILTER WEL AAN)
ThomasBNL 46:9279c7a725bf 534
ThomasBNL 46:9279c7a725bf 535
ThomasBNL 46:9279c7a725bf 536
ThomasBNL 46:9279c7a725bf 537 // Nieuwe_actie:
ThomasBNL 46:9279c7a725bf 538 // TICKER AAN // start here again when action has finished
ThomasBNL 46:9279c7a725bf 539 // debug_led_red=off;
ThomasBNL 46:9279c7a725bf 540 // debug_led_blue=off;
ThomasBNL 46:9279c7a725bf 541 // debug_led_green=on; // LED Turns green if ready for a new action
ThomasBNL 46:9279c7a725bf 542 // wait(1);
ThomasBNL 46:9279c7a725bf 543 // moving_average_right = (potmeter1.read())*100; //EMG Right bicep (tussen nul en 100%)
ThomasBNL 46:9279c7a725bf 544 // moving_average_left = (potmeter2.read())*100; // EMG Left bicep (tussen nul en 100%)
ThomasBNL 46:9279c7a725bf 545 // pc.printf("moving_average_left = %f moving_average_right = %f \n\r", moving_average_left, moving_average_right);
ThomasBNL 46:9279c7a725bf 546 //
ThomasBNL 46:9279c7a725bf 547 //
ThomasBNL 47:c61873a0b646 548 // // STRIKE // (No LED -> 0.55s -> red on (turns on every time a new pwm is given) -> finish)
ThomasBNL 46:9279c7a725bf 549 // if (moving_average_left > Threshold_Bicep_Left_1 && moving_average_right > Threshold_Bicep_Right_1)
ThomasBNL 46:9279c7a725bf 550 // {
ThomasBNL 46:9279c7a725bf 551 // TICKER OFF
ThomasBNL 46:9279c7a725bf 552 // debug_led_green=off;
ThomasBNL 46:9279c7a725bf 553 // n=0;
ThomasBNL 46:9279c7a725bf 554 // pc.printf("slag \n\r");
ThomasBNL 46:9279c7a725bf 555 // wait(0.5);
ThomasBNL 46:9279c7a725bf 556 //
ThomasBNL 46:9279c7a725bf 557 // while(moving_average_left > Threshold_Bicep_Left_1 && moving_average_right > Threshold_Bicep_Right_1) // Threshold statement still true after 0.5 seconds?
ThomasBNL 46:9279c7a725bf 558 // {
ThomasBNL 46:9279c7a725bf 559 // if (n==0) //wordt maar 1 keer uitgevoerd
ThomasBNL 46:9279c7a725bf 560 // {
ThomasBNL 46:9279c7a725bf 561 // deactivate_PID_Controller_strike(); // function that sets P, I and D gain values to zero
ThomasBNL 46:9279c7a725bf 562 // n=1;
ThomasBNL 46:9279c7a725bf 563 // }
ThomasBNL 46:9279c7a725bf 564 // debug_led_red=off;
ThomasBNL 46:9279c7a725bf 565 // wait(0.10); // wait 20 samples
ThomasBNL 46:9279c7a725bf 566 // debug_led_red=on;
ThomasBNL 46:9279c7a725bf 567 // pwm_motor_strike=((moving_average_left-EMG_L_min)+(moving_average_right-EMG_R_min))/((EMG_L_max-EMG_L_min)+(EMG_R_max-EMG_R_min))*0.7 + 0.3; // min speed 0.3 en max 1
ThomasBNL 46:9279c7a725bf 568 // wait(0.10); // wait 20 samples more (pwm changes per 0.1 seconds)
ThomasBNL 46:9279c7a725bf 569 // motordirection_strike=cw; // towards bottle
ThomasBNL 46:9279c7a725bf 570 //
ThomasBNL 46:9279c7a725bf 571 // if((position_strike-Hit)<2) // bottle is hit when position-hit <2 defrees
ThomasBNL 46:9279c7a725bf 572 // {
ThomasBNL 46:9279c7a725bf 573 // activate_PID_Controller_strike(); // function sets back P, I and D gain values
ThomasBNL 46:9279c7a725bf 574 // pc.printf("einde \n\r");
ThomasBNL 46:9279c7a725bf 575 // goto Nieuwe_actie; // Finished: Get Ready for new Action control
ThomasBNL 46:9279c7a725bf 576 // }
ThomasBNL 46:9279c7a725bf 577 // }
ThomasBNL 46:9279c7a725bf 578 // }
ThomasBNL 46:9279c7a725bf 579 //
ThomasBNL 46:9279c7a725bf 580 // activate_PID_Controller_strike(); // function sets back P, I and D gain values
ThomasBNL 46:9279c7a725bf 581 // TICKER AAN
ThomasBNL 46:9279c7a725bf 582 // debug_led_red=off; // not inside an action loop (green light)
ThomasBNL 46:9279c7a725bf 583 // debug_led_blue=off;
ThomasBNL 46:9279c7a725bf 584 // debug_led_green=on; // want het kan zijn dat bovenste script half wordt uitgevoerd en dan moet het slag mechanisme wel weer terug
ThomasBNL 46:9279c7a725bf 585 //
ThomasBNL 47:c61873a0b646 586 // // TURN LEFT // // (blue->blue off (very short/visible?)-> red<->blue<-> red -> klaar)
ThomasBNL 46:9279c7a725bf 587 // if (moving_average_left > Threshold_Bicep_Left_2 && moving_average_right < Threshold_Bicep_Right_1)
ThomasBNL 46:9279c7a725bf 588 // {
ThomasBNL 46:9279c7a725bf 589 // debug_led_green=off; // Executing action
ThomasBNL 46:9279c7a725bf 590 // TICKER UIT
ThomasBNL 46:9279c7a725bf 591 // n=0;
ThomasBNL 46:9279c7a725bf 592 // pc.printf("links \n\r");
ThomasBNL 46:9279c7a725bf 593 // while(moving_average_left > Threshold_Bicep_Left_1 && moving_average_right < Threshold_Bicep_Right_1)
ThomasBNL 46:9279c7a725bf 594 // {
ThomasBNL 46:9279c7a725bf 595 // debug_led_blue=on;
ThomasBNL 46:9279c7a725bf 596 // if (n==0) //wordt maar 1 keer uitgevoerd
ThomasBNL 46:9279c7a725bf 597 // {
ThomasBNL 46:9279c7a725bf 598 // debug_led_blue=off;
ThomasBNL 46:9279c7a725bf 599 // reference_turn=reference_turn+change_one_bottle;
ThomasBNL 46:9279c7a725bf 600 // n=1;
ThomasBNL 46:9279c7a725bf 601 // }
ThomasBNL 46:9279c7a725bf 602 //
ThomasBNL 46:9279c7a725bf 603 // if (fabs(position_turn-reference_turn)<2) // als error en kleiner dan twee graden
ThomasBNL 46:9279c7a725bf 604 // {
ThomasBNL 46:9279c7a725bf 605 // debug_led_blue=off;
ThomasBNL 46:9279c7a725bf 606 // debug_led_red=on;
ThomasBNL 46:9279c7a725bf 607 // wait(0.5);
ThomasBNL 46:9279c7a725bf 608 // if (fabs(position_turn-reference_turn)<2) // Is de error na 0.5 seconde nog steeds kleiner dan twee graden?
ThomasBNL 46:9279c7a725bf 609 // {
ThomasBNL 46:9279c7a725bf 610 // goto Nieuwe_actie; // kunt weer iets nieuws doen indien vorige actie is uitgevoerd
ThomasBNL 46:9279c7a725bf 611 // }
ThomasBNL 46:9279c7a725bf 612 // }
ThomasBNL 46:9279c7a725bf 613 // }
ThomasBNL 46:9279c7a725bf 614 // }
ThomasBNL 46:9279c7a725bf 615 //
ThomasBNL 46:9279c7a725bf 616 // debug_led_red=off; // not inside an action loop
ThomasBNL 46:9279c7a725bf 617 // debug_led_blue=off;
ThomasBNL 46:9279c7a725bf 618 // debug_led_green=on; // not executing an action
ThomasBNL 46:9279c7a725bf 619 // TICKER AAN
ThomasBNL 46:9279c7a725bf 620 //
ThomasBNL 47:c61873a0b646 621 // // TURN RIGHT // (blue->blue uit (heel kort/zichtbaar?)-> red<->blue<-> red -> klaar)
ThomasBNL 46:9279c7a725bf 622 // if (moving_average_right > Threshold_Bicep_Right_2 && moving_average_left < Threshold_Bicep_Right_1)
ThomasBNL 46:9279c7a725bf 623 // {
ThomasBNL 46:9279c7a725bf 624 // TICKER UIT
ThomasBNL 46:9279c7a725bf 625 // debug_led_green=off; // Executing action
ThomasBNL 46:9279c7a725bf 626 // pc.printf("rechts \n\r");
ThomasBNL 46:9279c7a725bf 627 // n=0;
ThomasBNL 46:9279c7a725bf 628 // while(moving_average_right > Threshold_Bicep_Right_1 && moving_average_left < Threshold_Bicep_Left_1)
ThomasBNL 46:9279c7a725bf 629 // {
ThomasBNL 46:9279c7a725bf 630 // debug_led_blue=on;
ThomasBNL 46:9279c7a725bf 631 // if (n==0)
ThomasBNL 46:9279c7a725bf 632 // {
ThomasBNL 46:9279c7a725bf 633 // debug_led_blue=off;
ThomasBNL 46:9279c7a725bf 634 // reference_turn=reference_turn-change_one_bottle;
ThomasBNL 46:9279c7a725bf 635 // n=1;
ThomasBNL 46:9279c7a725bf 636 // }
ThomasBNL 46:9279c7a725bf 637 // //(45 graden naar rechts voor volgende fles) //wordt maar 1 keer uitgevoerd
ThomasBNL 46:9279c7a725bf 638 // pc.printf("Ref: %f Err: %f \n\r", reference_turn, position_turn);
ThomasBNL 46:9279c7a725bf 639 //
ThomasBNL 46:9279c7a725bf 640 // if (abs(position_turn-reference_turn)<2) // als error en kleiner dan twee graden
ThomasBNL 46:9279c7a725bf 641 // {
ThomasBNL 46:9279c7a725bf 642 // debug_led_blue=off;
ThomasBNL 46:9279c7a725bf 643 // debug_led_red=on;
ThomasBNL 46:9279c7a725bf 644 // wait(0.5);
ThomasBNL 46:9279c7a725bf 645 // if (abs(position_turn-reference_turn)<2) // Is de error na 0.5 seconde nog steeds kleiner dan twee graden?
ThomasBNL 46:9279c7a725bf 646 // {
ThomasBNL 46:9279c7a725bf 647 // goto Nieuwe_actie; // kunt weer iets nieuws doen indien vorige actie is uitgevoerd
ThomasBNL 46:9279c7a725bf 648 // }
ThomasBNL 46:9279c7a725bf 649 // }
ThomasBNL 46:9279c7a725bf 650 // }
ThomasBNL 46:9279c7a725bf 651 // }
ThomasBNL 46:9279c7a725bf 652 // debug_led_red=off; // not inside an action loop
ThomasBNL 46:9279c7a725bf 653 // debug_led_blue=off;
ThomasBNL 46:9279c7a725bf 654 // debug_led_green=on; // not executing an action
ThomasBNL 46:9279c7a725bf 655 //
ThomasBNL 46:9279c7a725bf 656
ThomasBNL 46:9279c7a725bf 657
ThomasBNL 46:9279c7a725bf 658 // TICKER WEER AAN
ThomasBNL 46:9279c7a725bf 659
ThomasBNL 46:9279c7a725bf 660 // ___________________________
ThomasBNL 46:9279c7a725bf 661 // // \\
ThomasBNL 34:c672f5c0763f 662 // || [PID CONTROLLER] ||
ThomasBNL 34:c672f5c0763f 663 // \\___________________________//
ThomasBNL 34:c672f5c0763f 664 // // Calculate error then multiply it with the (P, I and D) gains, and store in pwm_to_motor \\
ThomasBNL 34:c672f5c0763f 665
ThomasBNL 42:0a7898cb3e94 666 error_turn=(reference_turn - position_turn); // TURN: Current error (input controller)
ThomasBNL 44:5dd0a3d24662 667 integrate_error_turn=integrate_error_turn + sample_time*error_turn; // TURN: Integral error output
ThomasBNL 44:5dd0a3d24662 668 // overwrite previous integrate error by adding the current error
ThomasBNL 34:c672f5c0763f 669 // multiplied by the sample time.
ThomasBNL 8:50d6e2323d3b 670 //
ThomasBNL 44:5dd0a3d24662 671 double error_derivative_turn=(error_turn - previous_error_turn)/sample_time; // TURN: Derivative error output
ThomasBNL 44:5dd0a3d24662 672 error_derivative_turn=Lowpassfilter_derivative.step(error_derivative_turn); // TURN: Filter
ThomasBNL 3:11a7da46e093 673
ThomasBNL 44:5dd0a3d24662 674 previous_error_turn=error_turn; // current error is saved to memory constant to be used in
ThomasBNL 44:5dd0a3d24662 675 // the next loop for calculating the derivative error
ThomasBNL 44:5dd0a3d24662 676
ThomasBNL 44:5dd0a3d24662 677 pwm_motor_turn_P = error_turn*P_gain_turn; // Output P controller to pwm
ThomasBNL 44:5dd0a3d24662 678 pwm_motor_turn_I = integrate_error_turn*I_gain_turn; // Output I controller to pwm
ThomasBNL 44:5dd0a3d24662 679 pwm_motor_turn_D = error_derivative_turn*D_gain_turn; // Output D controller to pwm
ThomasBNL 10:09ba965045a7 680
ThomasBNL 13:bcf8ec7120ab 681 pwm_to_motor_turn = pwm_motor_turn_P + pwm_motor_turn_I + pwm_motor_turn_D;
ThomasBNL 20:bdc62ee49197 682
ThomasBNL 34:c672f5c0763f 683 // ___________________________
ThomasBNL 34:c672f5c0763f 684 // // \\
ThomasBNL 34:c672f5c0763f 685 // || [PID error -> pwm motor] ||
ThomasBNL 34:c672f5c0763f 686 // \\___________________________//
ThomasBNL 36:da07b5c2984d 687 // // Keep Pwm between -1 and 1 -> and decide the direction of the motor to compensate the error
ThomasBNL 34:c672f5c0763f 688
ThomasBNL 10:09ba965045a7 689 keep_in_range(&pwm_to_motor_turn, -1,1); // Pass to motor controller but keep it in range!
ThomasBNL 10:09ba965045a7 690 pc.printf("pwm %f \n\r", pwm_to_motor_turn);
ThomasBNL 0:40052f5ca77b 691
ThomasBNL 8:50d6e2323d3b 692 // Check error and decide direction to turn
ThomasBNL 10:09ba965045a7 693 if(pwm_to_motor_turn > 0)
ThomasBNL 3:11a7da46e093 694 {
ThomasBNL 8:50d6e2323d3b 695 motordirection_turn=ccw;
ThomasBNL 15:f029351f1f3a 696 pc.printf("if loop pwm > 0 \n\r");
ThomasBNL 3:11a7da46e093 697 }
ThomasBNL 0:40052f5ca77b 698 else
ThomasBNL 3:11a7da46e093 699 {
ThomasBNL 8:50d6e2323d3b 700 motordirection_turn=cw;
ThomasBNL 15:f029351f1f3a 701 pc.printf("else loop pwm < 0 \n\r");
ThomasBNL 3:11a7da46e093 702 }
ThomasBNL 8:50d6e2323d3b 703
ThomasBNL 34:c672f5c0763f 704 // ___________________________
ThomasBNL 34:c672f5c0763f 705 // // \\
ThomasBNL 34:c672f5c0763f 706 // || [pwm -> Plant] ||
ThomasBNL 34:c672f5c0763f 707 // \\___________________________//
ThomasBNL 34:c672f5c0763f 708 // // Put pwm to the motor \\
ThomasBNL 34:c672f5c0763f 709
ThomasBNL 40:bbe7922723df 710 pwm_motor_turn=(abs(pwm_to_motor_turn));
ThomasBNL 14:599896acf576 711
ThomasBNL 34:c672f5c0763f 712 // ___________________________
ThomasBNL 34:c672f5c0763f 713 // // \\
ThomasBNL 34:c672f5c0763f 714 // || [HIDSCOPE] ||
ThomasBNL 34:c672f5c0763f 715 // \\___________________________//
ThomasBNL 34:c672f5c0763f 716 // // Check signals inside HIDSCOPE \\
ThomasBNL 34:c672f5c0763f 717
ThomasBNL 48:950b1f34161b 718 //scope.set(0,error_turn); // HIDSCOPE channel 0 : Current Error
ThomasBNL 48:950b1f34161b 719 //scope.set(1,position_turn); // HIDSCOPE channel 1 : Position_turn
ThomasBNL 48:950b1f34161b 720 //scope.set(2,pwm_to_motor_turn); // HIDSCOPE channel 2 : Pwm_to_motor_turn
ThomasBNL 48:950b1f34161b 721 //scope.send(); // Send channel info to HIDSCOPE
ThomasBNL 40:bbe7922723df 722
ThomasBNL 47:c61873a0b646 723 // __________________________________
ThomasBNL 47:c61873a0b646 724 // // \\
ThomasBNL 47:c61873a0b646 725 // || [Deactivate if reached position] ||
ThomasBNL 47:c61873a0b646 726 // \\__________________________________//
ThomasBNL 41:424264a4c39c 727 // // Check whether the motor has reached reference position and can be shut down
ThomasBNL 40:bbe7922723df 728
ThomasBNL 42:0a7898cb3e94 729 if(fabs(error_turn)<2) // Shut down if error is smaller than two degrees
ThomasBNL 42:0a7898cb3e94 730 {deactivate_PID_Controller(P_gain_turn,I_gain_turn,D_gain_turn);}
ThomasBNL 40:bbe7922723df 731 else
ThomasBNL 42:0a7898cb3e94 732 {activate_PID_Controller_turn(P_gain_turn,I_gain_turn,D_gain_turn);}
ThomasBNL 0:40052f5ca77b 733 }
ThomasBNL 0:40052f5ca77b 734 }
ThomasBNL 5:8fb74a22fe3c 735 }
ThomasBNL 0:40052f5ca77b 736
ThomasBNL 46:9279c7a725bf 737 //============================================================================================================================
ThomasBNL 46:9279c7a725bf 738 ///////////////////////////////
ThomasBNL 46:9279c7a725bf 739 // //
ThomasBNL 46:9279c7a725bf 740 ///////////////////////////////////////////// [FUNCTIONS DESCRIBED] /////////////////////////////////////////////////////
ThomasBNL 46:9279c7a725bf 741 // //
ThomasBNL 46:9279c7a725bf 742 ///////////////////////////////
ThomasBNL 46:9279c7a725bf 743 //============================================================================================================================
ThomasBNL 46:9279c7a725bf 744 // ___________________________
ThomasBNL 46:9279c7a725bf 745 // // \\
ThomasBNL 46:9279c7a725bf 746 // || [MOTOR FUCNTIONS] ||
ThomasBNL 46:9279c7a725bf 747 // \\___________________________//
ThomasBNL 46:9279c7a725bf 748 //
ThomasBNL 34:c672f5c0763f 749
ThomasBNL 0:40052f5ca77b 750 // Keep in range function
ThomasBNL 0:40052f5ca77b 751 void keep_in_range(double * in, double min, double max)
ThomasBNL 0:40052f5ca77b 752 {
ThomasBNL 0:40052f5ca77b 753 *in > min ? *in < max? : *in = max: *in = min;
ThomasBNL 0:40052f5ca77b 754 }
ThomasBNL 0:40052f5ca77b 755
ThomasBNL 0:40052f5ca77b 756 // Looptimerflag function
ThomasBNL 0:40052f5ca77b 757 void setlooptimerflag(void)
ThomasBNL 0:40052f5ca77b 758 {
ThomasBNL 0:40052f5ca77b 759 looptimerflag = true;
ThomasBNL 1:dc683e88b44e 760 }
ThomasBNL 1:dc683e88b44e 761
ThomasBNL 39:104a038f7b92 762 // Change reference
ThomasBNL 39:104a038f7b92 763 void Change_Turn_Position_Left (double& reference, double change_one_bottle)
ThomasBNL 39:104a038f7b92 764 {
ThomasBNL 46:9279c7a725bf 765 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 766 {
ThomasBNL 45:359df0594588 767 reference=-90;
ThomasBNL 45:359df0594588 768 }
ThomasBNL 45:359df0594588 769 else
ThomasBNL 45:359df0594588 770 {
ThomasBNL 45:359df0594588 771 reference = reference + change_one_bottle;
ThomasBNL 46:9279c7a725bf 772 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 773 }
ThomasBNL 39:104a038f7b92 774 }
ThomasBNL 39:104a038f7b92 775
ThomasBNL 39:104a038f7b92 776 void Change_Turn_Position_Right (double& reference, double change_one_bottle)
ThomasBNL 39:104a038f7b92 777 {
ThomasBNL 45:359df0594588 778 if(reference==-90)
ThomasBNL 45:359df0594588 779 {
ThomasBNL 45:359df0594588 780 reference=90;
ThomasBNL 45:359df0594588 781 }
ThomasBNL 45:359df0594588 782 else
ThomasBNL 45:359df0594588 783 {
ThomasBNL 45:359df0594588 784 reference = reference - change_one_bottle;
ThomasBNL 45:359df0594588 785 keep_in_range(&reference, -90, 90);
ThomasBNL 45:359df0594588 786 }
ThomasBNL 39:104a038f7b92 787 }
ThomasBNL 39:104a038f7b92 788
ThomasBNL 42:0a7898cb3e94 789 // Deactivate or Activate PID_Controller
ThomasBNL 42:0a7898cb3e94 790 void deactivate_PID_Controller(double& P_gain, double& I_gain, double& D_gain)
ThomasBNL 42:0a7898cb3e94 791 {
ThomasBNL 42:0a7898cb3e94 792 P_gain=0;
ThomasBNL 42:0a7898cb3e94 793 I_gain=0;
ThomasBNL 42:0a7898cb3e94 794 D_gain=0;
ThomasBNL 42:0a7898cb3e94 795 }
ThomasBNL 42:0a7898cb3e94 796
ThomasBNL 42:0a7898cb3e94 797 void activate_PID_Controller_turn(double& P_gain, double& I_gain, double& D_gain)
ThomasBNL 42:0a7898cb3e94 798 {
ThomasBNL 45:359df0594588 799 P_gain=0.01; // hier waardes P,I,D veranderen (waardes bovenaan doen (tijdelijk) niks meer
ThomasBNL 45:359df0594588 800 I_gain=0.5;
ThomasBNL 45:359df0594588 801 D_gain=5;
ThomasBNL 42:0a7898cb3e94 802 }
ThomasBNL 42:0a7898cb3e94 803
ThomasBNL 42:0a7898cb3e94 804 void activate_PID_Controller_strike(double& P_gain, double& I_gain, double& D_gain)
ThomasBNL 42:0a7898cb3e94 805 {
ThomasBNL 42:0a7898cb3e94 806 P_gain=0; // hier waardes P,I,D veranderen (waardes bovenaan doen (tijdelijk) niks meer
ThomasBNL 42:0a7898cb3e94 807 I_gain=0;
ThomasBNL 42:0a7898cb3e94 808 D_gain=0;
ThomasBNL 46:9279c7a725bf 809 }
ThomasBNL 46:9279c7a725bf 810
ThomasBNL 46:9279c7a725bf 811 //============================================================================================================================
ThomasBNL 46:9279c7a725bf 812 // ___________________________
ThomasBNL 46:9279c7a725bf 813 // // \\
ThomasBNL 46:9279c7a725bf 814 // || [EMG FUCNTIONS] ||
ThomasBNL 46:9279c7a725bf 815 // \\___________________________//
ThomasBNL 46:9279c7a725bf 816 //
ThomasBNL 46:9279c7a725bf 817
ThomasBNL 46:9279c7a725bf 818 // [CALIBRATION] // (blue LED)
ThomasBNL 46:9279c7a725bf 819 void calibration()
ThomasBNL 46:9279c7a725bf 820 {
ThomasBNL 46:9279c7a725bf 821
ThomasBNL 46:9279c7a725bf 822
ThomasBNL 46:9279c7a725bf 823 // [MINIMUM VALUE BICEPS CALIBRATION] //
ThomasBNL 46:9279c7a725bf 824
ThomasBNL 46:9279c7a725bf 825
ThomasBNL 46:9279c7a725bf 826 debug_led_blue=on;
ThomasBNL 46:9279c7a725bf 827 pc.printf("Start minimum calibration in 5 seconds \n\r");
ThomasBNL 46:9279c7a725bf 828 pc.printf("Keep your biceps as relaxed as possible \n\r");
ThomasBNL 46:9279c7a725bf 829
ThomasBNL 46:9279c7a725bf 830 countdown_from_5();
ThomasBNL 46:9279c7a725bf 831 c=0;
ThomasBNL 46:9279c7a725bf 832
ThomasBNL 46:9279c7a725bf 833 while(c<2560) // 512Hz -> 2560 is equal to five seconds
ThomasBNL 46:9279c7a725bf 834 {
ThomasBNL 46:9279c7a725bf 835 Filter(); // Filter EMG signal
ThomasBNL 46:9279c7a725bf 836 minimum_L=EMG_Left_Bicep_filtered+minimum_L; // Take previous sample EMG_Left_Bicep_filtered and add the new value
ThomasBNL 46:9279c7a725bf 837 minimum_R=EMG_Right_Bicep_filtered+minimum_R;
ThomasBNL 46:9279c7a725bf 838 // scope.set(0,EMG_left_Bicep);
ThomasBNL 46:9279c7a725bf 839 // scope.set(1,EMG_Left_Bicep_filtered);
ThomasBNL 46:9279c7a725bf 840 // scope.set(2,minimum_L);
ThomasBNL 46:9279c7a725bf 841 // scope.send();
ThomasBNL 46:9279c7a725bf 842 c++; // Every sample c is increased by one until the statement c<2560 is false
ThomasBNL 46:9279c7a725bf 843 wait(0.001953125); // wait one sample
ThomasBNL 46:9279c7a725bf 844 }
ThomasBNL 46:9279c7a725bf 845
ThomasBNL 46:9279c7a725bf 846 pc.printf("Finished minimum calibration \n\r");
ThomasBNL 46:9279c7a725bf 847
ThomasBNL 46:9279c7a725bf 848 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 849 EMG_R_min=minimum_R/2560;
ThomasBNL 46:9279c7a725bf 850
ThomasBNL 46:9279c7a725bf 851 pc.printf("EMG_L_min = %f \n\r EMG_R_min = %f \n\r", EMG_L_min, EMG_R_min);
ThomasBNL 46:9279c7a725bf 852
ThomasBNL 46:9279c7a725bf 853 wait (3); //cooldown
ThomasBNL 46:9279c7a725bf 854
ThomasBNL 46:9279c7a725bf 855
ThomasBNL 46:9279c7a725bf 856 // [MAXIMUM VALUE BICEPS CALIBRATION] //
ThomasBNL 46:9279c7a725bf 857
ThomasBNL 46:9279c7a725bf 858
ThomasBNL 46:9279c7a725bf 859 pc.printf("start maximum calibration in 5 seconds (start contraction at 3) \n\r");
ThomasBNL 46:9279c7a725bf 860
ThomasBNL 46:9279c7a725bf 861 countdown_from_5();
ThomasBNL 46:9279c7a725bf 862 c=0;
ThomasBNL 46:9279c7a725bf 863
ThomasBNL 46:9279c7a725bf 864 while(c<2560) // 512Hz -> 2560 is equal to five seconds
ThomasBNL 46:9279c7a725bf 865 {
ThomasBNL 46:9279c7a725bf 866 Filter(); // Filter EMG signal
ThomasBNL 46:9279c7a725bf 867 maximum_L=EMG_Left_Bicep_filtered+maximum_L; // Take previous sample EMG_Left_Bicep_filtered and add the new value
ThomasBNL 46:9279c7a725bf 868 maximum_R=EMG_Right_Bicep_filtered+maximum_R;
ThomasBNL 46:9279c7a725bf 869 // scope.set(0,EMG_left_Bicep);
ThomasBNL 46:9279c7a725bf 870 // scope.set(1,EMG_Left_Bicep_filtered);
ThomasBNL 46:9279c7a725bf 871 // scope.set(2,maximum_R);
ThomasBNL 46:9279c7a725bf 872 // scope.send();
ThomasBNL 46:9279c7a725bf 873 c++; // Every sample c is increased by one until the statement c<2560 is false
ThomasBNL 46:9279c7a725bf 874 wait(0.002);
ThomasBNL 46:9279c7a725bf 875 }
ThomasBNL 46:9279c7a725bf 876
ThomasBNL 46:9279c7a725bf 877 pc.printf("Finished minimum calibration \n\r");
ThomasBNL 46:9279c7a725bf 878
ThomasBNL 46:9279c7a725bf 879 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 880 EMG_R_max=maximum_R/2560;
ThomasBNL 46:9279c7a725bf 881
ThomasBNL 46:9279c7a725bf 882 pc.printf("EMG_L_max = %f \n\r EMG_R_max = %f \n\r", EMG_L_max, EMG_R_max);
ThomasBNL 46:9279c7a725bf 883
ThomasBNL 46:9279c7a725bf 884 wait (3); //cooldown
ThomasBNL 46:9279c7a725bf 885
ThomasBNL 46:9279c7a725bf 886 debug_led_blue=off;
ThomasBNL 46:9279c7a725bf 887
ThomasBNL 46:9279c7a725bf 888
ThomasBNL 46:9279c7a725bf 889 // [MAXIMUM VALUE BICEPS CALIBRATION] //
ThomasBNL 46:9279c7a725bf 890 // Calculate threshold percentages //
ThomasBNL 46:9279c7a725bf 891
ThomasBNL 46:9279c7a725bf 892 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 893 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 894 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 895 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 896
ThomasBNL 46:9279c7a725bf 897 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 898
ThomasBNL 46:9279c7a725bf 899 }
ThomasBNL 46:9279c7a725bf 900
ThomasBNL 46:9279c7a725bf 901
ThomasBNL 46:9279c7a725bf 902 // [COUNTDOWN FUNCTION] //
ThomasBNL 46:9279c7a725bf 903
ThomasBNL 46:9279c7a725bf 904
ThomasBNL 46:9279c7a725bf 905 void countdown_from_5() // Countdown from 5 till 0 inside Putty (interface)
ThomasBNL 46:9279c7a725bf 906 {
ThomasBNL 46:9279c7a725bf 907 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 908 wait(1); pc.printf("1 Set \n\r"); wait(1); pc.printf("Go \n\r");
ThomasBNL 46:9279c7a725bf 909 }
ThomasBNL 46:9279c7a725bf 910
ThomasBNL 46:9279c7a725bf 911
ThomasBNL 46:9279c7a725bf 912 // [FLOW CONTROL FUNCTIONS] //
ThomasBNL 46:9279c7a725bf 913
ThomasBNL 46:9279c7a725bf 914
ThomasBNL 46:9279c7a725bf 915 void ControlGo() //Control flag
ThomasBNL 46:9279c7a725bf 916 {
ThomasBNL 46:9279c7a725bf 917 control_go = true;
ThomasBNL 46:9279c7a725bf 918 }
ThomasBNL 46:9279c7a725bf 919
ThomasBNL 46:9279c7a725bf 920 void take_sample() // Take a sample every 25th sample
ThomasBNL 46:9279c7a725bf 921 {
ThomasBNL 46:9279c7a725bf 922 if(n==25)
ThomasBNL 46:9279c7a725bf 923 {
ThomasBNL 46:9279c7a725bf 924 sample = true;
ThomasBNL 46:9279c7a725bf 925 n=0;
ThomasBNL 46:9279c7a725bf 926 debug_led_green = off;
ThomasBNL 46:9279c7a725bf 927 }
ThomasBNL 46:9279c7a725bf 928 }
ThomasBNL 46:9279c7a725bf 929
ThomasBNL 46:9279c7a725bf 930
ThomasBNL 46:9279c7a725bf 931 // [FILTER FUNCTIONS] //
ThomasBNL 46:9279c7a725bf 932 // [EMG] //
ThomasBNL 46:9279c7a725bf 933
ThomasBNL 46:9279c7a725bf 934 void Filter() // Unfiltered EMG (input) -> highpass filter -> rectify -> lowpass filter -> Filtered EMG (output)
ThomasBNL 46:9279c7a725bf 935 {
ThomasBNL 46:9279c7a725bf 936 EMG_left_Bicep = input1; EMG_Right_Bicep = input2;
ThomasBNL 46:9279c7a725bf 937
ThomasBNL 46:9279c7a725bf 938 EMG_Left_Bicep_filtered = highpassfilter_1.step(EMG_left_Bicep); EMG_Right_Bicep_filtered = highpassfilter_2.step(EMG_Right_Bicep);
ThomasBNL 46:9279c7a725bf 939 EMG_Left_Bicep_filtered = fabs(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = fabs(EMG_Right_Bicep_filtered);
ThomasBNL 46:9279c7a725bf 940 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 941
ThomasBNL 46:9279c7a725bf 942 EMG_Left_Bicep_filtered = EMG_Left_Bicep_filtered; EMG_Right_Bicep_filtered = EMG_Right_Bicep_filtered;
ThomasBNL 46:9279c7a725bf 943 }
ThomasBNL 46:9279c7a725bf 944
ThomasBNL 46:9279c7a725bf 945
ThomasBNL 46:9279c7a725bf 946 // [FILTER FUNCTIONS] //
ThomasBNL 46:9279c7a725bf 947 // [Include Moving Average] //
ThomasBNL 46:9279c7a725bf 948
ThomasBNL 46:9279c7a725bf 949 void sample_filter()
ThomasBNL 46:9279c7a725bf 950 {
ThomasBNL 46:9279c7a725bf 951 Filter();
ThomasBNL 46:9279c7a725bf 952 take_sample();
ThomasBNL 46:9279c7a725bf 953 if(sample)
ThomasBNL 46:9279c7a725bf 954 {
ThomasBNL 46:9279c7a725bf 955 sample=false;
ThomasBNL 46:9279c7a725bf 956 Sample_EMG_L_1 = EMG_Left_Bicep_filtered; Sample_EMG_R_1 = EMG_Right_Bicep_filtered;
ThomasBNL 46:9279c7a725bf 957
ThomasBNL 46:9279c7a725bf 958 Sample_EMG_L_10= Sample_EMG_L_9; Sample_EMG_R_10= Sample_EMG_R_9;
ThomasBNL 46:9279c7a725bf 959 Sample_EMG_L_9 = Sample_EMG_L_8; Sample_EMG_R_9 = Sample_EMG_R_8;
ThomasBNL 46:9279c7a725bf 960 Sample_EMG_L_8 = Sample_EMG_L_7; Sample_EMG_R_8 = Sample_EMG_R_7;
ThomasBNL 46:9279c7a725bf 961 Sample_EMG_L_7 = Sample_EMG_L_6; Sample_EMG_R_7 = Sample_EMG_R_6;
ThomasBNL 46:9279c7a725bf 962 Sample_EMG_L_6 = Sample_EMG_L_5; Sample_EMG_R_6 = Sample_EMG_R_5;
ThomasBNL 46:9279c7a725bf 963 Sample_EMG_L_5 = Sample_EMG_L_4; Sample_EMG_R_5 = Sample_EMG_R_4;
ThomasBNL 46:9279c7a725bf 964 Sample_EMG_L_4 = Sample_EMG_L_3; Sample_EMG_R_4 = Sample_EMG_R_3;
ThomasBNL 46:9279c7a725bf 965 Sample_EMG_L_3 = Sample_EMG_L_2; Sample_EMG_R_3 = Sample_EMG_R_2;
ThomasBNL 46:9279c7a725bf 966 Sample_EMG_L_2 = Sample_EMG_L_1; Sample_EMG_R_2 = Sample_EMG_R_1;
ThomasBNL 46:9279c7a725bf 967 }
ThomasBNL 46:9279c7a725bf 968 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 969 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 970 n++;
ThomasBNL 46:9279c7a725bf 971 }
ThomasBNL 46:9279c7a725bf 972
ThomasBNL 47:c61873a0b646 973 //============================================================================================================================
ThomasBNL 47:c61873a0b646 974 // ___________________________
ThomasBNL 47:c61873a0b646 975 // // \\
ThomasBNL 47:c61873a0b646 976 // || [Action Controller] ||
ThomasBNL 47:c61873a0b646 977 // \\___________________________//
ThomasBNL 47:c61873a0b646 978 //