ZZ-TheChenneRobot

Users » ThomasBNL » Code » ZZ-TheChenneRobot

Thomas Burgers / Mbed 2 deprecated ZZ-TheChenneRobot

Dependencies: Encoder HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@100:e930203df49a, 2015-10-30 (annotated)

Committer:: ThomasBNL
Date:: Fri Oct 30 08:55:05 2015 +0000
Revision:: 100:e930203df49a
Parent:: 99:be25087d1bfc
Child:: 101:b5e968a44040

Calibrate motor position with EMG

Who changed what in which revision?

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