Bouke Scheltinga / Mbed 2 deprecated Werk_college_23sept

Werkcollege opgave 23 september BMT K9

Dependencies:   Encoder HIDScope MODSERIAL mbed QEI biquadFilter

main.cpp@40:7f928b465f8d, 2015-10-21 (annotated)

Committer:
ThomasBNL
Date:
Wed Oct 21 10:02:10 2015 +0000
Revision:
40:7f928b465f8d
Parent:
39:9fa091753592
Child:
41:9f28072c882d
Child:
47:926669fe14b1
GEHELE SCRIPT: inclusief comments

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ThomasBNL 40:7f928b465f8d 1 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 2 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 3 ///// //// ////// ////// /////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 4 ///// //// ////// ////// /////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 5 //////////// //////////// ////// ////// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 6 //////////// //////////// ////// ////// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 7 //////////// //////////// ////// /////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 8 //////////// //////////// ////// /////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 9 //////////// //////////// ////// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 10 //////////// //////////// ////// ////// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 11 //////////// //////////// ////// ////// /////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 12 //////////// //////////// ////// ////// /////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 13 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 14 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 15 /////// ////// ////// ////// ////// /////// ////// /////// ////// /////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 16 /////// ////// ////// ////// ////// ///// ////// ///// ////// /////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 17 /////// ///////////////// ////// ////// //////////// //// ////// //// ////// ///////////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 18 /////// ///////////////// ////// ////// //////////// /// ////// /// ////// ///////////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 19 /////// ///////////////// ////// ////// // ////// // ////// /////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 20 /////// ///////////////// ////// ////// // ////// // ////// /////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 21 /////// ///////////////// ////// //////////// /// ////// /// ////// ///////////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 22 /////// ///////////////// ////// ////// //////////// //// ////// //// ////// ///////////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 23 /////// ////// ////// ////// ////// ///// ////// ///// ////// /////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 24 /////// ////// ////// ////// ////// ////// ////// ////// ////// /////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 25 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 26 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 27 /////// /////////////// ///////////// /////// ////// /////////////////////////////////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 28 /////// ///////////// //////////// ///// ////// ////////////////////////////////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 29 /////// ////// /////////// /////////// //// ////// ///// ///////////////////////////////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 30 /////// ////// ////////// // ////////// /// ////// ///// ///////////////////////////////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 31 /////// ///////// //// ///////// // ////// ///// ///////////////////////////////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 32 /////// //////// ////// //////// // ////// ///// ///////////////////////////////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 33 /////// ////// /////// /////// /// ////// ///// ///////////////////////////////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 34 /////// ////// ////// ////// //// ////// ///// ///////////////////////////////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 35 /////// ////// //////////// ///// ///// ////// ////////////////////////////////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 36 /////// ////// ////////////// //// ////// ////// /////////////////////////////////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 37 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 38 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 39 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
ThomasBNL 40:7f928b465f8d 40 // Groep 12: The Chenne Band //
ThomasBNL 40:7f928b465f8d 41 // ___________________________
ThomasBNL 40:7f928b465f8d 42 // // \\
ThomasBNL 40:7f928b465f8d 43 // || [Table of content] ||
ThomasBNL 40:7f928b465f8d 44 // \\___________________________//
ThomasBNL 40:7f928b465f8d 45 //
ThomasBNL 40:7f928b465f8d 46 // Introduction
ThomasBNL 40:7f928b465f8d 47
ThomasBNL 40:7f928b465f8d 48 // Libraries.................................................
ThomasBNL 40:7f928b465f8d 49
ThomasBNL 40:7f928b465f8d 50 //
ThomasBNL 40:7f928b465f8d 51 //
ThomasBNL 40:7f928b465f8d 52 //
ThomasBNL 40:7f928b465f8d 53 //
ThomasBNL 40:7f928b465f8d 54
ThomasBNL 40:7f928b465f8d 55
ThomasBNL 40:7f928b465f8d 56 //============================================================================================================================
ThomasBNL 40:7f928b465f8d 57 // ___________________________
ThomasBNL 40:7f928b465f8d 58 // // \\
ThomasBNL 40:7f928b465f8d 59 // || [Libraries] ||
ThomasBNL 40:7f928b465f8d 60 // \\___________________________//
ThomasBNL 40:7f928b465f8d 61 //
ThomasBNL 40:7f928b465f8d 62
bscheltinga 0:fe3896c6eeb0 63 #include "mbed.h"
bscheltinga 12:0a079e86348e 64 #include "HIDScope.h"
ThomasBNL 40:7f928b465f8d 65 #include "QEI.h"
bscheltinga 0:fe3896c6eeb0 66 #include "MODSERIAL.h"
ThomasBNL 40:7f928b465f8d 67 #include "biquadFilter.h"
ThomasBNL 40:7f928b465f8d 68 #include "encoder.h"
ThomasBNL 40:7f928b465f8d 69
ThomasBNL 40:7f928b465f8d 70 //============================================================================================================================
ThomasBNL 40:7f928b465f8d 71 // ___________________________
ThomasBNL 40:7f928b465f8d 72 // // \\
ThomasBNL 40:7f928b465f8d 73 // || [FLOW AND DEBUGGING TOOLS] ||
ThomasBNL 40:7f928b465f8d 74 // \\___________________________//
ThomasBNL 40:7f928b465f8d 75
ThomasBNL 40:7f928b465f8d 76 //HIDScope scope(1); // DEBUG : HIDSCOPE has the ability to display signals over time and can be used to monitor signals
ThomasBNL 40:7f928b465f8d 77
ThomasBNL 40:7f928b465f8d 78 MODSERIAL pc(USBTX,USBRX); // MODSERIAL : makes it possible to send messages to the computer (eg. inside Putty)
ThomasBNL 40:7f928b465f8d 79
ThomasBNL 40:7f928b465f8d 80 DigitalOut debug_led_red(LED_RED) , debug_led_green(LED_GREEN) , debug_led_blue(LED_BLUE); // DEBUG : Red, Blue and Green LED
ThomasBNL 40:7f928b465f8d 81 DigitalIn buttonL1(PTC6) , buttonL2(PTA4) , buttonH1(D2) , buttonH2(D1); // DEBUG/CALIBRATION: 4 buttons for calibration and debugging
ThomasBNL 40:7f928b465f8d 82 Ticker looptimer; // FLOW : Ticker called looptimer to set a looptimerflag that puts the volatile bool control_go to true every sample
ThomasBNL 40:7f928b465f8d 83
ThomasBNL 40:7f928b465f8d 84 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 40:7f928b465f8d 85 volatile bool looptimerflag; // CONTROLLER : boolean that controls the sample time of the whole controller
ThomasBNL 40:7f928b465f8d 86
ThomasBNL 40:7f928b465f8d 87 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 40:7f928b465f8d 88 e19, e18, e17, e16, e15, e14, e13, e12, e11, e10, e9,
ThomasBNL 40:7f928b465f8d 89 e8, e7, e6, e5, e4, e3, e2, e1, er, error_count, error_turn_average, error_strike_average;
ThomasBNL 40:7f928b465f8d 90
ThomasBNL 40:7f928b465f8d 91 AnalogIn input1(A0), input2(A1); // EMG : Two AnalogIn EMG inputs, input1 (Left bicep), input2 (Right bicep)
ThomasBNL 40:7f928b465f8d 92
ThomasBNL 40:7f928b465f8d 93 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 40:7f928b465f8d 94 Sample_EMG_L_7, Sample_EMG_L_8, Sample_EMG_L_9, Sample_EMG_L_10, moving_average_left;
ThomasBNL 40:7f928b465f8d 95 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 40:7f928b465f8d 96 Sample_EMG_R_7, Sample_EMG_R_8, Sample_EMG_R_9, Sample_EMG_R_10, moving_average_right;
ThomasBNL 40:7f928b465f8d 97
ThomasBNL 40:7f928b465f8d 98 double minimum_L, maximum_L, EMG_L_min, EMG_L_max; // EMG CALIBRATION: variables that are used during the EMG calibration
ThomasBNL 40:7f928b465f8d 99 double minimum_R, maximum_R, EMG_R_min, EMG_R_max;
ThomasBNL 40:7f928b465f8d 100
ThomasBNL 40:7f928b465f8d 101 double EMG_left_Bicep, EMG_Left_Bicep_filtered,
ThomasBNL 40:7f928b465f8d 102 EMG_Left_Bicep_filtered_notch_1, EMG_Right_Bicep_filtered_notch_1;
ThomasBNL 40:7f928b465f8d 103 double EMG_Right_Bicep, EMG_Left_Bicep_filtered_notch_2,
ThomasBNL 40:7f928b465f8d 104 EMG_Right_Bicep_filtered_notch_2, EMG_Right_Bicep_filtered;
ThomasBNL 40:7f928b465f8d 105
ThomasBNL 40:7f928b465f8d 106 double Threshold_Bicep_Left_1, Threshold_Bicep_Left_2, // EMG ACTION: variables to witch the threshold values calculated after the calibration get asigned to
ThomasBNL 40:7f928b465f8d 107 Threshold_Bicep_Right_1, Threshold_Bicep_Right_2;
ThomasBNL 40:7f928b465f8d 108
ThomasBNL 40:7f928b465f8d 109 double n=0; double c=0; double k=0; double p=0; // FLOW : these flow variables are assigned to certain values through out the script values in order to regulate the flow of the script
bscheltinga 0:fe3896c6eeb0 110
ThomasBNL 40:7f928b465f8d 111 // FILTERS EMG
ThomasBNL 40:7f928b465f8d 112 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 40:7f928b465f8d 113 low_a1 = -1.205716572226748 , low_a2 = 0.44143409003801976 ;
ThomasBNL 40:7f928b465f8d 114 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 40:7f928b465f8d 115 high_a1 = -1.1429772843080923 , high_a2 = 0.41279762014290533 ;
ThomasBNL 40:7f928b465f8d 116 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 40:7f928b465f8d 117 high_a1_HP = -1.6564788473046559 , high_a2_HP = 0.7376149706228688 ;
ThomasBNL 40:7f928b465f8d 118 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 40:7f928b465f8d 119 low_a1_LP = -1.9238184798980429 , low_a2_LP = 0.9290453117389691 ;
ThomasBNL 40:7f928b465f8d 120
ThomasBNL 40:7f928b465f8d 121 //Left bicep Filters
ThomasBNL 40:7f928b465f8d 122 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 40:7f928b465f8d 123 biquadFilter notchL1(high_a1, high_a2, high_b0, high_b1, high_b2); // EMG : moeten nog waardes voor gemaakt worden (>52Hz doorlaten)
ThomasBNL 40:7f928b465f8d 124 biquadFilter notchL2(low_a1, low_a2, low_b0, low_b1, low_b2); // EMG : moeten nog waardes voor gemaakt worden (<48Hz doorlaten)
ThomasBNL 40:7f928b465f8d 125 biquadFilter lowpassfilter_1(low_a1_LP, low_a2_LP, low_b0_LP, low_b1_LP, low_b2_LP);
ThomasBNL 40:7f928b465f8d 126
ThomasBNL 40:7f928b465f8d 127 // Right bicep Filters
ThomasBNL 40:7f928b465f8d 128 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 40:7f928b465f8d 129 biquadFilter notchR1(high_a1, high_a2, high_b0, high_b1, high_b2); // EMG : moeten nog waardes voor gemaakt worden
ThomasBNL 40:7f928b465f8d 130 biquadFilter notchR2(low_a1, low_a2, low_b0, low_b1, low_b2); // EMG : moeten nog waardes voor gemaakt worden
ThomasBNL 40:7f928b465f8d 131 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 40:7f928b465f8d 132
ThomasBNL 36:f29a36683b1a 133
ThomasBNL 40:7f928b465f8d 134 // MOTORS
ThomasBNL 40:7f928b465f8d 135 QEI motor_turn(D12,D13,NC,32); QEI motor_strike(D9,D10,NC,32); // TURN - STRIKE : Encoder for motor
ThomasBNL 40:7f928b465f8d 136 PwmOut pwm_motor_turn(D5); PwmOut pwm_motor_strike(D6); // TURN - STRIKE : Pwm for motor
ThomasBNL 40:7f928b465f8d 137 DigitalOut motordirection_turn(D4); DigitalOut motordirection_strike(D7); // TURN - STRIKE : Direction of the motor
ThomasBNL 40:7f928b465f8d 138
ThomasBNL 40:7f928b465f8d 139 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 40:7f928b465f8d 140
ThomasBNL 40:7f928b465f8d 141 double position_turn, error_turn, reference_turn; double position_strike, error_strike, reference_strike;
ThomasBNL 40:7f928b465f8d 142 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 40:7f928b465f8d 143 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 40:7f928b465f8d 144 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 40:7f928b465f8d 145
ThomasBNL 40:7f928b465f8d 146
ThomasBNL 40:7f928b465f8d 147 // FILTER: D-action
ThomasBNL 40:7f928b465f8d 148 const double a0 = 0.000332685098529822, a1 = 0.000665370197059644, a2 = 0.000332685098529822, b1 = -1.9625271814290315, b2 = 0.9638579218231508;
ThomasBNL 40:7f928b465f8d 149 // const double a0 = 0.000021080713160785432, a1 = 0.000042161426321570865, a2 = 0.000021080713160785432, b1 = -1.990754082898736, b2 = 0.9908384057513788; (0.75Hz)
ThomasBNL 40:7f928b465f8d 150 // const double a0 = 0.003543360146633312, a1 = 0.007086720293266624, a2 = 0.003543360146633312, b1 = -1.8704759567901301, b2 = 0.8846493973766635; (10Hz)
ThomasBNL 40:7f928b465f8d 151 // const double a0 = 0.0009129521023626334, a1 = 0.0018259042047252668, a2 = 0.0009129521023626334, b1 = -1.9368516414202819, b2 = 0.9405034498297324; (5Hz)
ThomasBNL 40:7f928b465f8d 152 biquadFilter Lowpassfilter_derivative(b1,b2,a0,a1,a2); // BiquadFilter used for the filtering of the Derivative action of the PID-action
ThomasBNL 40:7f928b465f8d 153
ThomasBNL 40:7f928b465f8d 154 const double cw=0; // MOTOR: turn direction zero is clockwise (front view)
ThomasBNL 40:7f928b465f8d 155 const double ccw=1; // MOTOR: turn direction one is counterclockwise (front view)
sigert 37:6c04c15d9bbe 156 const double off=1; // Led off
sigert 37:6c04c15d9bbe 157 const double on=0; // Led on
ThomasBNL 40:7f928b465f8d 158 const int Fs = 512; // sampling frequency (512 Hz)
ThomasBNL 40:7f928b465f8d 159 const double sample_time = 0.001953125; // duration of one sample
ThomasBNL 40:7f928b465f8d 160 double conversion_counts_to_degrees=0.085877862594198; // Calculation conversion counts to degrees
ThomasBNL 40:7f928b465f8d 161 // gear ratio motor = 131
ThomasBNL 40:7f928b465f8d 162 // ticks per magnet rotation = 32 (X2 Encoder)
ThomasBNL 40:7f928b465f8d 163 // One revolution = 360 degrees
ThomasBNL 40:7f928b465f8d 164 // degrees_per_encoder_tick = 360/(gear_ratio*ticks_per_magnet_rotation)=360/131*32=0.085877862594198
ThomasBNL 40:7f928b465f8d 165 const double change_one_bottle=45;
ThomasBNL 40:7f928b465f8d 166 const double Hit=60; // position when bottle is hit
bscheltinga 15:7870f7912904 167
ThomasBNL 40:7f928b465f8d 168
ThomasBNL 40:7f928b465f8d 169 //============================================================================================================================
ThomasBNL 40:7f928b465f8d 170 // ___________________________
ThomasBNL 40:7f928b465f8d 171 // // \\
ThomasBNL 40:7f928b465f8d 172 // || [FUNCTIONS USED] ||
ThomasBNL 40:7f928b465f8d 173 // \\___________________________//
ThomasBNL 40:7f928b465f8d 174 void execute_plant_turn (); // TURN : Check error -> execute PID controller -> write pwm and direction to motor
ThomasBNL 40:7f928b465f8d 175 void execute_plant_strike ();
ThomasBNL 40:7f928b465f8d 176 double PID_control (double reference, double position, double &integrate_error,
ThomasBNL 40:7f928b465f8d 177 double sample_time, double &previous_error,
ThomasBNL 40:7f928b465f8d 178 double P_gain, double I_gain, double D_gain);
ThomasBNL 40:7f928b465f8d 179
ThomasBNL 40:7f928b465f8d 180 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 40:7f928b465f8d 181 void setlooptimerflag (void); // Sets looptimerflag volatile bool to true
ThomasBNL 40:7f928b465f8d 182
ThomasBNL 40:7f928b465f8d 183 void deactivate_PID_Controller (double& P_gain, double& I_gain, double& D_gain); // STRIKE/TURN: Deactivate PID Controller
bscheltinga 15:7870f7912904 184
ThomasBNL 40:7f928b465f8d 185 void activate_PID_Controller_strike (double& P_gain, double& I_gain, double& D_gain); // STRIKE: Activate PID Controller
ThomasBNL 40:7f928b465f8d 186 void activate_PID_Controller_turn (double& P_gain, double& I_gain, double& D_gain); // TURN : Activate PID Controller
ThomasBNL 40:7f928b465f8d 187
ThomasBNL 40:7f928b465f8d 188 void Change_Turn_Position_Left (double& reference, double change_one_bottle); // TURN : Change Reference position one bottle to the left
ThomasBNL 40:7f928b465f8d 189 void Change_Turn_Position_Right (double& reference, double change_one_bottle); // TURN : Change Reference position one bottle to the right
ThomasBNL 40:7f928b465f8d 190
ThomasBNL 40:7f928b465f8d 191 void countdown_from_5(); // PUTTY : 5 second countdown inside
ThomasBNL 40:7f928b465f8d 192 void calibrate_motor(); // MOTOR : Calibrate starting position motor
ThomasBNL 40:7f928b465f8d 193 void calibration(); // EMG : Calibrate the EMG signal (calculate min and max signal and determine threshold values)
ThomasBNL 40:7f928b465f8d 194 void Filter(); // EMG : Filter the incoming EMG signals
ThomasBNL 40:7f928b465f8d 195 void sample_filter(); // EMG : Calculate moving average (10 samples, one sample per 25 samples) using sample_filter => moving average over +-0.5 seconds
ThomasBNL 40:7f928b465f8d 196 void take_sample(); // EMG : Take a sample once every 25 samples that's used to calculate the moving average
ThomasBNL 40:7f928b465f8d 197 void ControlGo(); // EMG : function that gets a ticker attached and sets a certain loop to true every sample
ThomasBNL 40:7f928b465f8d 198
ThomasBNL 40:7f928b465f8d 199 void red();void blue();void green();void white();void yellow();void cyan();void purple(); void black(); // DEBUG: Different color LEDS
ThomasBNL 40:7f928b465f8d 200
ThomasBNL 40:7f928b465f8d 201 void calibrate_potmeter(); // DEBUG/TEST : Calibration thresholds with potmeter
bscheltinga 22:14abcfdd1554 202
ThomasBNL 40:7f928b465f8d 203 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 40:7f928b465f8d 204
ThomasBNL 40:7f928b465f8d 205 //============================================================================================================================
ThomasBNL 40:7f928b465f8d 206 ///////////////////////////////
ThomasBNL 40:7f928b465f8d 207 // //
ThomasBNL 40:7f928b465f8d 208 ///////////////////////////////////////////// [MAIN FUNCTION] /////////////////////////////////////////////////////
ThomasBNL 40:7f928b465f8d 209 // //
ThomasBNL 40:7f928b465f8d 210 ///////////////////////////////
ThomasBNL 40:7f928b465f8d 211 //============================================================================================================================
ThomasBNL 40:7f928b465f8d 212 int main() {
ThomasBNL 40:7f928b465f8d 213
ThomasBNL 40:7f928b465f8d 214 black(); // No LED active
ThomasBNL 40:7f928b465f8d 215 pc.printf("Start of code \n\r");
ThomasBNL 40:7f928b465f8d 216
ThomasBNL 40:7f928b465f8d 217 pc.baud(115200); // PC contactspeed : Set the baudrate
ThomasBNL 40:7f928b465f8d 218
ThomasBNL 40:7f928b465f8d 219 red();
ThomasBNL 40:7f928b465f8d 220 calibrate_motor(); // MOTOR CALIBRATE : The motor starting position (RED LED)
ThomasBNL 40:7f928b465f8d 221
ThomasBNL 40:7f928b465f8d 222 //blue();
ThomasBNL 40:7f928b465f8d 223 calibration(); // EMG CALIBRATE : The motor starting position (BLUE LED)
ThomasBNL 40:7f928b465f8d 224
ThomasBNL 40:7f928b465f8d 225 calibrate_potmeter(); // DEBUG/TEST : Calibration thresholds with potmeter
ThomasBNL 40:7f928b465f8d 226
ThomasBNL 40:7f928b465f8d 227 looptimer.attach(setlooptimerflag,(float)1/Fs); // CONTROLLER FLOW : Calls the looptimer flag every sample
ThomasBNL 40:7f928b465f8d 228
ThomasBNL 40:7f928b465f8d 229 black();
ThomasBNL 40:7f928b465f8d 230 wait (3); // REST before starting position
ThomasBNL 40:7f928b465f8d 231
ThomasBNL 40:7f928b465f8d 232 green(); // START CONTROLLOOP (GREEN LED)
ThomasBNL 40:7f928b465f8d 233
ThomasBNL 40:7f928b465f8d 234 while(1)
ThomasBNL 40:7f928b465f8d 235 {
ThomasBNL 40:7f928b465f8d 236 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 40:7f928b465f8d 237 }
ThomasBNL 40:7f928b465f8d 238 }
sigert 37:6c04c15d9bbe 239
ThomasBNL 40:7f928b465f8d 240 //============================================================================================================================
ThomasBNL 40:7f928b465f8d 241 ///////////////////////////////
ThomasBNL 40:7f928b465f8d 242 // //
ThomasBNL 40:7f928b465f8d 243 ///////////////////////////////////////////// [FUNCTIONS DESCRIBED] /////////////////////////////////////////////////////
ThomasBNL 40:7f928b465f8d 244 // //
ThomasBNL 40:7f928b465f8d 245 ///////////////////////////////
ThomasBNL 40:7f928b465f8d 246 //============================================================================================================================
bscheltinga 20:d5f5c60adc43 247
ThomasBNL 27:85e5d36bb6c5 248
ThomasBNL 40:7f928b465f8d 249 // FUNCTION 1 ___________________________
ThomasBNL 40:7f928b465f8d 250 // // \\
ThomasBNL 40:7f928b465f8d 251 // || [ACTIONCONTROLLER] ||
ThomasBNL 40:7f928b465f8d 252 // \\___________________________//
ThomasBNL 40:7f928b465f8d 253 void Action_Controller()
ThomasBNL 40:7f928b465f8d 254 { // Start while loop
ThomasBNL 40:7f928b465f8d 255 while(looptimerflag != true);
ThomasBNL 40:7f928b465f8d 256 looptimerflag = false;
ThomasBNL 40:7f928b465f8d 257
ThomasBNL 40:7f928b465f8d 258 Nieuwe_actie: // Return here if action left, right or strike is executed
ThomasBNL 40:7f928b465f8d 259 green(); // GREEN LED: ready to fire again
ThomasBNL 40:7f928b465f8d 260
ThomasBNL 40:7f928b465f8d 261 //// sample_filter(); // TIJDELIJK UIT: What is the current EMG value
ThomasBNL 40:7f928b465f8d 262
ThomasBNL 40:7f928b465f8d 263 // POTMETER: SIMULATE EMG SIGNAL
ThomasBNL 40:7f928b465f8d 264 moving_average_left = (input1.read())*100; // EMG Right bicep (tussen nul en 100%) // DEBUG TEST TOOL: substitute EMG input for potmeter inputs
ThomasBNL 40:7f928b465f8d 265 moving_average_right = (input2.read())*100; // EMG Left bicep (tussen nul en 100%) // DEBUG TEST TOOL: substitute EMG input for potmeter inputs
ThomasBNL 40:7f928b465f8d 266 pc.printf("mov_r = %f, mov_l = %f \n\r", moving_average_right, moving_average_left);
ThomasBNL 36:f29a36683b1a 267
ThomasBNL 40:7f928b465f8d 268 // ___________________________
ThomasBNL 40:7f928b465f8d 269 // : [Action 2: Turn Left] :
ThomasBNL 40:7f928b465f8d 270 // :___________________________:
ThomasBNL 40:7f928b465f8d 271 //
ThomasBNL 40:7f928b465f8d 272 // //Blue (strike) - Yellow (return)\\
ThomasBNL 36:f29a36683b1a 273
ThomasBNL 40:7f928b465f8d 274 // TEMPORARY: TO TEST STRIKE MECHANISM
ThomasBNL 40:7f928b465f8d 275 moving_average_left = 40; // TIJDELIJK PID TEST
ThomasBNL 40:7f928b465f8d 276 moving_average_right = 40; // TIJDELIJK PID TEST
ThomasBNL 40:7f928b465f8d 277
ThomasBNL 40:7f928b465f8d 278 if (moving_average_right > Threshold_Bicep_Right_1 && moving_average_left > Threshold_Bicep_Left_1) // Statement 1 (if both are satisfied execute)
ThomasBNL 40:7f928b465f8d 279 { // Statement 1 start
ThomasBNL 40:7f928b465f8d 280 blue(); n=0; k=0; p=0;
ThomasBNL 40:7f928b465f8d 281 pc.printf("Slag \n\r");
ThomasBNL 40:7f928b465f8d 282 wait(0.5); // TIJDELIJK??
ThomasBNL 40:7f928b465f8d 283
ThomasBNL 40:7f928b465f8d 284 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 40:7f928b465f8d 285 { // Statement 2 start
ThomasBNL 40:7f928b465f8d 286 while(1)
ThomasBNL 40:7f928b465f8d 287 { // inf while loop strike until finished start
ThomasBNL 40:7f928b465f8d 288 if (n==0) // Change the reference point of the PID Strike controller
ThomasBNL 40:7f928b465f8d 289 { reference_strike=90; n=1; error_count=0; } // Execute once (n is set to one and only gets executed if n equals zero)
ThomasBNL 40:7f928b465f8d 290
ThomasBNL 40:7f928b465f8d 291 if (looptimerflag == true) // Loop that executes the strike controller every sample (inside the controller the loudness is regulated)
ThomasBNL 40:7f928b465f8d 292 {
ThomasBNL 40:7f928b465f8d 293 looptimerflag=false;
ThomasBNL 40:7f928b465f8d 294 activate_PID_Controller_strike(P_gain_strike, I_gain_strike, D_gain_strike);
ThomasBNL 40:7f928b465f8d 295 execute_plant_strike();
ThomasBNL 40:7f928b465f8d 296 }
ThomasBNL 40:7f928b465f8d 297
ThomasBNL 40:7f928b465f8d 298 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 40:7f928b465f8d 299 { // Statement Return start
ThomasBNL 40:7f928b465f8d 300 while(1)
ThomasBNL 40:7f928b465f8d 301 { // inf while loop return after strike start
ThomasBNL 40:7f928b465f8d 302 yellow();
ThomasBNL 40:7f928b465f8d 303 if(k==0) // Change reference point of the PID Strike controller back to the original position
ThomasBNL 40:7f928b465f8d 304 {
ThomasBNL 40:7f928b465f8d 305 p=1; reference_strike=0; error_count=0; k=1;
ThomasBNL 40:7f928b465f8d 306 pc.printf("return \n\r");
ThomasBNL 40:7f928b465f8d 307 }
ThomasBNL 40:7f928b465f8d 308 //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 40:7f928b465f8d 309
ThomasBNL 40:7f928b465f8d 310 if (looptimerflag == true) // Loop that executes the strike controller every sample (loudness is deactivated by the value of p)
ThomasBNL 40:7f928b465f8d 311 {
ThomasBNL 40:7f928b465f8d 312 looptimerflag=false;
ThomasBNL 40:7f928b465f8d 313 activate_PID_Controller_strike(P_gain_strike, I_gain_strike, D_gain_strike);
ThomasBNL 40:7f928b465f8d 314 execute_plant_strike();
ThomasBNL 40:7f928b465f8d 315 }
ThomasBNL 40:7f928b465f8d 316
ThomasBNL 40:7f928b465f8d 317 printf(" %f \n\r",error_strike_average); // LINE USED FOR TESTING
ThomasBNL 40:7f928b465f8d 318
ThomasBNL 40:7f928b465f8d 319 if (fabs(error_strike_average) < 0.5 && error_count>100) // If error is small enough and at least 100 samples have passed since the return execute new action
ThomasBNL 40:7f928b465f8d 320 {
ThomasBNL 40:7f928b465f8d 321 yellow();
ThomasBNL 40:7f928b465f8d 322 pc.printf("new action \n\r");
ThomasBNL 40:7f928b465f8d 323 deactivate_PID_Controller(P_gain_strike, I_gain_strike, D_gain_strike);
ThomasBNL 40:7f928b465f8d 324 execute_plant_strike();
ThomasBNL 40:7f928b465f8d 325 goto Nieuwe_actie;
ThomasBNL 40:7f928b465f8d 326 }
ThomasBNL 40:7f928b465f8d 327 } // inf while loop return after strike end
ThomasBNL 40:7f928b465f8d 328 } // Statement Return end
ThomasBNL 40:7f928b465f8d 329 } // inf while loop strike until finished end
ThomasBNL 40:7f928b465f8d 330 } // Statement 2 end
ThomasBNL 40:7f928b465f8d 331 } // Statement 1 end
ThomasBNL 40:7f928b465f8d 332 // ___________________________
ThomasBNL 40:7f928b465f8d 333 // : [Action 2: Turn Left] :
ThomasBNL 40:7f928b465f8d 334 // :___________________________:
ThomasBNL 40:7f928b465f8d 335 //
ThomasBNL 40:7f928b465f8d 336 // //Yellow\\
ThomasBNL 36:f29a36683b1a 337
ThomasBNL 40:7f928b465f8d 338 if (moving_average_left > Threshold_Bicep_Left_2 && moving_average_right < Threshold_Bicep_Right_1)
ThomasBNL 40:7f928b465f8d 339 {
ThomasBNL 40:7f928b465f8d 340 yellow(); n=0;
ThomasBNL 40:7f928b465f8d 341 pc.printf("LEFT \n\r");
ThomasBNL 40:7f928b465f8d 342 wait(2); // TIJDELIJK
ThomasBNL 40:7f928b465f8d 343
ThomasBNL 40:7f928b465f8d 344 while(moving_average_left > Threshold_Bicep_Left_1 && moving_average_right < Threshold_Bicep_Right_1)
ThomasBNL 40:7f928b465f8d 345 {
ThomasBNL 40:7f928b465f8d 346 if (n==0)
ThomasBNL 40:7f928b465f8d 347 {
ThomasBNL 40:7f928b465f8d 348 Change_Turn_Position_Left(reference_turn, change_one_bottle);
ThomasBNL 40:7f928b465f8d 349 n=1; error_count=0;
ThomasBNL 40:7f928b465f8d 350 }
ThomasBNL 40:7f928b465f8d 351
ThomasBNL 40:7f928b465f8d 352 // 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 40:7f928b465f8d 353
ThomasBNL 40:7f928b465f8d 354 if (looptimerflag == true)
ThomasBNL 40:7f928b465f8d 355 {
ThomasBNL 40:7f928b465f8d 356 looptimerflag=false;
ThomasBNL 40:7f928b465f8d 357 activate_PID_Controller_turn(P_gain_turn, I_gain_turn, D_gain_turn);
ThomasBNL 40:7f928b465f8d 358 execute_plant_turn();
ThomasBNL 40:7f928b465f8d 359 }
ThomasBNL 40:7f928b465f8d 360
ThomasBNL 40:7f928b465f8d 361 if (fabs(error_turn_average) < 1 && error_count>250)
ThomasBNL 40:7f928b465f8d 362 {
ThomasBNL 40:7f928b465f8d 363 pc.printf("new action \n\r");
ThomasBNL 40:7f928b465f8d 364 deactivate_PID_Controller(P_gain_turn, I_gain_turn, D_gain_turn);
ThomasBNL 40:7f928b465f8d 365 execute_plant_turn();
ThomasBNL 40:7f928b465f8d 366 goto Nieuwe_actie;
ThomasBNL 40:7f928b465f8d 367 }
ThomasBNL 40:7f928b465f8d 368 }
ThomasBNL 40:7f928b465f8d 369 }
ThomasBNL 40:7f928b465f8d 370 // ___________________________
ThomasBNL 40:7f928b465f8d 371 // : [Action 3: Turn Right] :
ThomasBNL 40:7f928b465f8d 372 // :___________________________:
ThomasBNL 40:7f928b465f8d 373 //
ThomasBNL 40:7f928b465f8d 374 // //Purple\\
ThomasBNL 36:f29a36683b1a 375
ThomasBNL 40:7f928b465f8d 376 if (moving_average_right > Threshold_Bicep_Right_2 && moving_average_left < Threshold_Bicep_Left_1)
ThomasBNL 40:7f928b465f8d 377 {
ThomasBNL 40:7f928b465f8d 378 purple(); n=0;
ThomasBNL 40:7f928b465f8d 379 pc.printf("Right \n\r");
ThomasBNL 40:7f928b465f8d 380 wait(2); // TIJDELIJK
ThomasBNL 40:7f928b465f8d 381
ThomasBNL 40:7f928b465f8d 382 while(moving_average_right > Threshold_Bicep_Right_1 && moving_average_left < Threshold_Bicep_Left_1)
ThomasBNL 40:7f928b465f8d 383 {
ThomasBNL 40:7f928b465f8d 384 if (n==0)
ThomasBNL 40:7f928b465f8d 385 {
ThomasBNL 40:7f928b465f8d 386 Change_Turn_Position_Right(reference_turn, change_one_bottle);
ThomasBNL 40:7f928b465f8d 387 n=1; error_count=0;
ThomasBNL 40:7f928b465f8d 388 }
ThomasBNL 40:7f928b465f8d 389
ThomasBNL 40:7f928b465f8d 390 // 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 40:7f928b465f8d 391
ThomasBNL 40:7f928b465f8d 392 if (looptimerflag == true)
ThomasBNL 40:7f928b465f8d 393 {
ThomasBNL 40:7f928b465f8d 394 looptimerflag=false;
ThomasBNL 40:7f928b465f8d 395 activate_PID_Controller_turn(P_gain_turn, I_gain_turn, D_gain_turn);
ThomasBNL 40:7f928b465f8d 396 execute_plant_turn();
ThomasBNL 40:7f928b465f8d 397 }
ThomasBNL 40:7f928b465f8d 398
ThomasBNL 40:7f928b465f8d 399 if (fabs(error_turn_average) < 1 && error_count>250)
ThomasBNL 40:7f928b465f8d 400 {
ThomasBNL 40:7f928b465f8d 401 pc.printf("new action \n\r");
ThomasBNL 40:7f928b465f8d 402 deactivate_PID_Controller(P_gain_turn, I_gain_turn, D_gain_turn);
ThomasBNL 40:7f928b465f8d 403 execute_plant_turn();
ThomasBNL 40:7f928b465f8d 404 goto Nieuwe_actie;
ThomasBNL 40:7f928b465f8d 405 }
ThomasBNL 40:7f928b465f8d 406 }
ThomasBNL 40:7f928b465f8d 407 }
ThomasBNL 40:7f928b465f8d 408 }
ThomasBNL 36:f29a36683b1a 409
ThomasBNL 40:7f928b465f8d 410 // ___________________________
ThomasBNL 40:7f928b465f8d 411 // // \\
ThomasBNL 40:7f928b465f8d 412 // || [CALIBRATE] ||
ThomasBNL 40:7f928b465f8d 413 // \\___________________________//
ThomasBNL 40:7f928b465f8d 414 // Calibrate starting postion (RED LED)
ThomasBNL 40:7f928b465f8d 415 // ___________________________
ThomasBNL 40:7f928b465f8d 416 // : [Starting position motor] :
ThomasBNL 40:7f928b465f8d 417 // :___________________________:
ThomasBNL 40:7f928b465f8d 418 //
ThomasBNL 40:7f928b465f8d 419
ThomasBNL 40:7f928b465f8d 420 void calibrate_motor()
ThomasBNL 40:7f928b465f8d 421 {
ThomasBNL 40:7f928b465f8d 422 pc.printf("Button calibration \n\r");
ThomasBNL 27:85e5d36bb6c5 423 while(1)
ThomasBNL 40:7f928b465f8d 424 {
ThomasBNL 40:7f928b465f8d 425 red();// RED LED
ThomasBNL 40:7f928b465f8d 426
ThomasBNL 40:7f928b465f8d 427 if (buttonL2.read() < 0.5)
ThomasBNL 40:7f928b465f8d 428 { motordirection_turn = cw;
ThomasBNL 40:7f928b465f8d 429 pwm_motor_turn = 0.02; }
ThomasBNL 40:7f928b465f8d 430
ThomasBNL 40:7f928b465f8d 431 if (buttonL1.read() < 0.5){
ThomasBNL 40:7f928b465f8d 432 { motordirection_turn = ccw;
ThomasBNL 40:7f928b465f8d 433 pwm_motor_turn = 0.02; }
ThomasBNL 40:7f928b465f8d 434
ThomasBNL 40:7f928b465f8d 435 pwm_motor_turn = 0;
ThomasBNL 40:7f928b465f8d 436
ThomasBNL 40:7f928b465f8d 437 if (buttonH2.read() < 0.5){
ThomasBNL 40:7f928b465f8d 438 { motordirection_strike = cw;
ThomasBNL 40:7f928b465f8d 439 pwm_motor_strike = 0.02; }
ThomasBNL 40:7f928b465f8d 440
ThomasBNL 40:7f928b465f8d 441 if (buttonH1.read() < 0.5){
ThomasBNL 40:7f928b465f8d 442 { motordirection_strike = ccw;
ThomasBNL 40:7f928b465f8d 443 pwm_motor_strike = 0.02; }
ThomasBNL 40:7f928b465f8d 444
ThomasBNL 40:7f928b465f8d 445 pwm_motor_strike = 0;
ThomasBNL 40:7f928b465f8d 446
ThomasBNL 40:7f928b465f8d 447 if ((buttonL2.read() < 0.5) && (buttonL1.read() < 0.5)) // Save current TURN and STRIKE positions as starting position
ThomasBNL 40:7f928b465f8d 448 {
ThomasBNL 40:7f928b465f8d 449 motor_turn.reset(); // TURN : Starting Position
ThomasBNL 40:7f928b465f8d 450 reference_turn=0; // TURN : Set reference position to zero
ThomasBNL 40:7f928b465f8d 451 motor_strike.reset(); // STRIKE : Starting Position
ThomasBNL 40:7f928b465f8d 452 goto calibration_starting_position_complete; // Calibration complete (exit while loop)
ThomasBNL 40:7f928b465f8d 453 }
ThomasBNL 40:7f928b465f8d 454 }
ThomasBNL 40:7f928b465f8d 455 calibration_starting_position_complete:
ThomasBNL 40:7f928b465f8d 456 }
ThomasBNL 40:7f928b465f8d 457
ThomasBNL 40:7f928b465f8d 458 // ___________________________
ThomasBNL 40:7f928b465f8d 459 // // \\
ThomasBNL 40:7f928b465f8d 460 // || [EMG_Calibration] ||
ThomasBNL 40:7f928b465f8d 461 // \\___________________________//
ThomasBNL 40:7f928b465f8d 462 //
ThomasBNL 40:7f928b465f8d 463 void calibration()
ThomasBNL 27:85e5d36bb6c5 464 {
ThomasBNL 40:7f928b465f8d 465 // ___________________________________
ThomasBNL 40:7f928b465f8d 466 // : [Minimum value bicep calibration] :
ThomasBNL 40:7f928b465f8d 467 // :___________________________________:
ThomasBNL 40:7f928b465f8d 468
ThomasBNL 40:7f928b465f8d 469 blue();
ThomasBNL 40:7f928b465f8d 470 pc.printf("Start minimum calibration in 5 seconds \n\r");
ThomasBNL 40:7f928b465f8d 471 pc.printf("Keep your biceps as relaxed as possible \n\r");
ThomasBNL 40:7f928b465f8d 472
ThomasBNL 40:7f928b465f8d 473 countdown_from_5();
ThomasBNL 40:7f928b465f8d 474 c=0;
ThomasBNL 40:7f928b465f8d 475
ThomasBNL 40:7f928b465f8d 476 while(c<2560) // 512Hz -> 2560 is equal to five seconds
ThomasBNL 40:7f928b465f8d 477 {
ThomasBNL 40:7f928b465f8d 478 Filter(); // Filter EMG signal
ThomasBNL 40:7f928b465f8d 479 minimum_L=EMG_Left_Bicep_filtered+minimum_L; // Take previous sample EMG_Left_Bicep_filtered and add the new value
ThomasBNL 40:7f928b465f8d 480 minimum_R=EMG_Right_Bicep_filtered+minimum_R;
ThomasBNL 40:7f928b465f8d 481 c++; // Every sample c is increased by one until the statement c<2560 is false
ThomasBNL 40:7f928b465f8d 482 wait(0.001953125); // wait one sample
ThomasBNL 40:7f928b465f8d 483 }
ThomasBNL 40:7f928b465f8d 484
ThomasBNL 40:7f928b465f8d 485 pc.printf("Finished minimum calibration \n\r");
ThomasBNL 40:7f928b465f8d 486
ThomasBNL 40:7f928b465f8d 487 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 40:7f928b465f8d 488 EMG_R_min=minimum_R/2560;
ThomasBNL 40:7f928b465f8d 489
ThomasBNL 40:7f928b465f8d 490 pc.printf("EMG_L_min = %f \n\r EMG_R_min = %f \n\r", EMG_L_min, EMG_R_min);
ThomasBNL 40:7f928b465f8d 491
ThomasBNL 40:7f928b465f8d 492 wait (3); // Cooldown
ThomasBNL 40:7f928b465f8d 493
ThomasBNL 40:7f928b465f8d 494 // ___________________________________
ThomasBNL 40:7f928b465f8d 495 // : [Maximum value bicep calibration] :
ThomasBNL 40:7f928b465f8d 496 // :___________________________________:
ThomasBNL 40:7f928b465f8d 497 //
ThomasBNL 40:7f928b465f8d 498 pc.printf("start maximum calibration in 5 seconds (start contraction at 3) \n\r");
ThomasBNL 40:7f928b465f8d 499
ThomasBNL 40:7f928b465f8d 500 countdown_from_5();
ThomasBNL 40:7f928b465f8d 501 c=0;
ThomasBNL 40:7f928b465f8d 502
ThomasBNL 40:7f928b465f8d 503 while(c<2560) // 512Hz -> 2560 is equal to five seconds
ThomasBNL 40:7f928b465f8d 504 {
ThomasBNL 40:7f928b465f8d 505 Filter(); // Filter EMG signal
ThomasBNL 40:7f928b465f8d 506 maximum_L=EMG_Left_Bicep_filtered+maximum_L; // Take previous sample EMG_Left_Bicep_filtered and add the new value
ThomasBNL 40:7f928b465f8d 507 maximum_R=EMG_Right_Bicep_filtered+maximum_R;
ThomasBNL 40:7f928b465f8d 508 c++; // Every sample c is increased by one until the statement c<2560 is false
ThomasBNL 40:7f928b465f8d 509 wait(0.001953125);
ThomasBNL 40:7f928b465f8d 510 }
ThomasBNL 40:7f928b465f8d 511
ThomasBNL 40:7f928b465f8d 512 pc.printf("Finished minimum calibration \n\r");
ThomasBNL 40:7f928b465f8d 513
ThomasBNL 40:7f928b465f8d 514 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 40:7f928b465f8d 515 EMG_R_max=maximum_R/2560;
ThomasBNL 40:7f928b465f8d 516
ThomasBNL 40:7f928b465f8d 517 pc.printf("EMG_L_max = %f \n\r EMG_R_max = %f \n\r", EMG_L_max, EMG_R_max);
ThomasBNL 40:7f928b465f8d 518
ThomasBNL 40:7f928b465f8d 519 wait (3); // Cooldown
ThomasBNL 40:7f928b465f8d 520
ThomasBNL 40:7f928b465f8d 521
ThomasBNL 40:7f928b465f8d 522 // _______________________________________________
ThomasBNL 40:7f928b465f8d 523 // : [THRESHOLDS CALCULATION FROM MIN AND MAX EMG] :
ThomasBNL 40:7f928b465f8d 524 // :_______________________________________________:
ThomasBNL 40:7f928b465f8d 525 //
ThomasBNL 40:7f928b465f8d 526
ThomasBNL 40:7f928b465f8d 527 const float Threshold_Bicep_Left_1=((EMG_L_max-EMG_L_min)*0.2)+EMG_L_min;; // EMG LEFT: Threshold put at 20% of the EMG range
ThomasBNL 40:7f928b465f8d 528 const float Threshold_Bicep_Left_2=((EMG_L_max-EMG_L_min)*0.6)+EMG_L_min; // EMG LEFT: Threshold put at 60% of the EMG range
ThomasBNL 40:7f928b465f8d 529 const float Threshold_Bicep_Right_1=((EMG_R_max-EMG_R_min)*0.2)+EMG_R_min; // EMG RIGHT: Threshold put at 20% of the EMG range
ThomasBNL 40:7f928b465f8d 530 const float Threshold_Bicep_Right_2=((EMG_R_max-EMG_R_min)*0.6)+EMG_R_min; // EMG RIGHT: Threshold put at 60% of the EMG range
ThomasBNL 40:7f928b465f8d 531
ThomasBNL 40:7f928b465f8d 532 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 40:7f928b465f8d 533 }
ThomasBNL 40:7f928b465f8d 534
ThomasBNL 40:7f928b465f8d 535 // ___________________________
ThomasBNL 40:7f928b465f8d 536 // // \\
ThomasBNL 40:7f928b465f8d 537 // || [TURN PLANT] ||
ThomasBNL 40:7f928b465f8d 538 // \\___________________________//
ThomasBNL 40:7f928b465f8d 539
ThomasBNL 40:7f928b465f8d 540 void execute_plant_turn()
ThomasBNL 40:7f928b465f8d 541 {
ThomasBNL 40:7f928b465f8d 542 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 40:7f928b465f8d 543 { motor_turn.reset(); }
ThomasBNL 40:7f928b465f8d 544
ThomasBNL 40:7f928b465f8d 545 position_turn = conversion_counts_to_degrees * motor_turn.getPulses(); // Convert counts to degrees
ThomasBNL 40:7f928b465f8d 546
ThomasBNL 40:7f928b465f8d 547 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 40:7f928b465f8d 548 // Execute PID controller and calculate the pwm to put to the motor
ThomasBNL 40:7f928b465f8d 549
ThomasBNL 40:7f928b465f8d 550 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 40:7f928b465f8d 551
ThomasBNL 40:7f928b465f8d 552 //pc.printf("pwm %f \n\r", pwm_to_motor_turn); // LINE FOR TESTING
ThomasBNL 40:7f928b465f8d 553
ThomasBNL 40:7f928b465f8d 554 if(pwm_to_motor_turn > 0) // Check error and decide the direction the motor has to turn
ThomasBNL 40:7f928b465f8d 555 { motordirection_turn=ccw;}
ThomasBNL 40:7f928b465f8d 556 else
ThomasBNL 40:7f928b465f8d 557 { motordirection_turn=cw; }
ThomasBNL 40:7f928b465f8d 558
ThomasBNL 40:7f928b465f8d 559 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 40:7f928b465f8d 560
ThomasBNL 40:7f928b465f8d 561 take_sample(); // TEMPORARY -> use sample_filter() normally
ThomasBNL 40:7f928b465f8d 562
ThomasBNL 40:7f928b465f8d 563 // sample_filter();
ThomasBNL 40:7f928b465f8d 564
ThomasBNL 40:7f928b465f8d 565 if(sample_error) // sample_error -- e10;e9;e8;e7;e6;e5:e4;e3;e2;e1 -- error_turn_average --- er
ThomasBNL 40:7f928b465f8d 566 {
ThomasBNL 40:7f928b465f8d 567 sample_error=false;
ThomasBNL 40:7f928b465f8d 568 e1 = (position_turn - reference_turn);
ThomasBNL 40:7f928b465f8d 569 e30=e29; e29=e28 ;e28=e27; e27=e26; e26=e25; e25=e24; e24=e23; e23=e22; e22=e21; e21=e20;
ThomasBNL 40:7f928b465f8d 570 e20=e19; e19=e18 ;e18=e17; e17=e16; e16=e15; e15=e14; e14=e13; e13=e12; e12=e11; e11=e10;
ThomasBNL 40:7f928b465f8d 571 e10=e9 ;e9=e8; e8=e7; e7=e6; e6=e5; e5=e4; e4=e3; e3=e2; e2=e1;
ThomasBNL 40:7f928b465f8d 572 }
ThomasBNL 40:7f928b465f8d 573
ThomasBNL 40:7f928b465f8d 574 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 40:7f928b465f8d 575 er++;
ThomasBNL 40:7f928b465f8d 576 error_count++;
ThomasBNL 40:7f928b465f8d 577 }
ThomasBNL 40:7f928b465f8d 578
ThomasBNL 40:7f928b465f8d 579 // ___________________________
ThomasBNL 40:7f928b465f8d 580 // // \\
ThomasBNL 40:7f928b465f8d 581 // || [STRIKE PLANT] ||
ThomasBNL 40:7f928b465f8d 582 // \\___________________________//
ThomasBNL 40:7f928b465f8d 583 //
ThomasBNL 40:7f928b465f8d 584 void execute_plant_strike()
ThomasBNL 40:7f928b465f8d 585 {
ThomasBNL 40:7f928b465f8d 586 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 40:7f928b465f8d 587 {
ThomasBNL 40:7f928b465f8d 588 motor_strike.reset();
ThomasBNL 40:7f928b465f8d 589 pc.printf("RESET \n\r");
ThomasBNL 40:7f928b465f8d 590 }
ThomasBNL 40:7f928b465f8d 591
ThomasBNL 40:7f928b465f8d 592 position_strike = conversion_counts_to_degrees * motor_strike.getPulses();
ThomasBNL 40:7f928b465f8d 593
ThomasBNL 40:7f928b465f8d 594 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 40:7f928b465f8d 595 keep_in_range(&pwm_to_motor_strike, -1,1); // Pass to motor controller but keep it in range!
ThomasBNL 40:7f928b465f8d 596
ThomasBNL 40:7f928b465f8d 597 if(pwm_to_motor_strike > 0) // Check error and decide direction to turn
ThomasBNL 40:7f928b465f8d 598 { motordirection_strike=cw; }
ThomasBNL 40:7f928b465f8d 599 else
ThomasBNL 40:7f928b465f8d 600 { motordirection_strike=ccw; }
ThomasBNL 40:7f928b465f8d 601
ThomasBNL 40:7f928b465f8d 602
ThomasBNL 40:7f928b465f8d 603 if(p==1) // p is put to one if return action is put to active
ThomasBNL 40:7f928b465f8d 604 { pwm_motor_strike=(abs(pwm_to_motor_strike)); }
ThomasBNL 40:7f928b465f8d 605
ThomasBNL 40:7f928b465f8d 606
ThomasBNL 40:7f928b465f8d 607 // TEMPORARY USAGE WHILE POTMETER ACTIVE
ThomasBNL 40:7f928b465f8d 608 EMG_L_max = 100; // Calibreren (max average over 5 seconde?) gemeten integraal EMG over tijd / (tijdsample stappen)=100
ThomasBNL 40:7f928b465f8d 609 EMG_L_min = 0;
ThomasBNL 40:7f928b465f8d 610 EMG_R_max = 100; // Calibreren
ThomasBNL 40:7f928b465f8d 611 EMG_R_min = 0;
ThomasBNL 40:7f928b465f8d 612
ThomasBNL 40:7f928b465f8d 613 const double Threshold_Bicep_Left_1 =((EMG_L_max-EMG_L_min)*0.2)+EMG_L_min;; //(waarde waarop het gemeten EMG signaal 20% van max het maximale is); // LEFT
ThomasBNL 40:7f928b465f8d 614 const double Threshold_Bicep_Right_1 =((EMG_R_max-EMG_R_min)*0.2)+EMG_R_min; //(waarde waarop het gemeten EMG signaal 20% van max het maximale is); // RIGHT
ThomasBNL 40:7f928b465f8d 615
ThomasBNL 40:7f928b465f8d 616 moving_average_left = 40; // TIJDELIJK PID TEST
ThomasBNL 40:7f928b465f8d 617 moving_average_right = 40; // TIJDELIJK PID TEST
ThomasBNL 40:7f928b465f8d 618 double signal_above_threshold=(moving_average_right-Threshold_Bicep_Right_1)+(moving_average_left-Threshold_Bicep_Left_1);
ThomasBNL 40:7f928b465f8d 619 double max_signal=(EMG_R_max-Threshold_Bicep_Right_1)+(EMG_L_max-Threshold_Bicep_Left_1);
ThomasBNL 40:7f928b465f8d 620 double pwm_strike=signal_above_threshold/max_signal;
ThomasBNL 40:7f928b465f8d 621
ThomasBNL 40:7f928b465f8d 622 //pc.printf("mov_r = %f, mov_l = %f, pwm_strike = %f position = %f \n\r", moving_average_right, moving_average_left, pwm_strike, position_strike); // LINE FOR TESTING
ThomasBNL 40:7f928b465f8d 623
ThomasBNL 40:7f928b465f8d 624 if(p==0)
ThomasBNL 40:7f928b465f8d 625 { pwm_motor_strike=pwm_strike; }
ThomasBNL 40:7f928b465f8d 626
ThomasBNL 40:7f928b465f8d 627 take_sample(); // UITEINDELIJK: UIT
ThomasBNL 40:7f928b465f8d 628
ThomasBNL 40:7f928b465f8d 629 // sample_filter(); --> sample filter aan als EMG
ThomasBNL 40:7f928b465f8d 630
ThomasBNL 40:7f928b465f8d 631 if(sample_error_strike)
ThomasBNL 40:7f928b465f8d 632 {
ThomasBNL 40:7f928b465f8d 633 sample_error_strike=false;
ThomasBNL 40:7f928b465f8d 634 e1 = fabs(position_strike - reference_strike);
ThomasBNL 40:7f928b465f8d 635 e30=e29; e29=e28 ;e28=e27; e27=e26; e26=e25; e25=e24; e24=e23; e23=e22; e22=e21; e21=e20;
ThomasBNL 40:7f928b465f8d 636 e20=e19; e19=e18 ;e18=e17; e17=e16; e16=e15; e15=e14; e14=e13; e13=e12; e12=e11; e11=e10;
ThomasBNL 40:7f928b465f8d 637 e10=e9 ;e9=e8; e8=e7; e7=e6; e6=e5; e5=e4; e4=e3; e3=e2; e2=e1;
ThomasBNL 40:7f928b465f8d 638 }
ThomasBNL 40:7f928b465f8d 639
ThomasBNL 40:7f928b465f8d 640 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 40:7f928b465f8d 641 er++;
ThomasBNL 40:7f928b465f8d 642 error_count++;
ThomasBNL 40:7f928b465f8d 643 }
ThomasBNL 40:7f928b465f8d 644
ThomasBNL 40:7f928b465f8d 645 // ___________________________
ThomasBNL 40:7f928b465f8d 646 // // \\
ThomasBNL 40:7f928b465f8d 647 // || [PID CONTROLLER] ||
ThomasBNL 40:7f928b465f8d 648 // \\___________________________//
ThomasBNL 40:7f928b465f8d 649 //
ThomasBNL 40:7f928b465f8d 650 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 27:85e5d36bb6c5 651 {
ThomasBNL 40:7f928b465f8d 652 double error=(reference - position); // Current error (input controller)
ThomasBNL 40:7f928b465f8d 653 integrate_error=integrate_error_turn + sample_time*error_turn; // Integral error output
ThomasBNL 40:7f928b465f8d 654 // overwrite previous integrate error by adding the current error
ThomasBNL 40:7f928b465f8d 655 // multiplied by the sample time.
ThomasBNL 40:7f928b465f8d 656
ThomasBNL 40:7f928b465f8d 657 double error_derivative=(error - previous_error)/sample_time; // Derivative error output
ThomasBNL 40:7f928b465f8d 658 error_derivative=Lowpassfilter_derivative.step(error_derivative); // Filter
ThomasBNL 40:7f928b465f8d 659
ThomasBNL 40:7f928b465f8d 660 previous_error_turn=error_turn; // current error is saved to memory constant to be used in
ThomasBNL 40:7f928b465f8d 661 // the next loop for calculating the derivative error
ThomasBNL 40:7f928b465f8d 662
ThomasBNL 40:7f928b465f8d 663 double pwm_motor_P = error*P_gain; // Output P controller to pwm
ThomasBNL 40:7f928b465f8d 664 double pwm_motor_I = integrate_error*I_gain; // Output I controller to pwm
ThomasBNL 40:7f928b465f8d 665 double pwm_motor_D = error_derivative*D_gain; // Output D controller to pwm
ThomasBNL 36:f29a36683b1a 666
ThomasBNL 40:7f928b465f8d 667 double pwm_to_motor = pwm_motor_P + pwm_motor_I + pwm_motor_D;
ThomasBNL 40:7f928b465f8d 668
ThomasBNL 40:7f928b465f8d 669 return pwm_to_motor;
ThomasBNL 40:7f928b465f8d 670 }
ThomasBNL 36:f29a36683b1a 671
ThomasBNL 40:7f928b465f8d 672 // ___________________________
ThomasBNL 40:7f928b465f8d 673 // // \\
ThomasBNL 40:7f928b465f8d 674 // || [SAMPLE] ||
ThomasBNL 40:7f928b465f8d 675 // \\___________________________//
ThomasBNL 40:7f928b465f8d 676 //
ThomasBNL 36:f29a36683b1a 677 void sample_filter()
ThomasBNL 36:f29a36683b1a 678 {
ThomasBNL 36:f29a36683b1a 679 Filter();
ThomasBNL 36:f29a36683b1a 680 take_sample();
ThomasBNL 36:f29a36683b1a 681 if(sample)
ThomasBNL 36:f29a36683b1a 682 {
ThomasBNL 36:f29a36683b1a 683 sample=false;
ThomasBNL 36:f29a36683b1a 684 Sample_EMG_L_1 = EMG_Left_Bicep_filtered; Sample_EMG_R_1 = EMG_Right_Bicep_filtered;
ThomasBNL 36:f29a36683b1a 685
ThomasBNL 36:f29a36683b1a 686 Sample_EMG_L_10= Sample_EMG_L_9; Sample_EMG_R_10= Sample_EMG_R_9;
ThomasBNL 36:f29a36683b1a 687 Sample_EMG_L_9 = Sample_EMG_L_8; Sample_EMG_R_9 = Sample_EMG_R_8;
ThomasBNL 36:f29a36683b1a 688 Sample_EMG_L_8 = Sample_EMG_L_7; Sample_EMG_R_8 = Sample_EMG_R_7;
ThomasBNL 36:f29a36683b1a 689 Sample_EMG_L_7 = Sample_EMG_L_6; Sample_EMG_R_7 = Sample_EMG_R_6;
ThomasBNL 36:f29a36683b1a 690 Sample_EMG_L_6 = Sample_EMG_L_5; Sample_EMG_R_6 = Sample_EMG_R_5;
ThomasBNL 36:f29a36683b1a 691 Sample_EMG_L_5 = Sample_EMG_L_4; Sample_EMG_R_5 = Sample_EMG_R_4;
ThomasBNL 36:f29a36683b1a 692 Sample_EMG_L_4 = Sample_EMG_L_3; Sample_EMG_R_4 = Sample_EMG_R_3;
ThomasBNL 36:f29a36683b1a 693 Sample_EMG_L_3 = Sample_EMG_L_2; Sample_EMG_R_3 = Sample_EMG_R_2;
ThomasBNL 36:f29a36683b1a 694 Sample_EMG_L_2 = Sample_EMG_L_1; Sample_EMG_R_2 = Sample_EMG_R_1;
ThomasBNL 36:f29a36683b1a 695 }
ThomasBNL 36:f29a36683b1a 696 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 36:f29a36683b1a 697 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 36:f29a36683b1a 698 n++;
ThomasBNL 36:f29a36683b1a 699 }
ThomasBNL 36:f29a36683b1a 700
ThomasBNL 40:7f928b465f8d 701 // ___________________________
ThomasBNL 40:7f928b465f8d 702 // // \\
ThomasBNL 40:7f928b465f8d 703 // || [ FILTER ] ||
ThomasBNL 40:7f928b465f8d 704 // \\___________________________//
ThomasBNL 40:7f928b465f8d 705 //
ThomasBNL 40:7f928b465f8d 706 void Filter() // Unfiltered EMG (input) -> highpass filter -> rectify -> lowpass filter -> Filtered EMG (output)
ThomasBNL 36:f29a36683b1a 707 {
ThomasBNL 40:7f928b465f8d 708 EMG_left_Bicep = input1; EMG_Right_Bicep = input2; // Current input EMG left and right
ThomasBNL 40:7f928b465f8d 709
ThomasBNL 40:7f928b465f8d 710 EMG_Left_Bicep_filtered = highpassfilter_1.step(EMG_left_Bicep); EMG_Right_Bicep_filtered = highpassfilter_2.step(EMG_Right_Bicep); // Highpassfilter
ThomasBNL 40:7f928b465f8d 711 EMG_Left_Bicep_filtered = fabs(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = fabs(EMG_Right_Bicep_filtered); // Rectify
ThomasBNL 40:7f928b465f8d 712
ThomasBNL 40:7f928b465f8d 713 EMG_Left_Bicep_filtered_notch_1 = notchL1.step(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered_notch_1 = notchR1.step(EMG_Right_Bicep_filtered); // Notch Filter part 1
ThomasBNL 40:7f928b465f8d 714 EMG_Left_Bicep_filtered_notch_2 = notchL2.step(EMG_Left_Bicep_filtered_notch_1); EMG_Right_Bicep_filtered_notch_2 = notchR2.step(EMG_Right_Bicep_filtered_notch_1); // Notch Filter part 2
ThomasBNL 40:7f928b465f8d 715
ThomasBNL 40:7f928b465f8d 716 EMG_Left_Bicep_filtered = lowpassfilter_1.step(EMG_Left_Bicep_filtered_notch_2); EMG_Right_Bicep_filtered = lowpassfilter_2.step(EMG_Right_Bicep_filtered_notch_2); // Lowpassfilter
ThomasBNL 36:f29a36683b1a 717 }
ThomasBNL 40:7f928b465f8d 718
ThomasBNL 40:7f928b465f8d 719 // ___________________________
ThomasBNL 40:7f928b465f8d 720 // // \\
ThomasBNL 40:7f928b465f8d 721 // || [TAKE SAMPLE] ||
ThomasBNL 40:7f928b465f8d 722 // \\___________________________//
ThomasBNL 40:7f928b465f8d 723 //
ThomasBNL 40:7f928b465f8d 724 void take_sample() // Take a sample every 25th sample for moving average, every 5th sample ....
ThomasBNL 40:7f928b465f8d 725 {
ThomasBNL 40:7f928b465f8d 726 if(n==25)
ThomasBNL 40:7f928b465f8d 727 {sample = true; n=0;}
ThomasBNL 40:7f928b465f8d 728
ThomasBNL 40:7f928b465f8d 729 if(er==5)
ThomasBNL 40:7f928b465f8d 730 {sample_error = true; er=0;}
ThomasBNL 40:7f928b465f8d 731
ThomasBNL 40:7f928b465f8d 732 sample_error_strike = true;
ThomasBNL 36:f29a36683b1a 733 }
ThomasBNL 36:f29a36683b1a 734
ThomasBNL 40:7f928b465f8d 735 // ___________________________
ThomasBNL 40:7f928b465f8d 736 // // \\
ThomasBNL 40:7f928b465f8d 737 // || [CHANGE REFERENCE] ||
ThomasBNL 40:7f928b465f8d 738 // \\___________________________//
ThomasBNL 40:7f928b465f8d 739 //
ThomasBNL 40:7f928b465f8d 740 void Change_Turn_Position_Right (double& reference, double change_one_bottle)
ThomasBNL 36:f29a36683b1a 741 {
ThomasBNL 40:7f928b465f8d 742 if(reference==90) // If reference value at right boundary bottle and function is executed than immediatly turn 5 bottles to the left (ref to -90)
ThomasBNL 40:7f928b465f8d 743 { reference=-90; }
ThomasBNL 40:7f928b465f8d 744 else
ThomasBNL 40:7f928b465f8d 745 { reference = reference + change_one_bottle;
ThomasBNL 40:7f928b465f8d 746 keep_in_range(&reference, -90, 90); } // reference position stays between -90 and 90 degrees (IF bottles at -90, -45, 0, 45, 90 degrees)
ThomasBNL 36:f29a36683b1a 747 }
ThomasBNL 36:f29a36683b1a 748
ThomasBNL 40:7f928b465f8d 749 void Change_Turn_Position_Left (double& reference, double change_one_bottle)
ThomasBNL 40:7f928b465f8d 750 {
ThomasBNL 40:7f928b465f8d 751 if(reference==-90) // If reference value at left boundary bottle and function is executed than immediatly turn 5 bottles to the left (ref to +90)
ThomasBNL 40:7f928b465f8d 752 { reference=90; }
ThomasBNL 40:7f928b465f8d 753 else
ThomasBNL 40:7f928b465f8d 754 { reference = reference - change_one_bottle;
ThomasBNL 40:7f928b465f8d 755 keep_in_range(&reference, -90, 90); }
ThomasBNL 40:7f928b465f8d 756 }
ThomasBNL 40:7f928b465f8d 757
ThomasBNL 40:7f928b465f8d 758 // ___________________________
ThomasBNL 40:7f928b465f8d 759 // // \\
ThomasBNL 40:7f928b465f8d 760 // | [(DE)ACTIVATE PID CONTROLLERS] |
ThomasBNL 40:7f928b465f8d 761 // \\___________________________//
ThomasBNL 40:7f928b465f8d 762 //
ThomasBNL 40:7f928b465f8d 763 void deactivate_PID_Controller(double& P_gain, double& I_gain, double& D_gain)
ThomasBNL 40:7f928b465f8d 764 {
ThomasBNL 40:7f928b465f8d 765 P_gain=0; I_gain=0; D_gain=0; // Deactivating values of PID controller
ThomasBNL 40:7f928b465f8d 766 pwm_motor_turn=0; pwm_motor_strike=0;
ThomasBNL 40:7f928b465f8d 767 }
ThomasBNL 40:7f928b465f8d 768
ThomasBNL 40:7f928b465f8d 769 void activate_PID_Controller_turn(double& P_gain, double& I_gain, double& D_gain)
ThomasBNL 40:7f928b465f8d 770 {
ThomasBNL 40:7f928b465f8d 771 P_gain_turn=0.02; // Change P,I,D values (activate)
ThomasBNL 40:7f928b465f8d 772 I_gain_turn=0.1;
ThomasBNL 40:7f928b465f8d 773 D_gain_turn=0;
ThomasBNL 40:7f928b465f8d 774 }
ThomasBNL 40:7f928b465f8d 775
ThomasBNL 40:7f928b465f8d 776 void activate_PID_Controller_strike(double& P_gain, double& I_gain, double& D_gain)
ThomasBNL 40:7f928b465f8d 777 {
ThomasBNL 40:7f928b465f8d 778 double Ku = (input1.read())*1; // EMG Right bicep (tussen nul en 100%) // DEBUG TEST TOOL: substitute EMG input for potmeter inputs
ThomasBNL 40:7f928b465f8d 779 double Pu = (input2.read())*1; // EMG Left bicep (tussen nul en 100%)
ThomasBNL 40:7f928b465f8d 780
ThomasBNL 40:7f928b465f8d 781 P_gain_strike=0.8*Ku; // Ku=0.2 (ultimate gain Ziegler-Nichols method)
ThomasBNL 40:7f928b465f8d 782 // Pu=0.25 (ultimate period) (4Hz)
ThomasBNL 40:7f928b465f8d 783
ThomasBNL 40:7f928b465f8d 784 pc.printf("Ku=%f Pu=%f \n\r", Ku, Pu);
ThomasBNL 40:7f928b465f8d 785 //0.09090909;
ThomasBNL 40:7f928b465f8d 786 //PI tyreus luyben : 0.0625, 0.55;
ThomasBNL 40:7f928b465f8d 787 //PID tyreus luyben : 0.09090909, 0.55, 0.0396825;
ThomasBNL 40:7f928b465f8d 788 // Ku=0.2 (ultimate gain Ziegler-Nichols method)
ThomasBNL 40:7f928b465f8d 789 // Pu=0.25 (ultimate period) (4Hz)
ThomasBNL 40:7f928b465f8d 790 // hier waardes P,I,D veranderen (waardes bovenaan doen (tijdelijk) niks meer // 0.00045 // 0.03
ThomasBNL 40:7f928b465f8d 791 I_gain_strike=0; //0.55;
ThomasBNL 40:7f928b465f8d 792 D_gain_strike=Pu/8; //0.0396825;
ThomasBNL 40:7f928b465f8d 793 }
ThomasBNL 40:7f928b465f8d 794
ThomasBNL 40:7f928b465f8d 795 // ___________________________
ThomasBNL 40:7f928b465f8d 796 // // \\
ThomasBNL 40:7f928b465f8d 797 // || [OTHER FUNCTIONS] ||
ThomasBNL 40:7f928b465f8d 798 // \\___________________________//
ThomasBNL 40:7f928b465f8d 799 //
ThomasBNL 36:f29a36683b1a 800 void countdown_from_5() // Countdown from 5 till 0 inside Putty (interface)
ThomasBNL 36:f29a36683b1a 801 {
ThomasBNL 36:f29a36683b1a 802 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 36:f29a36683b1a 803 wait(1); pc.printf("1 Set \n\r"); wait(1); pc.printf("Go \n\r");
ThomasBNL 40:7f928b465f8d 804 }
ThomasBNL 40:7f928b465f8d 805
ThomasBNL 40:7f928b465f8d 806 void ControlGo() // Control flag
ThomasBNL 40:7f928b465f8d 807 { control_go = true; }
ThomasBNL 36:f29a36683b1a 808
ThomasBNL 40:7f928b465f8d 809 void setlooptimerflag(void) // Looptimerflag function
ThomasBNL 40:7f928b465f8d 810 { looptimerflag = true; }
ThomasBNL 36:f29a36683b1a 811
sigert 37:6c04c15d9bbe 812 void red() { debug_led_red=on; debug_led_blue=off; debug_led_green=off; }
sigert 37:6c04c15d9bbe 813 void blue() { debug_led_red=off; debug_led_blue=on; debug_led_green=off; }
sigert 37:6c04c15d9bbe 814 void green() { debug_led_red=off; debug_led_blue=off; debug_led_green=on; }
sigert 37:6c04c15d9bbe 815 void white() { debug_led_red=on; debug_led_blue=on; debug_led_green=on; }
sigert 37:6c04c15d9bbe 816 void yellow() { debug_led_red=on; debug_led_blue=off; debug_led_green=on; }
sigert 37:6c04c15d9bbe 817 void cyan() { debug_led_red=off; debug_led_blue=on; debug_led_green=on; }
sigert 37:6c04c15d9bbe 818 void purple() { debug_led_red=on; debug_led_blue=on; debug_led_green=off; }
sigert 37:6c04c15d9bbe 819 void black() { debug_led_red=off; debug_led_blue=off; debug_led_green=off; }
ThomasBNL 36:f29a36683b1a 820
ThomasBNL 40:7f928b465f8d 821
ThomasBNL 40:7f928b465f8d 822 void calibrate_potmeter() // DEBUG/TEST: Calibration thresholds with potmeter
ThomasBNL 40:7f928b465f8d 823 {
ThomasBNL 40:7f928b465f8d 824 // TEMPORARY USAGE WHILE POTMETER ACTIVE
ThomasBNL 40:7f928b465f8d 825 EMG_L_max = 100;
ThomasBNL 40:7f928b465f8d 826 EMG_L_min = 0;
ThomasBNL 40:7f928b465f8d 827 EMG_R_max = 100;
ThomasBNL 40:7f928b465f8d 828 EMG_R_min = 0;
ThomasBNL 40:7f928b465f8d 829 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 40:7f928b465f8d 830 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 40:7f928b465f8d 831 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 40:7f928b465f8d 832 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 40:7f928b465f8d 833 }
ThomasBNL 40:7f928b465f8d 834
ThomasBNL 40:7f928b465f8d 835 // Keep in range function
ThomasBNL 40:7f928b465f8d 836 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 40:7f928b465f8d 837 {
ThomasBNL 40:7f928b465f8d 838 *in > min ? *in < max? : *in = max: *in = min;
ThomasBNL 40:7f928b465f8d 839 }