Thomas Burgers
/
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
Diff: main.cpp
- Revision:
- 46:9279c7a725bf
- Parent:
- 45:359df0594588
- Child:
- 47:c61873a0b646
--- a/main.cpp Wed Oct 14 18:57:27 2015 +0000
+++ b/main.cpp Wed Oct 14 21:00:20 2015 +0000
@@ -1,14 +1,43 @@
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
+///// //// ////// ////// /////////////////////////////////////////////////////////////////////////////////////////////////////////|
+///// //// ////// ////// /////////////////////////////////////////////////////////////////////////////////////////////////////////|
+//////////// //////////// ////// ////// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////|
+//////////// //////////// ////// ////// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////|
+//////////// //////////// ////// /////////////////////////////////////////////////////////////////////////////////////////////////////////|
+//////////// //////////// ////// /////////////////////////////////////////////////////////////////////////////////////////////////////////|
+//////////// //////////// ////// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////|
+//////////// //////////// ////// ////// ///////////////////////////////////////////////////////////////////////////////////////////////////////////////|
+//////////// //////////// ////// ////// /////////////////////////////////////////////////////////////////////////////////////////////////////////|
+//////////// //////////// ////// ////// /////////////////////////////////////////////////////////////////////////////////////////////////////////|
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
+/////// ////// ////// ////// ////// /////// ////// /////// ////// /////////////////////////////////////////////|
+/////// ////// ////// ////// ////// ///// ////// ///// ////// /////////////////////////////////////////////|
+/////// ///////////////// ////// ////// //////////// //// ////// //// ////// ///////////////////////////////////////////////////|
+/////// ///////////////// ////// ////// //////////// /// ////// /// ////// ///////////////////////////////////////////////////|
+/////// ///////////////// ////// ////// // ////// // ////// /////////////////////////////////////////////|
+/////// ///////////////// ////// ////// // ////// // ////// /////////////////////////////////////////////|
+/////// ///////////////// ////// //////////// /// ////// /// ////// ///////////////////////////////////////////////////|
+/////// ///////////////// ////// ////// //////////// //// ////// //// ////// ///////////////////////////////////////////////////|
+/////// ////// ////// ////// ////// ///// ////// ///// ////// /////////////////////////////////////////////|
+/////// ////// ////// ////// ////// ////// ////// ////// ////// /////////////////////////////////////////////|
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
+/////// /////////////// ///////////// /////// ////// /////////////////////////////////////////////////////////////////////////|
+/////// ///////////// //////////// ///// ////// ////////////////////////////////////////////////////////////////////////|
+/////// ////// /////////// /////////// //// ////// ///// ///////////////////////////////////////////////////////////////////////|
+/////// ////// ////////// // ////////// /// ////// ///// ///////////////////////////////////////////////////////////////////////|
+/////// ///////// //// ///////// // ////// ///// ///////////////////////////////////////////////////////////////////////|
+/////// //////// ////// //////// // ////// ///// ///////////////////////////////////////////////////////////////////////|
+/////// ////// /////// /////// /// ////// ///// ///////////////////////////////////////////////////////////////////////|
+/////// ////// ////// ////// //// ////// ///// ///////////////////////////////////////////////////////////////////////|
+/////// ////// //////////// ///// ///// ////// ////////////////////////////////////////////////////////////////////////|
+/////// ////// ////////////// //// ////// ////// /////////////////////////////////////////////////////////////////////////|
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////|
+//============================================================================================================================
// ___________________________
// // \\
// || [Libraries] ||
@@ -22,67 +51,187 @@
#include "biquadFilter.h"
#include "encoder.h"
+//============================================================================================================================
// ___________________________
// // \\
-// || [Inputs] ||
+// || [FLOW AND DEBUGGING TOOLS] ||
// \\___________________________//
+// _________________________
+// : [LEDS] :
+// :__________________________:
//
-
-
-MODSERIAL pc(USBTX,USBRX);
-
DigitalOut debug_led_red(LED_RED); // Debug LED
DigitalOut debug_led_green(LED_GREEN); // Debug LED
DigitalOut debug_led_blue(LED_BLUE); // Debug LED
-
+// __________________________
+// : [Buttons] :
+// :__________________________:
+//
DigitalIn buttonL1(PTC6); // Button 1 (low on k64f) for testing at the lower board
DigitalIn buttonL2(PTA4); // Button 2 (low on k64f) for testing at the lower board
DigitalIn buttonH1(D2); // Button 3 (high on k64f) for testing at the top board
-DigitalIn buttonH2(D6); // Button 4 (high on k64f) for testing at the top board
+DigitalIn buttonH2(D6); // Button 4 (high on k64f) for testing at the top board
+
+// __________________________
+// : [Potmeter] :
+// :__________________________:
+// DEBUGGING / TESTING
+//moving_average_right = (potmeter1.read())*100; // EMG Right bicep (tussen nul en 100%)
+//moving_average_left = (potmeter2.read())*100; // EMG Left bicep (tussen nul en 100%)
-AnalogIn potmeter(A0); // Potmeter that can read a reference value (DEBUG TOOL)
+// __________________________
+// : [FLOW] :
+// :__________________________:
+//
+Ticker looptimer; // Ticker called looptimer to set a looptimerflag
-Ticker looptimer; // Ticker called looptimer to set a looptimerflag
+// __________________________
+// : [HIDSCOPE] :
+// :__________________________:
+//
+
+HIDScope scope(3); // HIDSCOPE declared
+MODSERIAL pc(USBTX,USBRX);
+
+//============================================================================================================================
// ___________________________
// // \\
-// || [MOTOR TURN] ||
+// || [EMG] ||
// \\___________________________//
//
+AnalogIn input1(A0); // EMG: Two AnalogIn EMG inputs
+AnalogIn input2(A1); // EMG: Two AnalogIn EMG inputs
+
+// __________________________
+// : [EMG variables] :
+// :__________________________:
+//
+
+volatile bool control_go = false; // EMG:
+volatile bool sample = false; // EMG:
+//
+double Sample_EMG_L_1, Sample_EMG_L_2, Sample_EMG_L_3, Sample_EMG_L_4, Sample_EMG_L_5, Sample_EMG_L_6, Sample_EMG_L_7, Sample_EMG_L_8, Sample_EMG_L_9, Sample_EMG_L_10, moving_average_left; // EMG:
+double Sample_EMG_R_1, Sample_EMG_R_2, Sample_EMG_R_3, Sample_EMG_R_4, Sample_EMG_R_5, Sample_EMG_R_6, Sample_EMG_R_7, Sample_EMG_R_8, Sample_EMG_R_9, Sample_EMG_R_10, moving_average_right; // EMG:
+double minimum_L; // EMG:
+double maximum_L; // EMG:
+double EMG_L_min; // EMG:
+double EMG_L_max; // EMG:
+double minimum_R; // EMG:
+double maximum_R; // EMG:
+double EMG_R_min; // EMG:
+double EMG_R_max; // EMG:
+double n=0; // EMG:
+double c=0; // EMG:
+
+// __________________________
+// : [EMG Filter] :
+// :__________________________:
+// Biquad filters
+
+const double low_b0 = 0.05892937945281792;
+const double low_b1 = 0.11785875890563584;
+const double low_b2 = 0.05892937945281792;
+const double low_a1 = -1.205716572226748;
+const double low_a2 = 0.44143409003801976; // VIA online biquad calculator Lowpas 520-48-0.7071-6
+
+const double high_b0 = 0.6389437261127494;
+const double high_b1 = -1.2778874522254988;
+const double high_b2 = 0.6389437261127494;
+const double high_a1 = -1.1429772843080923;
+const double high_a2 = 0.41279762014290533; // VIA online biquad calculator Highpas 520-52-0.7071-6
+
+biquadFilter highpassfilter_1(high_a1, high_a2, high_b0, high_b1, high_b2);
+biquadFilter highpassfilter_2(high_a1, high_a2, high_b0, high_b1, high_b2);
+biquadFilter lowpassfilter_1(low_a1, low_a2, low_b0, low_b1, low_b2);
+biquadFilter lowpassfilter_2(low_a1, low_a2, low_b0, low_b1, low_b2);
+
+double EMG_left_Bicep; double EMG_Right_Bicep; // input variables
+double EMG_Left_Bicep_filtered; double EMG_Right_Bicep_filtered; // output variables
+
+//============================================================================================================================
+// ___________________________
+// // \\
+// || [MOTORS] ||
+// \\___________________________//
+//
+// __________________________
+// : [Motor TURN] :
+// :__________________________:
+//
+
QEI motor_turn(D12,D13,NC,32); // TURN: Encoder for motor
PwmOut pwm_motor_turn(D5); // TURN: Pwm for motor
DigitalOut motordirection_turn(D4); // TURN: Direction of the motor
// PID motor constants
-double integrate_error_turn=0; // integration error turn motor
-double previous_error_turn=0; // previous error turn motor
+ double integrate_error_turn=0; // TURN: integration error turn motor
+ double previous_error_turn=0; // TURN: previous error turn motor
-// ___________________________
-// // \\
-// || [MOTOR STRIKE] ||
-// \\___________________________//
+ double position_turn; // TURN: Set variable to store current position of the motor
+ double error_turn; // TURN: Set variable to store the current error of the motor
+
+ double pwm_motor_turn_P; // TURN: variable that stores the P action part of the error
+ double pwm_motor_turn_I; // TURN: variable that stores the I action part of the error
+ double pwm_motor_turn_D; // TURN: variable that stores the D action part of the error
+ double pwm_to_motor_turn; // TURN: variable that is constructed by summing the values of the P, I and D action
+
+ double P_gain_turn; // TURN: this gain gets multiplied with the error inside the P action of the PID controller
+ double I_gain_turn; // TURN: this gain gets multiplied with the error inside the I action of the PID controller
+ double D_gain_turn; // TURN: this gain gets multiplied with the error inside the D action of the PID controller
+
+ double reference_turn; // TURN: reference position of the motor (position the motor 'likes' to get to)
+
+
+// __________________________
+// : [Motor STRIKE] :
+// :__________________________:
+//
//QEI motor_strike(XX,XX,NC,32); // STRIKE: Encoder for motor Turn
//PwmOut pwm_motor_strike(XX); // STRIKE: Pwm for motor Turn
//DigitalOut motordirection_strike(XX); // STRIKE: Direction of the motor Turn
// PID motor constants
-//double integrate_error_strike=0; // STRIKE: integration error turn motor
-//double previous_error_strike=0; // STRIKE: previous error turn motor
+
+ //double integrate_error_strike=0; // STRIKE: integration error turn motor
+ //double previous_error_strike=0; // STRIKE: previous error turn motor
+
+ //double position_strike; // STRIKE: Set variable to store current position of the motor
+ //double error_strike; // STRIKE: Set variable to store the current error of the motor
+ //double pwm_motor_strike_P; // STRIKE: variable that stores the P action part of the error
+ //double pwm_motor_strike_I; // STRIKE: variable that stores the I action part of the error
+ //double pwm_motor_strike_D; // STRIKE: variable that stores the D action part of the error
+ //double pwm_to_motor_strike; // STRIKE: variable that is constructed by summing the values of the P, I and D action
+ //double reference_strike;
+
+ //double P_gain_turn; // STRIKE: this gain gets multiplied with the error inside the P action of the PID controller
+ //double I_gain_turn; // STRIKE: this gain gets multiplied with the error inside the I action of the PID controller
+ //double D_gain_turn; // STRIKE: this gain gets multiplied with the error inside the D action of the PID controller
+
+ //double reference_strike; // STRIKE: reference position of the motor (position the motor 'likes' to get to)
+
+// __________________________
+// : [D-action filter] :
+// :__________________________:
+// Applicable for both motors
+
+const double mT_f_a1=-1.965293372622690e+00; // Motor Turn derivative filter constants
+const double mT_f_a2=9.658854605688177e-01;
+const double mT_f_b0=1.480219865318266e-04;
+const double mT_f_b1=2.960439730636533e-04;
+const double mT_f_b2=1.480219865318266e-04;
+
+biquadFilter Lowpassfilter_derivative(mT_f_a1,mT_f_a2,mT_f_b0,mT_f_b1,mT_f_b2); // BiquadFilter used for the filtering of the Derivative action of the PID-action
+
+
+//============================================================================================================================
// ___________________________
// // \\
-// || [HIDSCOPE] ||
-// \\___________________________//
-//
-
-HIDScope scope(3); // HIDSCOPE declared
-
-// ___________________________
-// // \\
-// || [System CONSTANTS] ||
+// || [SYSTEM CONSTANTS] ||
// \\___________________________//
//
@@ -98,81 +247,60 @@
// ticks per magnet rotation = 32 (X2 Encoder)
// One revolution = 360 degrees
// degrees_per_encoder_tick = 360/(gear_ratio*ticks_per_magnet_rotation)=360/131*32=0.085877862594198
-
-
-// ___________________________
-// // \\
-// || [FILTER CONSTANTS] ||
-// \\___________________________//
-//
-
-const double mT_f_a1=-1.965293372622690e+00; // Motor Turn derivative filter constants
-const double mT_f_a2=9.658854605688177e-01;
-const double mT_f_b0=1.480219865318266e-04;
-const double mT_f_b1=2.960439730636533e-04;
-const double mT_f_b2=1.480219865318266e-04;
-
-biquadFilter Lowpassfilter_derivative(mT_f_a1,mT_f_a2,mT_f_b0,mT_f_b1,mT_f_b2); // BiquadFilter used for the filtering of the Derivative action of the PID-action
-
+const double change_one_bottle=45;
+
+//============================================================================================================================
// ___________________________
// // \\
// || [FUNCTIONS USED] ||
// \\___________________________//
-//
-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
-void setlooptimerflag(void); // Sets looptimerflag volatile bool to true
+// __________________________
+// : [Motor Functions] :
+// :__________________________:
+// Applicable for both motors
+
+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
+void setlooptimerflag (void); // Sets looptimerflag volatile bool to true
-void deactivate_PID_Controller(double& P_gain, double& I_gain, double& D_gain); // STRIKE/TURN: Deactivate PID Controller
+void deactivate_PID_Controller (double& P_gain, double& I_gain, double& D_gain); // STRIKE/TURN: Deactivate PID Controller
+
+void activate_PID_Controller_strike (double& P_gain, double& I_gain, double& D_gain); // STRIKE: Activate PID Controller
+
+void activate_PID_Controller_turn (double& P_gain, double& I_gain, double& D_gain); // TURN: Activate PID Controller
+void Change_Turn_Position_Left (double& reference, double change_one_bottle); // TURN: Change Reference position one bottle to the left
+void Change_Turn_Position_Right (double& reference, double change_one_bottle); // TURN: Change Reference position one bottle to the right
-void activate_PID_Controller_strike(double& P_gain, double& I_gain, double& D_gain); // STRIKE: Activate PID Controller
+// __________________________
+// : [EMG Functions] :
+// :__________________________:
+// Applicable for both biceps
-void activate_PID_Controller_turn(double& P_gain, double& I_gain, double& D_gain); // TURN: Activate PID Controller
-void Change_Turn_Position_Left (double& reference, double change_one_bottle); // TURN: Change Reference position one bottle to the left
-void Change_Turn_Position_Right (double& reference, double change_one_bottle); // TURN: Change Reference position one bottle to the right
+void calibration(); // EMG: Calibrate the EMG signal (calculate min and max signal and determine threshold values)
+void countdown_from_5(); // PUTTY: 5 second countdown inside
+void Filter(); // EMG: Filter the incoming EMG signals
+void sample_filter(); // EMG: Calculate moving average (10 samples, one sample per 25 samples) using sample_filter => moving average over +-0.5 seconds
+void take_sample(); // EMG: Take a sample once every 25 samples that's used to calculate the moving average
+void ControlGo(); // EMG: function that gets a ticker attached and sets a certain loop to true every sample
+
+// __________________________
+// : [Action Controller] :
+// :__________________________:
+//
- ///////////////////////////////
- // //
-/////////////////////////////////////////////////////// [MAIN FUNCTION] ////////////////////////////////////////////////////////////////////////////
- // // //
- /////////////////////////////// //
-
+//============================================================================================================================
+ ///////////////////////////////
+ // //
+///////////////////////////////////////////// [MAIN FUNCTION] /////////////////////////////////////////////////////
+ // //
+ ///////////////////////////////
+//============================================================================================================================
int main() {
debug_led_red=off;
debug_led_blue=off;
debug_led_green=off;
-
- const double change_one_bottle=45;
- double position_turn; // TURN: Set variable to store current position of the motor
- double error_turn; // TURN: Set variable to store the current error of the motor
-
- double pwm_motor_turn_P; // TURN: variable that stores the P action part of the error
- double pwm_motor_turn_I; // TURN: variable that stores the I action part of the error
- double pwm_motor_turn_D; // TURN: variable that stores the D action part of the error
- double pwm_to_motor_turn; // TURN: variable that is constructed by summing the values of the P, I and D action
-
- double P_gain_turn; // TURN: this gain gets multiplied with the error inside the P action of the PID controller
- double I_gain_turn; // TURN: this gain gets multiplied with the error inside the I action of the PID controller
- double D_gain_turn; // TURN: this gain gets multiplied with the error inside the D action of the PID controller
-
- double reference_turn; // TURN: reference position of the motor (position the motor 'likes' to get to)
-
- //double position_strike; // TURN: Set variable to store current position of the motor
- //double error_strike; // TURN: Set variable to store the current error of the motor
-
- //double pwm_motor_strike_P; // STRIKE: variable that stores the P action part of the error
- //double pwm_motor_strike_I; // STRIKE: variable that stores the I action part of the error
- //double pwm_motor_strike_D; // STRIKE: variable that stores the D action part of the error
- //double pwm_to_motor_strike; // STRIKE: variable that is constructed by summing the values of the P, I and D action
- //double reference_strike;
-
- //double P_gain_turn; // STRIKE: this gain gets multiplied with the error inside the P action of the PID controller
- //double I_gain_turn; // STRIKE: this gain gets multiplied with the error inside the I action of the PID controller
- //double D_gain_turn; // STRIKE: this gain gets multiplied with the error inside the D action of the PID controller
-
- //double reference_strike; // STRIKE: reference position of the motor (position the motor 'likes' to get to)
-
+
// ___________________________
// // \\
// || [START OF CODE] ||
@@ -188,6 +316,11 @@
// \\___________________________//
// Calibrate starting postion (RED LED)
+// ___________________________
+// : [Starting position motor] :
+// :___________________________:
+//
+
pc.printf("Button calibration \n\r");
while(1)
{
@@ -212,25 +345,30 @@
//
if ((buttonL2.read() < 0.5) && (buttonL1.read() < 0.5)) // Save current TURN and STRIKE positions as starting position
{
- motor_turn.reset(); // TURN: Starting Position
- reference_turn=0; // TURN: Set reference position to zero
-// motor_strike.reset(); // STRIKE: Starting Position
-// double reference_strike=0; // STRIKE: Set reference position to zero
- goto calibration_complete; // Calibration complete
+ motor_turn.reset(); // TURN: Starting Position
+ reference_turn=0; // TURN: Set reference position to zero
+// motor_strike.reset(); // STRIKE: Starting Position
+// double reference_strike=0; // STRIKE: Set reference position to zero
+ goto calibration_starting_position_complete; // Calibration complete
}
}
-
- calibration_complete:
+ calibration_starting_position_complete:
debug_led_red=off; // Calibration end => RED LED off
-
+// ___________________________
+// : [EMG calibration] :
+// :___________________________:
+//
+ calibration(); // EMG: Calibration
+
// ___________________________
// // \\
// || [ATTACH TICKER] ||
// \\___________________________//
// Attach Ticker to looptimerflag
+ // EMG_ticker.attach(&ControlGo, (float)1/Fs); // EMG: Ticker
looptimer.attach(setlooptimerflag,(float)1/Fs); // calls the looptimer flag every sample
pc.printf("Start infinite loop \n\r");
@@ -238,6 +376,8 @@
activate_PID_Controller_strike(P_gain_turn, I_gain_turn, D_gain_turn);
+
+
// ___________________________
// // \\
// || [START INFINTTE LOOP] ||
@@ -297,10 +437,155 @@
position_turn = conversion_counts_to_degrees * motor_turn.getPulses();
pc.printf("calibrated setpoint: %f, calibrated position motor %i, position %f \n\r", reference_turn, motor_turn.getPulses(), position_turn);
+
+// ___________________________
+// // \\
+// || [GET CURRENT EMG VALUES] ||
+// \\___________________________//
+//
+ debug_led_green = on; //
+ sample_filter();
+// scope.set(0,EMG_left_Bicep);
+// scope.set(1,EMG_Left_Bicep_filtered);
+// scope.set(2,moving_average_left);
+// scope.send(); //
// ___________________________
// // \\
+// || [ACTION CONTROLLER] ||
+// \\___________________________//
+//
+
+ // DO STUFF / DECIDE WHETHER TO DO SOMETHING OR NOT ==> TICKER loop moet dan wel uit (TICKER VOOR SAMPLE FILTER WEL AAN)
+
+
+
+// Nieuwe_actie:
+// TICKER AAN // start here again when action has finished
+// debug_led_red=off;
+// debug_led_blue=off;
+// debug_led_green=on; // LED Turns green if ready for a new action
+// wait(1);
+// moving_average_right = (potmeter1.read())*100; //EMG Right bicep (tussen nul en 100%)
+// moving_average_left = (potmeter2.read())*100; // EMG Left bicep (tussen nul en 100%)
+// pc.printf("moving_average_left = %f moving_average_right = %f \n\r", moving_average_left, moving_average_right);
+//
+//
+// // SLAG // (No LED -> 0.55s -> red on (turns on every time a new pwm is given) -> finish)
+// if (moving_average_left > Threshold_Bicep_Left_1 && moving_average_right > Threshold_Bicep_Right_1)
+// {
+// TICKER OFF
+// debug_led_green=off;
+// n=0;
+// pc.printf("slag \n\r");
+// wait(0.5);
+//
+// while(moving_average_left > Threshold_Bicep_Left_1 && moving_average_right > Threshold_Bicep_Right_1) // Threshold statement still true after 0.5 seconds?
+// {
+// if (n==0) //wordt maar 1 keer uitgevoerd
+// {
+// deactivate_PID_Controller_strike(); // function that sets P, I and D gain values to zero
+// n=1;
+// }
+// debug_led_red=off;
+// wait(0.10); // wait 20 samples
+// debug_led_red=on;
+// pwm_motor_strike=((moving_average_left-EMG_L_min)+(moving_average_right-EMG_R_min))/((EMG_L_max-EMG_L_min)+(EMG_R_max-EMG_R_min))*0.7 + 0.3; // min speed 0.3 en max 1
+// wait(0.10); // wait 20 samples more (pwm changes per 0.1 seconds)
+// motordirection_strike=cw; // towards bottle
+//
+// if((position_strike-Hit)<2) // bottle is hit when position-hit <2 defrees
+// {
+// activate_PID_Controller_strike(); // function sets back P, I and D gain values
+// pc.printf("einde \n\r");
+// goto Nieuwe_actie; // Finished: Get Ready for new Action control
+// }
+// }
+// }
+//
+// activate_PID_Controller_strike(); // function sets back P, I and D gain values
+// TICKER AAN
+// debug_led_red=off; // not inside an action loop (green light)
+// debug_led_blue=off;
+// debug_led_green=on; // want het kan zijn dat bovenste script half wordt uitgevoerd en dan moet het slag mechanisme wel weer terug
+//
+// // DRAAI LINKS // // (blue->blue off (very short/visible?)-> red<->blue<-> red -> klaar)
+// if (moving_average_left > Threshold_Bicep_Left_2 && moving_average_right < Threshold_Bicep_Right_1)
+// {
+// debug_led_green=off; // Executing action
+// TICKER UIT
+// n=0;
+// pc.printf("links \n\r");
+// while(moving_average_left > Threshold_Bicep_Left_1 && moving_average_right < Threshold_Bicep_Right_1)
+// {
+// debug_led_blue=on;
+// if (n==0) //wordt maar 1 keer uitgevoerd
+// {
+// debug_led_blue=off;
+// reference_turn=reference_turn+change_one_bottle;
+// n=1;
+// }
+//
+// if (fabs(position_turn-reference_turn)<2) // als error en kleiner dan twee graden
+// {
+// debug_led_blue=off;
+// debug_led_red=on;
+// wait(0.5);
+// if (fabs(position_turn-reference_turn)<2) // Is de error na 0.5 seconde nog steeds kleiner dan twee graden?
+// {
+// goto Nieuwe_actie; // kunt weer iets nieuws doen indien vorige actie is uitgevoerd
+// }
+// }
+// }
+// }
+//
+// debug_led_red=off; // not inside an action loop
+// debug_led_blue=off;
+// debug_led_green=on; // not executing an action
+// TICKER AAN
+//
+// // DRAAI RECHTS // (blue->blue uit (heel kort/zichtbaar?)-> red<->blue<-> red -> klaar)
+// if (moving_average_right > Threshold_Bicep_Right_2 && moving_average_left < Threshold_Bicep_Right_1)
+// {
+// TICKER UIT
+// debug_led_green=off; // Executing action
+// pc.printf("rechts \n\r");
+// n=0;
+// while(moving_average_right > Threshold_Bicep_Right_1 && moving_average_left < Threshold_Bicep_Left_1)
+// {
+// debug_led_blue=on;
+// if (n==0)
+// {
+// debug_led_blue=off;
+// reference_turn=reference_turn-change_one_bottle;
+// n=1;
+// }
+// //(45 graden naar rechts voor volgende fles) //wordt maar 1 keer uitgevoerd
+// pc.printf("Ref: %f Err: %f \n\r", reference_turn, position_turn);
+//
+// if (abs(position_turn-reference_turn)<2) // als error en kleiner dan twee graden
+// {
+// debug_led_blue=off;
+// debug_led_red=on;
+// wait(0.5);
+// if (abs(position_turn-reference_turn)<2) // Is de error na 0.5 seconde nog steeds kleiner dan twee graden?
+// {
+// goto Nieuwe_actie; // kunt weer iets nieuws doen indien vorige actie is uitgevoerd
+// }
+// }
+// }
+// }
+// debug_led_red=off; // not inside an action loop
+// debug_led_blue=off;
+// debug_led_green=on; // not executing an action
+//
+
+
+// TICKER WEER AAN
+
+// ___________________________
+// // \\
// || [PID CONTROLLER] ||
// \\___________________________//
// // Calculate error then multiply it with the (P, I and D) gains, and store in pwm_to_motor \\
@@ -376,11 +661,18 @@
}
}
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
- // //
- // [Functions Described] //
- // //
- ////////////////////////////////////
+//============================================================================================================================
+ ///////////////////////////////
+ // //
+///////////////////////////////////////////// [FUNCTIONS DESCRIBED] /////////////////////////////////////////////////////
+ // //
+ ///////////////////////////////
+//============================================================================================================================
+// ___________________________
+// // \\
+// || [MOTOR FUCNTIONS] ||
+// \\___________________________//
+//
// Keep in range function
void keep_in_range(double * in, double min, double max)
@@ -397,14 +689,14 @@
// Change reference
void Change_Turn_Position_Left (double& reference, double change_one_bottle)
{
- if(reference==90)
+ if(reference==90) // added reference if at boundary bottle and try to go to the side where no bottles are; than immediatly turn 5 bottles to the left
{
reference=-90;
}
else
{
reference = reference + change_one_bottle;
- keep_in_range(&reference, -90, 90); // reference position stays between -90 and 90 degrees (IF bottles at -90, -45, 0, 45, 90 degrees)
+ keep_in_range(&reference, -90, 90); // reference position stays between -90 and 90 degrees (IF bottles at -90, -45, 0, 45, 90 degrees)
}
}
@@ -441,4 +733,168 @@
P_gain=0; // hier waardes P,I,D veranderen (waardes bovenaan doen (tijdelijk) niks meer
I_gain=0;
D_gain=0;
-}
\ No newline at end of file
+}
+
+//============================================================================================================================
+// ___________________________
+// // \\
+// || [EMG FUCNTIONS] ||
+// \\___________________________//
+//
+
+ // [CALIBRATION] // (blue LED)
+void calibration()
+{
+
+
+ // [MINIMUM VALUE BICEPS CALIBRATION] //
+
+
+ debug_led_blue=on;
+ pc.printf("Start minimum calibration in 5 seconds \n\r");
+ pc.printf("Keep your biceps as relaxed as possible \n\r");
+
+ countdown_from_5();
+ c=0;
+
+ while(c<2560) // 512Hz -> 2560 is equal to five seconds
+ {
+ Filter(); // Filter EMG signal
+ minimum_L=EMG_Left_Bicep_filtered+minimum_L; // Take previous sample EMG_Left_Bicep_filtered and add the new value
+ minimum_R=EMG_Right_Bicep_filtered+minimum_R;
+// scope.set(0,EMG_left_Bicep);
+// scope.set(1,EMG_Left_Bicep_filtered);
+// scope.set(2,minimum_L);
+// scope.send();
+ c++; // Every sample c is increased by one until the statement c<2560 is false
+ wait(0.001953125); // wait one sample
+ }
+
+ pc.printf("Finished minimum calibration \n\r");
+
+ EMG_L_min=minimum_L/2560; // Divide the summation by the number of measurements (2560 measurements) to get a mean value over 5 seconds
+ EMG_R_min=minimum_R/2560;
+
+ pc.printf("EMG_L_min = %f \n\r EMG_R_min = %f \n\r", EMG_L_min, EMG_R_min);
+
+ wait (3); //cooldown
+
+
+ // [MAXIMUM VALUE BICEPS CALIBRATION] //
+
+
+ pc.printf("start maximum calibration in 5 seconds (start contraction at 3) \n\r");
+
+ countdown_from_5();
+ c=0;
+
+ while(c<2560) // 512Hz -> 2560 is equal to five seconds
+ {
+ Filter(); // Filter EMG signal
+ maximum_L=EMG_Left_Bicep_filtered+maximum_L; // Take previous sample EMG_Left_Bicep_filtered and add the new value
+ maximum_R=EMG_Right_Bicep_filtered+maximum_R;
+// scope.set(0,EMG_left_Bicep);
+// scope.set(1,EMG_Left_Bicep_filtered);
+// scope.set(2,maximum_R);
+// scope.send();
+ c++; // Every sample c is increased by one until the statement c<2560 is false
+ wait(0.002);
+ }
+
+ pc.printf("Finished minimum calibration \n\r");
+
+ EMG_L_max=maximum_L/2560; // Divide the summation by the number of measurements (2560 measurements) to get a mean value over 5 seconds
+ EMG_R_max=maximum_R/2560;
+
+ pc.printf("EMG_L_max = %f \n\r EMG_R_max = %f \n\r", EMG_L_max, EMG_R_max);
+
+ wait (3); //cooldown
+
+ debug_led_blue=off;
+
+
+ // [MAXIMUM VALUE BICEPS CALIBRATION] //
+ // Calculate threshold percentages //
+
+ const float Threshold_Bicep_Left_1=((EMG_L_max-EMG_L_min)*0.2)+EMG_L_min;; //(waarde waarop het gemeten EMG signaal 20% van max het maximale is); // LEFT
+ const float Threshold_Bicep_Left_2=((EMG_L_max-EMG_L_min)*0.6)+EMG_L_min; //(waarde waarop het gemeten EMG signaal 60% van max het maximale is);
+ const float Threshold_Bicep_Right_1=((EMG_R_max-EMG_R_min)*0.2)+EMG_R_min; //(waarde waarop het gemeten EMG signaal 20% van max het maximale is); // RIGHT
+ const float Threshold_Bicep_Right_2=((EMG_R_max-EMG_R_min)*0.6)+EMG_R_min; //(waarde waarop het gemeten EMG signaal 60% van max het maximale is);
+
+ 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);
+
+}
+
+
+ // [COUNTDOWN FUNCTION] //
+
+
+void countdown_from_5() // Countdown from 5 till 0 inside Putty (interface)
+ {
+ 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");
+ wait(1); pc.printf("1 Set \n\r"); wait(1); pc.printf("Go \n\r");
+ }
+
+
+ // [FLOW CONTROL FUNCTIONS] //
+
+
+void ControlGo() //Control flag
+{
+ control_go = true;
+}
+
+void take_sample() // Take a sample every 25th sample
+{
+ if(n==25)
+ {
+ sample = true;
+ n=0;
+ debug_led_green = off;
+ }
+}
+
+
+ // [FILTER FUNCTIONS] //
+ // [EMG] //
+
+void Filter() // Unfiltered EMG (input) -> highpass filter -> rectify -> lowpass filter -> Filtered EMG (output)
+ {
+ EMG_left_Bicep = input1; EMG_Right_Bicep = input2;
+
+ EMG_Left_Bicep_filtered = highpassfilter_1.step(EMG_left_Bicep); EMG_Right_Bicep_filtered = highpassfilter_2.step(EMG_Right_Bicep);
+ EMG_Left_Bicep_filtered = fabs(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = fabs(EMG_Right_Bicep_filtered);
+ EMG_Left_Bicep_filtered = lowpassfilter_1.step(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = lowpassfilter_2.step(EMG_Right_Bicep_filtered);
+
+ EMG_Left_Bicep_filtered = EMG_Left_Bicep_filtered; EMG_Right_Bicep_filtered = EMG_Right_Bicep_filtered;
+ }
+
+
+ // [FILTER FUNCTIONS] //
+ // [Include Moving Average] //
+
+void sample_filter()
+{
+ Filter();
+ take_sample();
+ if(sample)
+ {
+ sample=false;
+ Sample_EMG_L_1 = EMG_Left_Bicep_filtered; Sample_EMG_R_1 = EMG_Right_Bicep_filtered;
+
+ Sample_EMG_L_10= Sample_EMG_L_9; Sample_EMG_R_10= Sample_EMG_R_9;
+ Sample_EMG_L_9 = Sample_EMG_L_8; Sample_EMG_R_9 = Sample_EMG_R_8;
+ Sample_EMG_L_8 = Sample_EMG_L_7; Sample_EMG_R_8 = Sample_EMG_R_7;
+ Sample_EMG_L_7 = Sample_EMG_L_6; Sample_EMG_R_7 = Sample_EMG_R_6;
+ Sample_EMG_L_6 = Sample_EMG_L_5; Sample_EMG_R_6 = Sample_EMG_R_5;
+ Sample_EMG_L_5 = Sample_EMG_L_4; Sample_EMG_R_5 = Sample_EMG_R_4;
+ Sample_EMG_L_4 = Sample_EMG_L_3; Sample_EMG_R_4 = Sample_EMG_R_3;
+ Sample_EMG_L_3 = Sample_EMG_L_2; Sample_EMG_R_3 = Sample_EMG_R_2;
+ Sample_EMG_L_2 = Sample_EMG_L_1; Sample_EMG_R_2 = Sample_EMG_R_1;
+ }
+ 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;
+ 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;
+ n++;
+}
+
+//============================================================================================================================
\ No newline at end of file
