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@96:94f44c06f31a, 2015-10-29 (annotated)

Committer:
ThomasBNL
Date:
Thu Oct 29 19:27:06 2015 +0000
Revision:
96:94f44c06f31a
Parent:
95:25b36fe02ee5
Child:
97:c23dd6dacb68
Nieuwe PID waardes toegevoegd om te testen

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