robot_2 - 2nd draft

group14 - k9 » Code » robot_2

group14 - k9 / Mbed 2 deprecated robot_2

2nd draft

Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed Servo

Fork of robot_mockup by Martijn Kern

main.cpp@57:d6192801fd6d, 2015-10-27 (annotated)

Committer:: Vigilance88
Date:: Tue Oct 27 20:46:20 2015 +0000
Revision:: 57:d6192801fd6d
Parent:: 56:5ff9e5c1ed44
Child:: 58:db11481da856

changed dls, less calculation each loop

Who changed what in which revision?

User	Revision	Line number	New contents of line
vsluiter	0:32bb76391d89	1	#include "mbed.h"
vsluiter	11:ce72ec658a95	2	#include "HIDScope.h"
Vigilance88	18:44905b008f44	3	#include "MODSERIAL.h"
Vigilance88	18:44905b008f44	4	#include "biquadFilter.h"
Vigilance88	54:4cda9af56bed	5	#include "Servo.h"
Vigilance88	18:44905b008f44	6	#include "QEI.h"
Vigilance88	21:d6a46315d5f5	7	#include "math.h"
Vigilance88	26:fe3a5469dd6b	8	#include <string>
Vigilance88	21:d6a46315d5f5	9
Vigilance88	21:d6a46315d5f5	10	/*--------------------------------------------------------------------------------------------------------------------
Vigilance88	21:d6a46315d5f5	11	-------------------------------- BIOROBOTICS GROUP 14 ----------------------------------------------------------------
Vigilance88	21:d6a46315d5f5	12	--------------------------------------------------------------------------------------------------------------------*/
vsluiter	0:32bb76391d89	13
Vigilance88	18:44905b008f44	14	//Define important constants in memory
Vigilance88	21:d6a46315d5f5	15	#define PI 3.14159265
Vigilance88	18:44905b008f44	16	#define SAMPLE_RATE 0.002 //500 Hz EMG sample rate
Vigilance88	57:d6192801fd6d	17	#define CONTROL_RATE 0.01 //100 Hz Control rate
Vigilance88	56:5ff9e5c1ed44	18	#define SERVO_RATE 0.05 //50 Hz Servo Control rate
Vigilance88	56:5ff9e5c1ed44	19	#define ENCODER_CPR 4200 //both motor encoders have 64 (X4), 32 (X2) counts per revolution of motor shaft
Vigilance88	49:6515c045cfd6	20	//gearbox 1:131.25 -> 4200 counts per revolution of the output shaft (X2),
Vigilance88	26:fe3a5469dd6b	21	#define PWM_PERIOD 0.0001 //10k Hz pwm motor frequency. Higher -> too hot, lower -> motor doesnt respond correctly
Vigilance88	21:d6a46315d5f5	22	/*--------------------------------------------------------------------------------------------------------------------
Vigilance88	21:d6a46315d5f5	23	---- OBJECTS ---------------------------------------------------------------------------------------------------------
Vigilance88	21:d6a46315d5f5	24	--------------------------------------------------------------------------------------------------------------------*/
Vigilance88	21:d6a46315d5f5	25
Vigilance88	18:44905b008f44	26	MODSERIAL pc(USBTX,USBRX); //serial communication
Vigilance88	18:44905b008f44	27
Vigilance88	36:4d4fc200171b	28	//Debug LEDs
Vigilance88	25:49ccdc98639a	29	DigitalOut red(LED_RED);
Vigilance88	25:49ccdc98639a	30	DigitalOut green(LED_GREEN);
Vigilance88	25:49ccdc98639a	31	DigitalOut blue(LED_BLUE);
Vigilance88	25:49ccdc98639a	32
Vigilance88	21:d6a46315d5f5	33	//EMG shields
Vigilance88	18:44905b008f44	34	AnalogIn emg1(A0); //Analog input - Biceps EMG
Vigilance88	18:44905b008f44	35	AnalogIn emg2(A1); //Analog input - Triceps EMG
Vigilance88	18:44905b008f44	36	AnalogIn emg3(A2); //Analog input - Flexor EMG
Vigilance88	18:44905b008f44	37	AnalogIn emg4(A3); //Analog input - Extensor EMG
Vigilance88	18:44905b008f44	38
Vigilance88	18:44905b008f44	39	Ticker sample_timer; //Ticker for EMG sampling
Vigilance88	18:44905b008f44	40	Ticker control_timer; //Ticker for control loop
Vigilance88	56:5ff9e5c1ed44	41	Ticker servo_timer; //Ticker for servo control
Vigilance88	56:5ff9e5c1ed44	42	Ticker debug_timer;
Vigilance88	57:d6192801fd6d	43	//HIDScope scope(2); //Scope 4 channels
Vigilance88	18:44905b008f44	44
Vigilance88	18:44905b008f44	45	// AnalogIn potmeter(A4); //potmeters
Vigilance88	18:44905b008f44	46	// AnalogIn potmeter2(A5); //
Vigilance88	18:44905b008f44	47
Vigilance88	21:d6a46315d5f5	48	//Encoders
Vigilance88	18:44905b008f44	49	QEI Encoder1(D13,D12,NC,32); //channel A and B from encoder, counts = Encoder.getPulses();
Vigilance88	18:44905b008f44	50	QEI Encoder2(D10,D9,NC,32); //channel A and B from encoder,
Vigilance88	21:d6a46315d5f5	51
Vigilance88	21:d6a46315d5f5	52	//Speed and Direction of motors - D4 (dir) and D5(speed) = motor 2, D7(dir) and D6(speed) = motor 1
Vigilance88	21:d6a46315d5f5	53	PwmOut pwm_motor1(D6); //PWM motor 1
Vigilance88	21:d6a46315d5f5	54	PwmOut pwm_motor2(D5); //PWM motor 2
Vigilance88	53:bf0d97487e84	55	Servo servoPwm(D11); //PWM servomotor
Vigilance88	26:fe3a5469dd6b	56
Vigilance88	18:44905b008f44	57	DigitalOut dir_motor1(D7); //Direction motor 1
Vigilance88	18:44905b008f44	58	DigitalOut dir_motor2(D4); //Direction motor 2
Vigilance88	18:44905b008f44	59
Vigilance88	24:56db31267f10	60	//Limit Switches
Vigilance88	28:743485bb51e4	61	InterruptIn shoulder_limit(D2); //using FRDM buttons
Vigilance88	28:743485bb51e4	62	InterruptIn elbow_limit(D3); //using FRDM buttons
Vigilance88	26:fe3a5469dd6b	63
Vigilance88	26:fe3a5469dd6b	64	//Other buttons
Vigilance88	26:fe3a5469dd6b	65	DigitalIn button1(PTA4); //using FRDM buttons
Vigilance88	26:fe3a5469dd6b	66	DigitalIn button2(PTC6); //using FRDM buttons
Vigilance88	26:fe3a5469dd6b	67
Vigilance88	26:fe3a5469dd6b	68	/*Text colors ASCII code: Set Attribute Mode <ESC>[{attr1};...;{attrn}m
Vigilance88	26:fe3a5469dd6b	69
Vigilance88	26:fe3a5469dd6b	70	\ 0 3 3 - ESC
Vigilance88	26:fe3a5469dd6b	71	[ 3 0 m - black
Vigilance88	26:fe3a5469dd6b	72	[ 3 1 m - red
Vigilance88	26:fe3a5469dd6b	73	[ 3 2 m - green
Vigilance88	26:fe3a5469dd6b	74	[ 3 3 m - yellow
Vigilance88	26:fe3a5469dd6b	75	[ 3 4 m - blue
Vigilance88	26:fe3a5469dd6b	76	[ 3 5 m - magenta
Vigilance88	26:fe3a5469dd6b	77	[ 3 6 m - cyan
Vigilance88	26:fe3a5469dd6b	78	[ 3 7 m - white
Vigilance88	26:fe3a5469dd6b	79	[ 0 m - reset attributes
Vigilance88	26:fe3a5469dd6b	80
Vigilance88	26:fe3a5469dd6b	81	Put the text you want to color between GREEN_ and _GREEN
Vigilance88	26:fe3a5469dd6b	82	*/
Vigilance88	26:fe3a5469dd6b	83	string GREEN_ = "\033[32m"; //esc - green
Vigilance88	26:fe3a5469dd6b	84	string _GREEN = "\033[0m"; //esc - reset
Vigilance88	24:56db31267f10	85
Vigilance88	21:d6a46315d5f5	86
Vigilance88	21:d6a46315d5f5	87	/*--------------------------------------------------------------------------------------------------------------------
Vigilance88	21:d6a46315d5f5	88	---- DECLARE VARIABLES -----------------------------------------------------------------------------------------------
Vigilance88	21:d6a46315d5f5	89	--------------------------------------------------------------------------------------------------------------------*/
Vigilance88	56:5ff9e5c1ed44	90	volatile bool debug = true;
Vigilance88	21:d6a46315d5f5	91
Vigilance88	47:c52f9b4c90c4	92	//EMG variables: raw EMG - filtered EMG - maximum voluntary contraction - max amplitude during relaxation.
Vigilance88	38:c8ac615d0c8f	93	double emg_biceps; double biceps_power; double bicepsMVC = 0; double bicepsmin=0;
Vigilance88	38:c8ac615d0c8f	94	double emg_triceps; double triceps_power; double tricepsMVC = 0; double tricepsmin=0;
Vigilance88	38:c8ac615d0c8f	95	double emg_flexor; double flexor_power; double flexorMVC = 0; double flexormin=0;
Vigilance88	38:c8ac615d0c8f	96	double emg_extens; double extens_power; double extensMVC = 0; double extensmin=0;
Vigilance88	47:c52f9b4c90c4	97
Vigilance88	39:e77f844d10d9	98	//Normalize and compare variables
Vigilance88	39:e77f844d10d9	99	double biceps, triceps, flexor, extens; //Storage for normalized emg
Vigilance88	39:e77f844d10d9	100	double xdir, ydir; //EMG reference position directions
Vigilance88	39:e77f844d10d9	101	double xpower, ypower; //EMG reference magnitude
Vigilance88	47:c52f9b4c90c4	102	double dx, dy; //Integral
Vigilance88	47:c52f9b4c90c4	103	double emg_control_time; //Elapsed time in EMG control
Vigilance88	46:c8c5c455dd51	104
Vigilance88	44:145827f5b091	105	//Threshold moving average
Vigilance88	47:c52f9b4c90c4	106	const int window=100; //100 samples
Vigilance88	47:c52f9b4c90c4	107	int i=0; //movavg array index
Vigilance88	47:c52f9b4c90c4	108	double movavg1[window]; //moving average arrays with size of window
Vigilance88	44:145827f5b091	109	double movavg2[window];
Vigilance88	44:145827f5b091	110	double movavg3[window];
Vigilance88	44:145827f5b091	111	double movavg4[window];
Vigilance88	47:c52f9b4c90c4	112	double biceps_avg, triceps_avg,flexor_avg, extens_avg; //sum divided by window size
Vigilance88	46:c8c5c455dd51	113
Vigilance88	36:4d4fc200171b	114	int muscle; //Muscle selector for MVC measurement, 1 = first emg etc.
Vigilance88	47:c52f9b4c90c4	115	double calibrate_time; //Elapsed time for each measurement
Vigilance88	25:49ccdc98639a	116
Vigilance88	24:56db31267f10	117	//PID variables
Vigilance88	36:4d4fc200171b	118	double u1; double u2; //Output of PID controller (PWM value for motor 1 and 2)
Vigilance88	47:c52f9b4c90c4	119	double m1_error=0; double m1_e_int=0; double m1_e_prev=0; //Error, integrated error, previous error motor 1
Vigilance88	55:726fdab812a9	120	const double m1_kp=5; const double m1_ki=0.1; const double m1_kd=0.3; //Proportional, integral and derivative gains.
Vigilance88	24:56db31267f10	121
Vigilance88	47:c52f9b4c90c4	122	double m2_error=0; double m2_e_int=0; double m2_e_prev=0; //Error, integrated error, previous error motor 2
Vigilance88	55:726fdab812a9	123	const double m2_kp=5; const double m2_ki=0.1; const double m2_kd=0.3; //Proportional, integral and derivative gains.
Vigilance88	24:56db31267f10	124
Vigilance88	36:4d4fc200171b	125	//Calibration bools, checks if elbow/shoulder limits are hit
Vigilance88	47:c52f9b4c90c4	126	volatile bool done1 = false;
Vigilance88	47:c52f9b4c90c4	127	volatile bool done2 = false;
Vigilance88	47:c52f9b4c90c4	128	volatile bool calibrating = false;
Vigilance88	32:76c4d7bb2022	129
Vigilance88	24:56db31267f10	130	//highpass filter 20 Hz
Vigilance88	24:56db31267f10	131	const double high_b0 = 0.956543225556877;
Vigilance88	24:56db31267f10	132	const double high_b1 = -1.91308645113754;
Vigilance88	24:56db31267f10	133	const double high_b2 = 0.956543225556877;
Vigilance88	24:56db31267f10	134	const double high_a1 = -1.91197067426073;
Vigilance88	24:56db31267f10	135	const double high_a2 = 0.9149758348014341;
Vigilance88	24:56db31267f10	136
Vigilance88	24:56db31267f10	137	//notchfilter 50Hz
Vigilance88	36:4d4fc200171b	138	/*
Vigilance88	24:56db31267f10	139	Method = Butterworth
Vigilance88	24:56db31267f10	140	Biquad = Yes
Vigilance88	24:56db31267f10	141	Stable = Yes
Vigilance88	24:56db31267f10	142	Sampling Frequency = 500Hz
Vigilance88	24:56db31267f10	143	Filter Order = 2
Vigilance88	24:56db31267f10	144
Vigilance88	24:56db31267f10	145	Band Frequencies (Hz) Att/Ripple (dB) Biquad #1 Biquad #2
Vigilance88	24:56db31267f10	146
Vigilance88	24:56db31267f10	147	1 0, 48 0.001 Gain = 1.000000 Gain = 0.973674
Vigilance88	24:56db31267f10	148	2 49, 51 -60.000 B = [ 1.00000000000, -1.61816176147, 1.00000000000] B = [ 1.00000000000, -1.61816176147, 1.00000000000]
Vigilance88	24:56db31267f10	149	3 52, 250 0.001 A = [ 1.00000000000, -1.58071559235, 0.97319685401] A = [ 1.00000000000, -1.61244708381, 0.97415116257]
Vigilance88	24:56db31267f10	150	*/
Vigilance88	24:56db31267f10	151
Vigilance88	24:56db31267f10	152	//biquad 1
Vigilance88	24:56db31267f10	153	const double notch1gain = 1.000000;
Vigilance88	24:56db31267f10	154	const double notch1_b0 = 1;
Vigilance88	24:56db31267f10	155	const double notch1_b1 = -1.61816176147 * notch1gain;
Vigilance88	24:56db31267f10	156	const double notch1_b2 = 1.00000000000 * notch1gain;
Vigilance88	24:56db31267f10	157	const double notch1_a1 = -1.58071559235 * notch1gain;
Vigilance88	24:56db31267f10	158	const double notch1_a2 = 0.97319685401 * notch1gain;
Vigilance88	24:56db31267f10	159
Vigilance88	24:56db31267f10	160	//biquad 2
Vigilance88	24:56db31267f10	161	const double notch2gain = 0.973674;
Vigilance88	24:56db31267f10	162	const double notch2_b0 = 1 * notch2gain;
Vigilance88	24:56db31267f10	163	const double notch2_b1 = -1.61816176147 * notch2gain;
Vigilance88	24:56db31267f10	164	const double notch2_b2 = 1.00000000000 * notch2gain;
Vigilance88	24:56db31267f10	165	const double notch2_a1 = -1.61244708381 * notch2gain;
Vigilance88	24:56db31267f10	166	const double notch2_a2 = 0.97415116257 * notch2gain;
Vigilance88	24:56db31267f10	167
Vigilance88	26:fe3a5469dd6b	168	//lowpass filter 7 Hz - envelope
Vigilance88	24:56db31267f10	169	const double low_b0 = 0.000119046743110057;
Vigilance88	24:56db31267f10	170	const double low_b1 = 0.000238093486220118;
Vigilance88	24:56db31267f10	171	const double low_b2 = 0.000119046743110057;
Vigilance88	24:56db31267f10	172	const double low_a1 = -1.968902268531908;
Vigilance88	24:56db31267f10	173	const double low_a2 = 0.9693784555043481;
Vigilance88	21:d6a46315d5f5	174
Vigilance88	51:e4a0ce7ff4b8	175	//Derivative lowpass filter 60 Hz - remove error noise
Vigilance88	51:e4a0ce7ff4b8	176	const double derlow_b0 = 0.027859766117136;
Vigilance88	51:e4a0ce7ff4b8	177	const double derlow_b1 = 0.0557195322342721;
Vigilance88	51:e4a0ce7ff4b8	178	const double derlow_b2 = 0.027859766117136;
Vigilance88	51:e4a0ce7ff4b8	179	const double derlow_a1 = -1.47548044359265;
Vigilance88	51:e4a0ce7ff4b8	180	const double derlow_a2 = 0.58691950806119;
Vigilance88	51:e4a0ce7ff4b8	181
Vigilance88	36:4d4fc200171b	182	//Forward Kinematics
Vigilance88	36:4d4fc200171b	183	const double l1 = 0.25; const double l2 = 0.25; //Arm lengths
Vigilance88	36:4d4fc200171b	184	double current_x; double current_y; //Current position
Vigilance88	54:4cda9af56bed	185	double theta1; double theta2; double theta3; //Current angles
Vigilance88	56:5ff9e5c1ed44	186	double deltatheta1; double deltatheta2; //Change in angles compared to mechanical lower limit - for servo
Vigilance88	54:4cda9af56bed	187	double servopos; //servo position in usec
Vigilance88	36:4d4fc200171b	188	double rpc; //Encoder resolution: radians per count
Vigilance88	36:4d4fc200171b	189
Vigilance88	36:4d4fc200171b	190	//Reference position
Vigilance88	28:743485bb51e4	191	double x; double y;
Vigilance88	36:4d4fc200171b	192
Vigilance88	39:e77f844d10d9	193	//Select whether to use Trig or DLS method, emg true or false
Vigilance88	38:c8ac615d0c8f	194	int control_method;
Vigilance88	39:e77f844d10d9	195	bool emg_control;
Vigilance88	38:c8ac615d0c8f	196
Vigilance88	36:4d4fc200171b	197	//Inverse Kinematics - Trig / Gonio method.
Vigilance88	36:4d4fc200171b	198	//First convert reference position to angle needed, then compare that angle to current angle.
Vigilance88	56:5ff9e5c1ed44	199	double reftheta1; double reftheta2; //reference angles
Vigilance88	36:4d4fc200171b	200	double costheta1; double sintheta1; //helper variables
Vigilance88	36:4d4fc200171b	201	double costheta2; double sintheta2; //
Vigilance88	57:d6192801fd6d	202	double powlambda2, powlambda4;
Vigilance88	36:4d4fc200171b	203
Vigilance88	36:4d4fc200171b	204	//Inverse Kinematics - Damped least squares method.
Vigilance88	36:4d4fc200171b	205	//Angle error is directly calculated from position error: dq = [DLS matrix] * position_error
Vigilance88	36:4d4fc200171b	206	// \|DLS1 DLS2\|
Vigilance88	36:4d4fc200171b	207	double dls1, dls2, dls3, dls4; //DLS matrix: \|DLS3 DLS4\|
Vigilance88	36:4d4fc200171b	208	double q1_error, q2_error; //Angle errors
Vigilance88	47:c52f9b4c90c4	209	double x_error, y_error; //Position errors
Vigilance88	36:4d4fc200171b	210	double lambda=0.1; //Damping constant
Vigilance88	21:d6a46315d5f5	211
Vigilance88	41:55face19e06b	212	//Mechanical Limits
Vigilance88	41:55face19e06b	213	int theta1_cal, theta2_cal; //Pulse counts at mechanical limits.
Vigilance88	41:55face19e06b	214	double theta1_lower=0.698132, theta1_upper=2.35619; //motor1: lower limit 40 degrees, upper limit 135
Vigilance88	41:55face19e06b	215	double theta2_lower=0.750492, theta2_upper=2.40855; //motor2: lower limit 43 degrees, upper limit 138 degrees.
Vigilance88	41:55face19e06b	216
Vigilance88	21:d6a46315d5f5	217	/*--------------------------------------------------------------------------------------------------------------------
Vigilance88	24:56db31267f10	218	---- Filters ---------------------------------------------------------------------------------------------------------
Vigilance88	21:d6a46315d5f5	219	--------------------------------------------------------------------------------------------------------------------*/
Vigilance88	24:56db31267f10	220
Vigilance88	24:56db31267f10	221	//Using biquadFilter library
Vigilance88	24:56db31267f10	222	//Syntax: biquadFilter filter(a1, a2, b0, b1, b2); coefficients. Call with: filter.step(u), with u signal to be filtered.
Vigilance88	26:fe3a5469dd6b	223	//Biceps
Vigilance88	24:56db31267f10	224	biquadFilter highpass( high_a1 , high_a2 , high_b0 , high_b1 , high_b2 ); // removes DC and movement artefacts
Vigilance88	24:56db31267f10	225	biquadFilter notch1( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 ); // removes 49-51 Hz power interference
Vigilance88	24:56db31267f10	226	biquadFilter notch2( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 ); //
Vigilance88	24:56db31267f10	227	biquadFilter lowpass( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // EMG envelope
Vigilance88	25:49ccdc98639a	228
Vigilance88	26:fe3a5469dd6b	229	//Triceps
Vigilance88	25:49ccdc98639a	230	biquadFilter highpass2( high_a1 , high_a2 , high_b0 , high_b1 , high_b2 ); // removes DC and movement artefacts
Vigilance88	26:fe3a5469dd6b	231	biquadFilter notch1_2( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 ); // removes 49-51 Hz power interference
Vigilance88	26:fe3a5469dd6b	232	biquadFilter notch2_2( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 ); //
Vigilance88	25:49ccdc98639a	233	biquadFilter lowpass2( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // EMG envelope
Vigilance88	25:49ccdc98639a	234
Vigilance88	26:fe3a5469dd6b	235	//Flexor
Vigilance88	25:49ccdc98639a	236	biquadFilter highpass3( high_a1 , high_a2 , high_b0 , high_b1 , high_b2 ); // removes DC and movement artefacts
Vigilance88	26:fe3a5469dd6b	237	biquadFilter notch1_3( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 ); // removes 49-51 Hz power interference
Vigilance88	26:fe3a5469dd6b	238	biquadFilter notch2_3( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 ); //
Vigilance88	25:49ccdc98639a	239	biquadFilter lowpass3( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // EMG envelope
Vigilance88	25:49ccdc98639a	240
Vigilance88	26:fe3a5469dd6b	241	//Extensor
Vigilance88	25:49ccdc98639a	242	biquadFilter highpass4( high_a1 , high_a2 , high_b0 , high_b1 , high_b2 ); // removes DC and movement artefacts
Vigilance88	26:fe3a5469dd6b	243	biquadFilter notch1_4( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 ); // removes 49-51 Hz power interference
Vigilance88	26:fe3a5469dd6b	244	biquadFilter notch2_4( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 ); //
Vigilance88	25:49ccdc98639a	245	biquadFilter lowpass4( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // EMG envelope
Vigilance88	25:49ccdc98639a	246
Vigilance88	51:e4a0ce7ff4b8	247	//PID filter (lowpass 60 Hz)
Vigilance88	51:e4a0ce7ff4b8	248	biquadFilter derfilter1( derlow_a1 , derlow_a2 , derlow_b0 , derlow_b1 , derlow_b2 ); // derivative filter
Vigilance88	51:e4a0ce7ff4b8	249	biquadFilter derfilter2( derlow_a1 , derlow_a2 , derlow_b0 , derlow_b1 , derlow_b2 ); // derivative filter
Vigilance88	40:d62f96ed44c0	250
Vigilance88	24:56db31267f10	251	/*--------------------------------------------------------------------------------------------------------------------
Vigilance88	24:56db31267f10	252	---- DECLARE FUNCTION NAMES ------------------------------------------------------------------------------------------
Vigilance88	24:56db31267f10	253	--------------------------------------------------------------------------------------------------------------------*/
Vigilance88	26:fe3a5469dd6b	254
Vigilance88	26:fe3a5469dd6b	255	void sample_filter(void); //Sampling and filtering
Vigilance88	26:fe3a5469dd6b	256	void control(); //Control - reference -> error -> pid -> motor output
Vigilance88	56:5ff9e5c1ed44	257	void servo_control(); //Mouse alignment with servo
Vigilance88	37:4d7b7ced20ef	258	void calibrate_emg(); //Instructions + measurement of Min and MVC of each muscle
Vigilance88	26:fe3a5469dd6b	259	void emg_mvc(); //Helper funcion for storing MVC value
Vigilance88	37:4d7b7ced20ef	260	void emg_min(); //Helper function for storing Min value
Vigilance88	26:fe3a5469dd6b	261	void calibrate_arm(void); //Calibration of the arm with limit switches
Vigilance88	26:fe3a5469dd6b	262	void start_sampling(void); //Attaches the sample_filter function to a 500Hz ticker
Vigilance88	26:fe3a5469dd6b	263	void stop_sampling(void); //Stops sample_filter
Vigilance88	26:fe3a5469dd6b	264	void start_control(void); //Attaches the control function to a 100Hz ticker
Vigilance88	26:fe3a5469dd6b	265	void stop_control(void); //Stops control function
Vigilance88	37:4d7b7ced20ef	266
Vigilance88	26:fe3a5469dd6b	267	//Keeps the input between min and max value
Vigilance88	24:56db31267f10	268	void keep_in_range(double * in, double min, double max);
Vigilance88	26:fe3a5469dd6b	269
Vigilance88	26:fe3a5469dd6b	270	//Reusable PID controller, previous and integral error need to be passed by reference
Vigilance88	21:d6a46315d5f5	271	double pid(double error, double kp, double ki, double kd,double &e_int, double &e_prev);
Vigilance88	47:c52f9b4c90c4	272	double pid2(double error, double kp, double ki, double kd,double &e_int, double &e_prev);
Vigilance88	18:44905b008f44	273
Vigilance88	26:fe3a5469dd6b	274	//Menu functions
Vigilance88	56:5ff9e5c1ed44	275	void debugging();
Vigilance88	21:d6a46315d5f5	276	void mainMenu();
Vigilance88	21:d6a46315d5f5	277	void caliMenu();
Vigilance88	28:743485bb51e4	278	void controlMenu();
Vigilance88	29:948b0b14f6be	279	void controlButtons();
Vigilance88	26:fe3a5469dd6b	280	void clearTerminal();
Vigilance88	28:743485bb51e4	281	void emgInstructions();
Vigilance88	28:743485bb51e4	282	void titleBox();
Vigilance88	26:fe3a5469dd6b	283
Vigilance88	32:76c4d7bb2022	284	//Limit switches - power off motors if switches hit (rising edge interrupt)
Vigilance88	47:c52f9b4c90c4	285	void calibrate(void);
Vigilance88	32:76c4d7bb2022	286	void shoulder();
Vigilance88	32:76c4d7bb2022	287	void elbow();
Vigilance88	21:d6a46315d5f5	288
Vigilance88	21:d6a46315d5f5	289	/*--------------------------------------------------------------------------------------------------------------------
Vigilance88	21:d6a46315d5f5	290	---- MAIN LOOP -------------------------------------------------------------------------------------------------------
Vigilance88	21:d6a46315d5f5	291	--------------------------------------------------------------------------------------------------------------------*/
Vigilance88	56:5ff9e5c1ed44	292
Vigilance88	21:d6a46315d5f5	293	int main()
Vigilance88	21:d6a46315d5f5	294	{
Vigilance88	29:948b0b14f6be	295	pc.baud(115200); //serial baudrate
Vigilance88	30:a9fdd3202ca2	296	red=1; green=1; blue=1; //Make sure debug LEDs are off
Vigilance88	26:fe3a5469dd6b	297
Vigilance88	56:5ff9e5c1ed44	298	servoPwm.Enable(600,20000); //Start position servo, PWM period in usecs
Vigilance88	53:bf0d97487e84	299
Vigilance88	50:54f71544964c	300	//theta1_cal = floor(theta1_lower * (4200/(2*PI)));
Vigilance88	50:54f71544964c	301	//Encoder1.setPulses(theta1_cal); //edited QEI library: added setPulses()
Vigilance88	46:c8c5c455dd51	302
Vigilance88	46:c8c5c455dd51	303	//Mechanical limit 43 degrees -> 43*(4200/360) = 350
Vigilance88	50:54f71544964c	304	//theta2_cal = floor(theta2_lower * (4200/(2*PI)));
Vigilance88	50:54f71544964c	305	//Encoder2.setPulses(theta2_cal);
Vigilance88	46:c8c5c455dd51	306
Vigilance88	50:54f71544964c	307	//x = 0.2220;
Vigilance88	50:54f71544964c	308	//y = 0.4088;
Vigilance88	46:c8c5c455dd51	309
Vigilance88	26:fe3a5469dd6b	310	//Set PwmOut frequency to 10k Hz
Vigilance88	42:b9d26b1218b0	311	pwm_motor1.period(0.001);
Vigilance88	42:b9d26b1218b0	312	pwm_motor2.period(0.001);
Vigilance88	26:fe3a5469dd6b	313
Vigilance88	26:fe3a5469dd6b	314	clearTerminal(); //Clear the putty window
Vigilance88	26:fe3a5469dd6b	315
Vigilance88	24:56db31267f10	316	// make a menu, user has to initiate next step
Vigilance88	28:743485bb51e4	317	titleBox();
Vigilance88	26:fe3a5469dd6b	318	mainMenu();
Vigilance88	47:c52f9b4c90c4	319
Vigilance88	32:76c4d7bb2022	320	char command=0;
Vigilance88	27:d1814e271a95	321
Vigilance88	28:743485bb51e4	322	while(command != 'Q' && command != 'q')
Vigilance88	28:743485bb51e4	323	{
Vigilance88	28:743485bb51e4	324	if(pc.readable()){
Vigilance88	28:743485bb51e4	325	command = pc.getc();
Vigilance88	28:743485bb51e4	326
Vigilance88	28:743485bb51e4	327	switch(command){
Vigilance88	28:743485bb51e4	328
Vigilance88	28:743485bb51e4	329	case 'c':
Vigilance88	47:c52f9b4c90c4	330	case 'C': {
Vigilance88	28:743485bb51e4	331	pc.printf("\n\r => You chose calibration.\r\n\n");
Vigilance88	28:743485bb51e4	332	caliMenu();
Vigilance88	28:743485bb51e4	333
Vigilance88	28:743485bb51e4	334	char command2=0;
Vigilance88	28:743485bb51e4	335
Vigilance88	56:5ff9e5c1ed44	336	while(command2 != 'B' && command2 != 'b'){
Vigilance88	56:5ff9e5c1ed44	337	command2 = pc.getc();
Vigilance88	28:743485bb51e4	338	switch(command2){
Vigilance88	28:743485bb51e4	339	case 'a':
Vigilance88	28:743485bb51e4	340	case 'A':
Vigilance88	28:743485bb51e4	341	pc.printf("\n\r => Arm Calibration Starting... please wait \n\r");
Vigilance88	56:5ff9e5c1ed44	342	calibrate_arm(); wait(1);
Vigilance88	28:743485bb51e4	343	caliMenu();
Vigilance88	28:743485bb51e4	344	break;
Vigilance88	28:743485bb51e4	345
Vigilance88	28:743485bb51e4	346	case 'e':
Vigilance88	28:743485bb51e4	347	case 'E':
Vigilance88	28:743485bb51e4	348	pc.printf("\n\r => EMG Calibration Starting... please wait \n\r");
Vigilance88	28:743485bb51e4	349	wait(1);
Vigilance88	28:743485bb51e4	350	emgInstructions();
Vigilance88	28:743485bb51e4	351	calibrate_emg();
Vigilance88	32:76c4d7bb2022	352	pc.printf("\n\r------------------------- \n\r");
Vigilance88	28:743485bb51e4	353	pc.printf("\n\r EMG Calibration complete \n\r");
Vigilance88	32:76c4d7bb2022	354	pc.printf("\n\r------------------------- \n\r");
Vigilance88	28:743485bb51e4	355	caliMenu();
Vigilance88	28:743485bb51e4	356	break;
Vigilance88	28:743485bb51e4	357
Vigilance88	28:743485bb51e4	358	case 'b':
Vigilance88	28:743485bb51e4	359	case 'B':
Vigilance88	28:743485bb51e4	360	pc.printf("\n\r => Going back to main menu.. \n\r");
Vigilance88	28:743485bb51e4	361	mainMenu();
Vigilance88	28:743485bb51e4	362	break;
Vigilance88	28:743485bb51e4	363	}//end switch
Vigilance88	28:743485bb51e4	364
Vigilance88	56:5ff9e5c1ed44	365	}//end while
Vigilance88	56:5ff9e5c1ed44	366	break;
Vigilance88	47:c52f9b4c90c4	367	}//end case c C
Vigilance88	35:7d9fca0b1545	368	case 't':
Vigilance88	35:7d9fca0b1545	369	case 'T':
Vigilance88	56:5ff9e5c1ed44	370	pc.printf("=> You chose TRIG button control \r\n\n"); wait(1);
Vigilance88	56:5ff9e5c1ed44	371	start_sampling(); wait(1);
Vigilance88	39:e77f844d10d9	372	emg_control=false;
Vigilance88	38:c8ac615d0c8f	373	control_method=1;
Vigilance88	56:5ff9e5c1ed44	374	start_control(); wait(1);
Vigilance88	56:5ff9e5c1ed44	375	if(debug)
Vigilance88	56:5ff9e5c1ed44	376	{
Vigilance88	56:5ff9e5c1ed44	377	debug_timer.attach(&debugging,1);
Vigilance88	56:5ff9e5c1ed44	378	}
Vigilance88	29:948b0b14f6be	379	controlButtons();
Vigilance88	28:743485bb51e4	380	break;
Vigilance88	35:7d9fca0b1545	381	case 'd':
Vigilance88	35:7d9fca0b1545	382	case 'D':
Vigilance88	56:5ff9e5c1ed44	383	pc.printf("=> You chose DLS button control \r\n\n"); wait(1);
Vigilance88	56:5ff9e5c1ed44	384	start_sampling(); wait(1);
Vigilance88	39:e77f844d10d9	385	emg_control=false;
Vigilance88	38:c8ac615d0c8f	386	control_method=2;
Vigilance88	56:5ff9e5c1ed44	387	start_control(); wait(1);
Vigilance88	56:5ff9e5c1ed44	388	if(debug)
Vigilance88	56:5ff9e5c1ed44	389	{
Vigilance88	56:5ff9e5c1ed44	390	debug_timer.attach(&debugging,1);
Vigilance88	56:5ff9e5c1ed44	391	}
Vigilance88	35:7d9fca0b1545	392	controlButtons();
Vigilance88	35:7d9fca0b1545	393	break;
Vigilance88	39:e77f844d10d9	394	case 'e':
Vigilance88	39:e77f844d10d9	395	case 'E':
Vigilance88	56:5ff9e5c1ed44	396	pc.printf("=> You chose EMG DLS control \r\n\n"); wait(1);
Vigilance88	56:5ff9e5c1ed44	397	start_sampling(); wait(1);
Vigilance88	46:c8c5c455dd51	398	emg_control_time = 0;
Vigilance88	39:e77f844d10d9	399	emg_control=true;
Vigilance88	39:e77f844d10d9	400	control_method=2;
Vigilance88	56:5ff9e5c1ed44	401	start_control(); wait(1);
Vigilance88	56:5ff9e5c1ed44	402	if(debug)
Vigilance88	56:5ff9e5c1ed44	403	{
Vigilance88	56:5ff9e5c1ed44	404	debug_timer.attach(&debugging,1);
Vigilance88	56:5ff9e5c1ed44	405	}
Vigilance88	56:5ff9e5c1ed44	406	controlButtons();
Vigilance88	47:c52f9b4c90c4	407
Vigilance88	39:e77f844d10d9	408	break;
Vigilance88	28:743485bb51e4	409	case 'R':
Vigilance88	28:743485bb51e4	410	red=!red;
Vigilance88	28:743485bb51e4	411	pc.printf("=> Red LED triggered \n\r");
Vigilance88	28:743485bb51e4	412	break;
Vigilance88	28:743485bb51e4	413	case 'G':
Vigilance88	28:743485bb51e4	414	green=!green;
Vigilance88	28:743485bb51e4	415	pc.printf("=> Green LED triggered \n\r");
Vigilance88	28:743485bb51e4	416	break;
Vigilance88	28:743485bb51e4	417	case 'B':
Vigilance88	28:743485bb51e4	418	blue=!blue;
Vigilance88	28:743485bb51e4	419	pc.printf("=> Blue LED triggered \n\r");
Vigilance88	28:743485bb51e4	420	break;
Vigilance88	28:743485bb51e4	421	case 'q':
Vigilance88	28:743485bb51e4	422	case 'Q':
Vigilance88	28:743485bb51e4	423
Vigilance88	28:743485bb51e4	424	break;
Vigilance88	28:743485bb51e4	425	default:
Vigilance88	28:743485bb51e4	426	pc.printf("=> Invalid Input \n\r");
Vigilance88	28:743485bb51e4	427	break;
Vigilance88	28:743485bb51e4	428	} //end switch
Vigilance88	28:743485bb51e4	429	} // end if pc readable
Vigilance88	28:743485bb51e4	430
Vigilance88	28:743485bb51e4	431	} // end while loop
Vigilance88	28:743485bb51e4	432
Vigilance88	47:c52f9b4c90c4	433	//When end of while loop reached (user chose quit program).
Vigilance88	28:743485bb51e4	434
Vigilance88	28:743485bb51e4	435	pc.printf("\r\n------------------------------ \n\r");
Vigilance88	28:743485bb51e4	436	pc.printf("Program Offline \n\r");
Vigilance88	28:743485bb51e4	437	pc.printf("Reset to start\r\n");
Vigilance88	28:743485bb51e4	438	pc.printf("------------------------------ \n\r");
Vigilance88	28:743485bb51e4	439	}
Vigilance88	28:743485bb51e4	440	//end of main
Vigilance88	28:743485bb51e4	441
Vigilance88	28:743485bb51e4	442	/*--------------------------------------------------------------------------------------------------------------------
Vigilance88	28:743485bb51e4	443	---- FUNCTIONS -------------------------------------------------------------------------------------------------------
Vigilance88	28:743485bb51e4	444	--------------------------------------------------------------------------------------------------------------------*/
Vigilance88	28:743485bb51e4	445
Vigilance88	56:5ff9e5c1ed44	446	//Debug function: prints important variables to check if things are calculating correctly - find errors
Vigilance88	56:5ff9e5c1ed44	447	void debugging()
Vigilance88	56:5ff9e5c1ed44	448	{
Vigilance88	56:5ff9e5c1ed44	449	//Debugging values:
Vigilance88	56:5ff9e5c1ed44	450	pc.printf("\r\nRef pos: %f and %f \r\n",x,y);
Vigilance88	56:5ff9e5c1ed44	451	pc.printf("Cur pos: %f and %f \r\n",current_x,current_y);
Vigilance88	57:d6192801fd6d	452	//pc.printf("Pos Error: %f and %f \r\n",x_error,y_error);
Vigilance88	57:d6192801fd6d	453	//pc.printf("Cur angles: %f and %f \r\n",theta1,theta2);
Vigilance88	57:d6192801fd6d	454	//pc.printf("DLS1: %f and DLS2 %f and DLS3 %f and DLS4: %f \r\n",dls1,dls2,dls3,dls4);
Vigilance88	56:5ff9e5c1ed44	455	pc.printf("Error angles: %f and %f \r\n",q1_error,q2_error);
Vigilance88	56:5ff9e5c1ed44	456	pc.printf("PID output: %f and %f \r\n",u1,u2);
Vigilance88	56:5ff9e5c1ed44	457	pc.printf("----------------------------------------\r\n");
Vigilance88	56:5ff9e5c1ed44	458	pc.printf("Buffer1: %f \r\n",biceps_avg);
Vigilance88	56:5ff9e5c1ed44	459	pc.printf("Buffer2: %f \r\n",triceps_avg);
Vigilance88	56:5ff9e5c1ed44	460	pc.printf("Buffer3: %f \r\n",flexor_avg);
Vigilance88	56:5ff9e5c1ed44	461	pc.printf("Buffer4: %f \r\n",extens_avg);
Vigilance88	56:5ff9e5c1ed44	462	pc.printf("----------------------------------------\r\n");
Vigilance88	56:5ff9e5c1ed44	463	pc.printf("Theta3: %f \r\n",theta3);
Vigilance88	56:5ff9e5c1ed44	464	pc.printf("Servopos us: %f \r\n",servopos);
Vigilance88	56:5ff9e5c1ed44	465	pc.printf("----------------------------------------\r\n");
Vigilance88	56:5ff9e5c1ed44	466
Vigilance88	56:5ff9e5c1ed44	467
Vigilance88	56:5ff9e5c1ed44	468	}
Vigilance88	56:5ff9e5c1ed44	469
Vigilance88	56:5ff9e5c1ed44	470	//Calculates how much servo needs to move to keep mouse aligned
Vigilance88	56:5ff9e5c1ed44	471	void servo_control(){
Vigilance88	56:5ff9e5c1ed44	472	//Servo alignment
Vigilance88	56:5ff9e5c1ed44	473	//When shoulder or elbow angle increases from starting position --> servo needs to turn counterclockwise to keep mouse aligned.
Vigilance88	56:5ff9e5c1ed44	474	deltatheta1 = theta1 - theta1_lower;
Vigilance88	56:5ff9e5c1ed44	475	deltatheta2 = theta2 - theta2_lower;
Vigilance88	56:5ff9e5c1ed44	476	theta3 = deltatheta1 + deltatheta2;
Vigilance88	56:5ff9e5c1ed44	477	servopos = -(2100/PI)*theta3 + 2700;
Vigilance88	56:5ff9e5c1ed44	478	servoPwm.SetPosition(servopos);
Vigilance88	56:5ff9e5c1ed44	479
Vigilance88	56:5ff9e5c1ed44	480	//old:
Vigilance88	56:5ff9e5c1ed44	481	//theta3 = 1.5*PI - deltatheta1 - deltatheta2;
Vigilance88	56:5ff9e5c1ed44	482	//servopos = -(2100/PI)*theta3 + 2700;
Vigilance88	56:5ff9e5c1ed44	483	}
Vigilance88	56:5ff9e5c1ed44	484
Vigilance88	47:c52f9b4c90c4	485	//Use WASD keys to change reference position, x is a and d, y is w and s.
Vigilance88	29:948b0b14f6be	486	void controlButtons()
Vigilance88	28:743485bb51e4	487	{
Vigilance88	28:743485bb51e4	488	controlMenu();
Vigilance88	28:743485bb51e4	489	char c=0;
Vigilance88	28:743485bb51e4	490	while(c != 'Q' && c != 'q') {
Vigilance88	56:5ff9e5c1ed44	491
Vigilance88	27:d1814e271a95	492
Vigilance88	51:e4a0ce7ff4b8	493	if( pc.readable() ){
Vigilance88	27:d1814e271a95	494	c = pc.getc();
Vigilance88	27:d1814e271a95	495	switch (c)
Vigilance88	27:d1814e271a95	496	{
Vigilance88	38:c8ac615d0c8f	497	case 'd' :
Vigilance88	27:d1814e271a95	498	x = x + 0.01;
Vigilance88	32:76c4d7bb2022	499
Vigilance88	27:d1814e271a95	500	break;
Vigilance88	27:d1814e271a95	501
Vigilance88	38:c8ac615d0c8f	502	case 'a' :
Vigilance88	27:d1814e271a95	503	x-=0.01;
Vigilance88	32:76c4d7bb2022	504
Vigilance88	27:d1814e271a95	505	break;
Vigilance88	27:d1814e271a95	506
Vigilance88	38:c8ac615d0c8f	507	case 'w' :
Vigilance88	27:d1814e271a95	508	y+=0.01;
Vigilance88	32:76c4d7bb2022	509
Vigilance88	27:d1814e271a95	510	break;
Vigilance88	27:d1814e271a95	511
Vigilance88	27:d1814e271a95	512
Vigilance88	38:c8ac615d0c8f	513	case 's' :
Vigilance88	27:d1814e271a95	514	y-=0.01;
Vigilance88	32:76c4d7bb2022	515
Vigilance88	27:d1814e271a95	516	break;
Vigilance88	27:d1814e271a95	517
Vigilance88	56:5ff9e5c1ed44	518	case 'g' :
Vigilance88	56:5ff9e5c1ed44	519	debug=true;
Vigilance88	56:5ff9e5c1ed44	520	break;
Vigilance88	27:d1814e271a95	521	case 'q' :
Vigilance88	28:743485bb51e4	522	case 'Q' :
Vigilance88	28:743485bb51e4	523	pc.printf("=> Quitting control... \r\n"); wait(1);
Vigilance88	28:743485bb51e4	524	stop_sampling();
Vigilance88	28:743485bb51e4	525	stop_control();
Vigilance88	56:5ff9e5c1ed44	526	debug_timer.detach();
Vigilance88	28:743485bb51e4	527	pc.printf("Sampling and Control detached \n\r"); wait(1);
Vigilance88	28:743485bb51e4	528	pc.printf("Returning to Main Menu \r\n\n"); wait(1);
Vigilance88	28:743485bb51e4	529	mainMenu();
Vigilance88	47:c52f9b4c90c4	530
Vigilance88	27:d1814e271a95	531	break;
Vigilance88	27:d1814e271a95	532	}//end switch
Vigilance88	55:726fdab812a9	533	/* if(c!='q' && c!='Q'){
Vigilance88	55:726fdab812a9	534	pc.printf("Reference position: %f and %f \r\n",x,y);
Vigilance88	55:726fdab812a9	535	pc.printf("Current position: %f and %f \r\n",current_x,current_y);
Vigilance88	55:726fdab812a9	536	pc.printf("Pos Error: %f and %f \r\n",x_error,y_error);
Vigilance88	55:726fdab812a9	537	pc.printf("Current angles: %f and %f \r\n",theta1,theta2);
Vigilance88	55:726fdab812a9	538	pc.printf("DLS1: %f and DLS2 %f and DLS3 %f and DLS4: %f \r\n",dls1,dls2,dls3,dls4);
Vigilance88	55:726fdab812a9	539	pc.printf("Error in angles: %f and %f \r\n",q1_error,q2_error);
Vigilance88	55:726fdab812a9	540	pc.printf("PID output: %f and %f \r\n",u1,u2);
Vigilance88	55:726fdab812a9	541	pc.printf("----------------------------------------\r\n");
Vigilance88	55:726fdab812a9	542	pc.printf("Buffer 1: %f \r\n",biceps_avg);
Vigilance88	55:726fdab812a9	543	pc.printf("Buffer 2: %f \r\n",triceps_avg);
Vigilance88	55:726fdab812a9	544	pc.printf("Buffer 3: %f \r\n",flexor_avg);
Vigilance88	55:726fdab812a9	545	pc.printf("Buffer 4: %f \r\n",extens_avg);
Vigilance88	55:726fdab812a9	546	pc.printf("----------------------------------------\r\n");
Vigilance88	55:726fdab812a9	547	pc.printf("Theta 3: %f \r\n",theta3);
Vigilance88	55:726fdab812a9	548	pc.printf("Servoposition (us): %f \r\n",servopos);
Vigilance88	55:726fdab812a9	549	pc.printf("----------------------------------------\r\n");
Vigilance88	55:726fdab812a9	550	}*/
Vigilance88	28:743485bb51e4	551	}
Vigilance88	47:c52f9b4c90c4	552	//end if pc readable
Vigilance88	51:e4a0ce7ff4b8	553
Vigilance88	50:54f71544964c	554
Vigilance88	21:d6a46315d5f5	555	}
Vigilance88	21:d6a46315d5f5	556	//end of while loop
Vigilance88	30:a9fdd3202ca2	557	}
Vigilance88	18:44905b008f44	558
Vigilance88	21:d6a46315d5f5	559	//Sample and Filter
Vigilance88	21:d6a46315d5f5	560	void sample_filter(void)
Vigilance88	18:44905b008f44	561	{
Vigilance88	32:76c4d7bb2022	562	emg_biceps = emg1.read(); //Biceps
Vigilance88	32:76c4d7bb2022	563	emg_triceps = emg2.read(); //Triceps
Vigilance88	32:76c4d7bb2022	564	emg_flexor = emg3.read(); //Flexor
Vigilance88	32:76c4d7bb2022	565	emg_extens = emg4.read(); //Extensor
Vigilance88	21:d6a46315d5f5	566
Vigilance88	21:d6a46315d5f5	567	//Filter: high-pass -> notch -> rectify -> lowpass or moving average
Vigilance88	22:1ba637601dc3	568	// Can we use same biquadFilter (eg. highpass) for other muscles or does each muscle need its own biquad?
Vigilance88	25:49ccdc98639a	569	biceps_power = highpass.step(emg_biceps); triceps_power = highpass2.step(emg_triceps); flexor_power = highpass3.step(emg_flexor); extens_power = highpass4.step(emg_extens);
Vigilance88	25:49ccdc98639a	570	biceps_power = notch1.step(biceps_power); triceps_power = notch1_2.step(triceps_power); flexor_power = notch1_3.step(flexor_power); extens_power = notch1_4.step(extens_power);
Vigilance88	25:49ccdc98639a	571	biceps_power = notch2.step(biceps_power); triceps_power = notch2_2.step(triceps_power); flexor_power = notch2_3.step(flexor_power); extens_power = notch2_4.step(extens_power);
Vigilance88	21:d6a46315d5f5	572	biceps_power = abs(biceps_power); triceps_power = abs(triceps_power); flexor_power = abs(flexor_power); extens_power = abs(extens_power);
Vigilance88	25:49ccdc98639a	573	biceps_power = lowpass.step(biceps_power); triceps_power = lowpass2.step(triceps_power); flexor_power = lowpass3.step(flexor_power); extens_power = lowpass4.step(extens_power);
Vigilance88	34:d6ec7c634763	574
Vigilance88	52:8a8c53dc8547	575	//send filtered emg to scope
Vigilance88	55:726fdab812a9	576	/*scope.set(0,biceps_power);
Vigilance88	55:726fdab812a9	577	scope.set(1,triceps_power);
Vigilance88	55:726fdab812a9	578	scope.set(2,flexor_power);
Vigilance88	55:726fdab812a9	579	scope.set(3,extens_power);
Vigilance88	55:726fdab812a9	580	scope.send();
Vigilance88	55:726fdab812a9	581	*/
Vigilance88	55:726fdab812a9	582	//send normalized emg to scope
Vigilance88	55:726fdab812a9	583	//scope.set(0,biceps);
Vigilance88	55:726fdab812a9	584	//scope.set(1,triceps);
Vigilance88	55:726fdab812a9	585	//scope.set(2,flexor);
Vigilance88	55:726fdab812a9	586	//scope.set(3,extens);
Vigilance88	50:54f71544964c	587	//scope.send();
Vigilance88	44:145827f5b091	588
Vigilance88	18:44905b008f44	589	}
Vigilance88	18:44905b008f44	590
Vigilance88	47:c52f9b4c90c4	591	//Limit switch - if hit: set pulsecount of encoder to angle of lower mechanical limit
Vigilance88	51:e4a0ce7ff4b8	592	void shoulder()
Vigilance88	51:e4a0ce7ff4b8	593	{
Vigilance88	41:55face19e06b	594	pwm_motor1=0;
Vigilance88	32:76c4d7bb2022	595	done1 = true;
Vigilance88	32:76c4d7bb2022	596	pc.printf("Shoulder Limit hit - shutting down motor 1\r\n");
Vigilance88	36:4d4fc200171b	597	//mechanical angle limits -> pulses. If 40 degrees -> counts = floor( 40 * (4200/360) )
Vigilance88	41:55face19e06b	598	theta1_cal = floor(theta1_lower * (4200/(2*PI)));
Vigilance88	51:e4a0ce7ff4b8	599	Encoder1.setPulses(theta1_cal); //edited QEI library: added setPulses(int p)
Vigilance88	32:76c4d7bb2022	600	}
Vigilance88	32:76c4d7bb2022	601
Vigilance88	32:76c4d7bb2022	602	void elbow(){
Vigilance88	41:55face19e06b	603	pwm_motor2=0;
Vigilance88	32:76c4d7bb2022	604	done2 = true;
Vigilance88	32:76c4d7bb2022	605	pc.printf("Elbow Limit hit - shutting down motor 2\r\n");
Vigilance88	47:c52f9b4c90c4	606
Vigilance88	41:55face19e06b	607	//Mechanical limit 43 degrees -> 43*(4200/360) = 350
Vigilance88	41:55face19e06b	608	theta2_cal = floor(theta2_lower * (4200/(2*PI)));
Vigilance88	51:e4a0ce7ff4b8	609	Encoder2.setPulses(theta2_cal); //edited QEI library: added setPulses(int p)
Vigilance88	32:76c4d7bb2022	610	}
Vigilance88	47:c52f9b4c90c4	611
Vigilance88	47:c52f9b4c90c4	612	//Run motors slowly clockwise to mechanical limit. Attached to 100Hz ticker
Vigilance88	47:c52f9b4c90c4	613	void calibrate(void){
Vigilance88	47:c52f9b4c90c4	614	if(done1==false){ //if motor 1 limit has not been hit yet
Vigilance88	55:726fdab812a9	615	pwm_motor1=0.4; //move upper arm slowly cw
Vigilance88	47:c52f9b4c90c4	616	pc.printf("Motor 1 running %f \r\n");
Vigilance88	47:c52f9b4c90c4	617	}
Vigilance88	47:c52f9b4c90c4	618	if(done1==true && done2==false){ //if limit motor 1 has been hit
Vigilance88	47:c52f9b4c90c4	619	pwm_motor1=0; //stop motor1
Vigilance88	55:726fdab812a9	620	pwm_motor2=0.4; //start moving forearm slowly cw
Vigilance88	47:c52f9b4c90c4	621	pc.printf("Motor 2 running %f \r\n");
Vigilance88	47:c52f9b4c90c4	622	}
Vigilance88	47:c52f9b4c90c4	623	}
Vigilance88	32:76c4d7bb2022	624
Vigilance88	18:44905b008f44	625	//Send arm to mechanical limits, and set encoder to 0. Then send arm to starting position.
Vigilance88	19:0a3ee31dcdb4	626	void calibrate_arm(void)
Vigilance88	19:0a3ee31dcdb4	627	{
Vigilance88	28:743485bb51e4	628	pc.printf("Calibrate_arm() entered\r\n");
Vigilance88	47:c52f9b4c90c4	629
Vigilance88	47:c52f9b4c90c4	630	calibrating = true;
Vigilance88	32:76c4d7bb2022	631	done1 = false;
Vigilance88	32:76c4d7bb2022	632	done2 = false;
Vigilance88	32:76c4d7bb2022	633
Vigilance88	27:d1814e271a95	634	pc.printf("To start arm calibration, press any key =>");
Vigilance88	47:c52f9b4c90c4	635	pc.getc();
Vigilance88	27:d1814e271a95	636	pc.printf("\r\n Calibrating... \r\n");
Vigilance88	47:c52f9b4c90c4	637	red=0; blue=0; //Debug light is purple during arm calibration
Vigilance88	47:c52f9b4c90c4	638
Vigilance88	36:4d4fc200171b	639	dir_motor1=0; //cw
Vigilance88	36:4d4fc200171b	640	dir_motor2=1; //cw
Vigilance88	36:4d4fc200171b	641
Vigilance88	47:c52f9b4c90c4	642	control_timer.attach(&calibrate,CONTROL_RATE);
Vigilance88	26:fe3a5469dd6b	643
Vigilance88	26:fe3a5469dd6b	644	while(calibrating){
Vigilance88	47:c52f9b4c90c4	645	shoulder_limit.fall(&shoulder);
Vigilance88	47:c52f9b4c90c4	646	elbow_limit.fall(&elbow);
Vigilance88	47:c52f9b4c90c4	647	if(done1 && done2){
Vigilance88	47:c52f9b4c90c4	648	pwm_motor2=0;
Vigilance88	47:c52f9b4c90c4	649	control_timer.detach(); //Leave while loop when both limits are reached
Vigilance88	47:c52f9b4c90c4	650	red=1; blue=1; //Turn debug light off when calibration complete
Vigilance88	47:c52f9b4c90c4	651	//set reference position to mechanical limits
Vigilance88	47:c52f9b4c90c4	652	calibrating=false;
Vigilance88	27:d1814e271a95	653
Vigilance88	47:c52f9b4c90c4	654	x = l1 * cos(theta1_lower) + l2 * cos(theta1_lower + theta2_lower);
Vigilance88	47:c52f9b4c90c4	655	y = l1 * sin(theta1_lower) + l2 * sin(theta1_lower + theta2_lower);
Vigilance88	47:c52f9b4c90c4	656	//x = 0.2220;
Vigilance88	47:c52f9b4c90c4	657	//y = 0.4088;
Vigilance88	47:c52f9b4c90c4	658	}
Vigilance88	47:c52f9b4c90c4	659	}
Vigilance88	47:c52f9b4c90c4	660	pc.printf("Current pulsecount motor 1: %i, motor 2: %i \r\n",Encoder1.getPulses(),Encoder2.getPulses());
Vigilance88	47:c52f9b4c90c4	661	pc.printf("Current reference. X: %f, Y: %f \r\n",x,y);
Vigilance88	47:c52f9b4c90c4	662	wait(1);
Vigilance88	47:c52f9b4c90c4	663	pc.printf("\n\r-------------------------- \n\r");
Vigilance88	47:c52f9b4c90c4	664	pc.printf(" Arm Calibration Complete\r\n");
Vigilance88	47:c52f9b4c90c4	665	pc.printf("-------------------------- \n\r");
Vigilance88	47:c52f9b4c90c4	666
Vigilance88	19:0a3ee31dcdb4	667	}
Vigilance88	19:0a3ee31dcdb4	668
Vigilance88	21:d6a46315d5f5	669	//EMG Maximum Voluntary Contraction measurement
Vigilance88	25:49ccdc98639a	670	void emg_mvc()
Vigilance88	25:49ccdc98639a	671	{
Vigilance88	24:56db31267f10	672	if(muscle==1){
Vigilance88	24:56db31267f10	673
Vigilance88	24:56db31267f10	674	if(biceps_power>bicepsMVC){
Vigilance88	26:fe3a5469dd6b	675	//printf("+ ");
Vigilance88	26:fe3a5469dd6b	676	printf("%s+ %s",GREEN_,_GREEN);
Vigilance88	21:d6a46315d5f5	677	bicepsMVC=biceps_power;
Vigilance88	24:56db31267f10	678	}
Vigilance88	25:49ccdc98639a	679	else
Vigilance88	25:49ccdc98639a	680	printf("- ");
Vigilance88	24:56db31267f10	681	}
Vigilance88	24:56db31267f10	682
Vigilance88	24:56db31267f10	683	if(muscle==2){
Vigilance88	24:56db31267f10	684
Vigilance88	24:56db31267f10	685	if(triceps_power>tricepsMVC){
Vigilance88	26:fe3a5469dd6b	686	printf("%s+ %s",GREEN_,_GREEN);
Vigilance88	24:56db31267f10	687	tricepsMVC=triceps_power;
Vigilance88	24:56db31267f10	688	}
Vigilance88	26:fe3a5469dd6b	689	else
Vigilance88	26:fe3a5469dd6b	690	printf("- ");
Vigilance88	24:56db31267f10	691	}
Vigilance88	24:56db31267f10	692
Vigilance88	24:56db31267f10	693	if(muscle==3){
Vigilance88	24:56db31267f10	694
Vigilance88	24:56db31267f10	695	if(flexor_power>flexorMVC){
Vigilance88	26:fe3a5469dd6b	696	printf("%s+ %s",GREEN_,_GREEN);
Vigilance88	24:56db31267f10	697	flexorMVC=flexor_power;
Vigilance88	24:56db31267f10	698	}
Vigilance88	26:fe3a5469dd6b	699	else
Vigilance88	26:fe3a5469dd6b	700	printf("- ");
Vigilance88	24:56db31267f10	701	}
Vigilance88	24:56db31267f10	702
Vigilance88	24:56db31267f10	703	if(muscle==4){
Vigilance88	24:56db31267f10	704
Vigilance88	24:56db31267f10	705	if(extens_power>extensMVC){
Vigilance88	26:fe3a5469dd6b	706	printf("%s+ %s",GREEN_,_GREEN);
Vigilance88	24:56db31267f10	707	extensMVC=extens_power;
Vigilance88	24:56db31267f10	708	}
Vigilance88	26:fe3a5469dd6b	709	else
Vigilance88	26:fe3a5469dd6b	710	printf("- ");
Vigilance88	24:56db31267f10	711	}
Vigilance88	25:49ccdc98639a	712
Vigilance88	25:49ccdc98639a	713	//}
Vigilance88	25:49ccdc98639a	714	calibrate_time = calibrate_time + 0.002;
Vigilance88	36:4d4fc200171b	715
Vigilance88	25:49ccdc98639a	716	}
Vigilance88	25:49ccdc98639a	717
Vigilance88	35:7d9fca0b1545	718	void emg_min()
Vigilance88	48:a1f97eb8c020	719	{
Vigilance88	38:c8ac615d0c8f	720	if(biceps_power>bicepsmin){
Vigilance88	35:7d9fca0b1545	721	bicepsmin=biceps_power;
Vigilance88	35:7d9fca0b1545	722	}
Vigilance88	35:7d9fca0b1545	723
Vigilance88	38:c8ac615d0c8f	724	if(triceps_power>tricepsmin){
Vigilance88	35:7d9fca0b1545	725	tricepsmin=triceps_power;
Vigilance88	35:7d9fca0b1545	726	}
Vigilance88	35:7d9fca0b1545	727
Vigilance88	38:c8ac615d0c8f	728	if(flexor_power>flexormin){
Vigilance88	35:7d9fca0b1545	729	flexormin=flexor_power;
Vigilance88	35:7d9fca0b1545	730	}
Vigilance88	35:7d9fca0b1545	731
Vigilance88	38:c8ac615d0c8f	732	if(extens_power > extensmin){
Vigilance88	35:7d9fca0b1545	733	extensmin = extens_power;
Vigilance88	35:7d9fca0b1545	734	}
Vigilance88	35:7d9fca0b1545	735
Vigilance88	35:7d9fca0b1545	736	calibrate_time = calibrate_time + 0.002;
Vigilance88	35:7d9fca0b1545	737
Vigilance88	35:7d9fca0b1545	738	}
Vigilance88	35:7d9fca0b1545	739
Vigilance88	25:49ccdc98639a	740	//EMG calibration
Vigilance88	25:49ccdc98639a	741	void calibrate_emg()
Vigilance88	25:49ccdc98639a	742	{
Vigilance88	25:49ccdc98639a	743	Ticker timer;
Vigilance88	25:49ccdc98639a	744
Vigilance88	38:c8ac615d0c8f	745	pc.printf("Starting sampling, to allow hidscope to normalize\r\n");
Vigilance88	38:c8ac615d0c8f	746	start_sampling();
Vigilance88	25:49ccdc98639a	747	wait(1);
Vigilance88	48:a1f97eb8c020	748
Vigilance88	48:a1f97eb8c020	749	/***************** All muscles: minimum measurement ***********************/
Vigilance88	48:a1f97eb8c020	750	pc.printf("Start of minimum measurement, relax all muscles.\r\n");
Vigilance88	35:7d9fca0b1545	751	wait(1);
Vigilance88	35:7d9fca0b1545	752	pc.printf(" Press any key to begin... "); wait(1);
Vigilance88	35:7d9fca0b1545	753	char input;
Vigilance88	35:7d9fca0b1545	754	input=pc.getc();
Vigilance88	35:7d9fca0b1545	755	pc.printf(" \r\n Starting in 3... \r\n"); wait(1);
Vigilance88	35:7d9fca0b1545	756	pc.printf(" \r\n Starting in 2... \r\n"); wait(1);
Vigilance88	35:7d9fca0b1545	757	pc.printf(" \r\n Starting in 1... \r\n"); wait(1);
Vigilance88	35:7d9fca0b1545	758
Vigilance88	35:7d9fca0b1545	759	timer.attach(&emg_min,SAMPLE_RATE);
Vigilance88	56:5ff9e5c1ed44	760	wait(3);
Vigilance88	35:7d9fca0b1545	761	timer.detach();
Vigilance88	35:7d9fca0b1545	762	pc.printf("\r\n Measurement complete."); wait(1);
Vigilance88	35:7d9fca0b1545	763	pc.printf("\r\n Biceps min = %f \r\n",bicepsmin); wait(1);
Vigilance88	35:7d9fca0b1545	764	pc.printf("\r\n Triceps min = %f \r\n",tricepsmin); wait(1);
Vigilance88	35:7d9fca0b1545	765	pc.printf("\r\n Flexor min = %f \r\n",flexormin); wait(1);
Vigilance88	35:7d9fca0b1545	766	pc.printf("\r\n Extensor min = %f \r\n",extensmin); wait(1);
Vigilance88	48:a1f97eb8c020	767	/****************************** Done **************************************/
Vigilance88	35:7d9fca0b1545	768
Vigilance88	48:a1f97eb8c020	769	pc.printf("\r\n Now we will measure maximum amplitudes \r\n"); wait(1);
Vigilance88	25:49ccdc98639a	770	pc.printf("+ means current sample is higher than stored MVC\r\n");
Vigilance88	25:49ccdc98639a	771	pc.printf("- means current sample is lower than stored MVC\r\n");
Vigilance88	48:a1f97eb8c020	772	wait(1);
Vigilance88	48:a1f97eb8c020	773	calibrate_time=0;
Vigilance88	48:a1f97eb8c020	774
Vigilance88	48:a1f97eb8c020	775	/******************* 1st channel: MVC measurement *************************/
Vigilance88	28:743485bb51e4	776	pc.printf("\r\n----------------\r\n ");
Vigilance88	28:743485bb51e4	777	pc.printf(" Biceps is first.\r\n ");
Vigilance88	28:743485bb51e4	778	pc.printf("----------------\r\n ");
Vigilance88	28:743485bb51e4	779	wait(1);
Vigilance88	25:49ccdc98639a	780	pc.printf(" Press any key to begin... "); wait(1);
Vigilance88	25:49ccdc98639a	781	input=pc.getc();
Vigilance88	25:49ccdc98639a	782	pc.putc(input);
Vigilance88	25:49ccdc98639a	783	pc.printf(" \r\n Starting in 3... \r\n"); wait(1);
Vigilance88	25:49ccdc98639a	784	pc.printf(" \r\n Starting in 2... \r\n"); wait(1);
Vigilance88	25:49ccdc98639a	785	pc.printf(" \r\n Starting in 1... \r\n"); wait(1);
Vigilance88	25:49ccdc98639a	786
Vigilance88	25:49ccdc98639a	787	muscle=1;
Vigilance88	27:d1814e271a95	788	timer.attach(&emg_mvc,SAMPLE_RATE);
Vigilance88	56:5ff9e5c1ed44	789	wait(3);
Vigilance88	25:49ccdc98639a	790	timer.detach();
Vigilance88	26:fe3a5469dd6b	791
Vigilance88	26:fe3a5469dd6b	792	pc.printf("\r\n Measurement complete."); wait(1);
Vigilance88	26:fe3a5469dd6b	793	pc.printf("\r\n Biceps MVC = %f \r\n",bicepsMVC); wait(1);
Vigilance88	26:fe3a5469dd6b	794	pc.printf("Calibrate_emg() exited \r\n"); wait(1);
Vigilance88	26:fe3a5469dd6b	795	pc.printf("Measured time: %f seconds \r\n\n",calibrate_time);
Vigilance88	25:49ccdc98639a	796	calibrate_time=0;
Vigilance88	48:a1f97eb8c020	797	/****************************** Done **************************************/
Vigilance88	25:49ccdc98639a	798
Vigilance88	48:a1f97eb8c020	799	/******************* 2nd channel: MVC measurement *************************/
Vigilance88	26:fe3a5469dd6b	800	muscle=2;
Vigilance88	28:743485bb51e4	801	pc.printf("\r\n----------------\r\n ");
Vigilance88	28:743485bb51e4	802	pc.printf(" Triceps is next.\r\n ");
Vigilance88	28:743485bb51e4	803	pc.printf("----------------\r\n ");
Vigilance88	28:743485bb51e4	804	wait(1);
Vigilance88	28:743485bb51e4	805
Vigilance88	25:49ccdc98639a	806	pc.printf(" Press any key to begin... "); wait(1);
Vigilance88	25:49ccdc98639a	807	input=pc.getc();
Vigilance88	25:49ccdc98639a	808	pc.putc(input);
Vigilance88	25:49ccdc98639a	809	pc.printf(" \r\n Starting in 3... \r\n"); wait(1);
Vigilance88	25:49ccdc98639a	810	pc.printf(" \r\n Starting in 2... \r\n"); wait(1);
Vigilance88	25:49ccdc98639a	811	pc.printf(" \r\n Starting in 1... \r\n"); wait(1);
Vigilance88	48:a1f97eb8c020	812
Vigilance88	25:49ccdc98639a	813	timer.attach(&emg_mvc,0.002);
Vigilance88	56:5ff9e5c1ed44	814	wait(3);
Vigilance88	25:49ccdc98639a	815	timer.detach();
Vigilance88	25:49ccdc98639a	816	pc.printf("\r\n Triceps MVC = %f \r\n",tricepsMVC);
Vigilance88	25:49ccdc98639a	817
Vigilance88	25:49ccdc98639a	818	pc.printf("Calibrate_emg() exited \r\n");
Vigilance88	25:49ccdc98639a	819	pc.printf("Measured time: %f seconds \r\n",calibrate_time);
Vigilance88	25:49ccdc98639a	820	calibrate_time=0;
Vigilance88	48:a1f97eb8c020	821	/****************************** Done **************************************/
Vigilance88	48:a1f97eb8c020	822
Vigilance88	48:a1f97eb8c020	823	/******************* 3rd channel: MVC measurement *************************/
Vigilance88	26:fe3a5469dd6b	824	muscle=3;
Vigilance88	35:7d9fca0b1545	825	pc.printf("\r\n----------------\r\n ");
Vigilance88	35:7d9fca0b1545	826	pc.printf(" Flexor is next.\r\n ");
Vigilance88	35:7d9fca0b1545	827	pc.printf("----------------\r\n ");
Vigilance88	35:7d9fca0b1545	828	wait(1);
Vigilance88	35:7d9fca0b1545	829
Vigilance88	35:7d9fca0b1545	830	pc.printf(" Press any key to begin... "); wait(1);
Vigilance88	35:7d9fca0b1545	831	input=pc.getc();
Vigilance88	35:7d9fca0b1545	832	pc.putc(input);
Vigilance88	35:7d9fca0b1545	833	pc.printf(" \r\n Starting in 3... \r\n"); wait(1);
Vigilance88	35:7d9fca0b1545	834	pc.printf(" \r\n Starting in 2... \r\n"); wait(1);
Vigilance88	35:7d9fca0b1545	835	pc.printf(" \r\n Starting in 1... \r\n"); wait(1);
Vigilance88	48:a1f97eb8c020	836
Vigilance88	35:7d9fca0b1545	837	timer.attach(&emg_mvc,0.002);
Vigilance88	56:5ff9e5c1ed44	838	wait(3);
Vigilance88	35:7d9fca0b1545	839	timer.detach();
Vigilance88	35:7d9fca0b1545	840	pc.printf("\r\n Flexor MVC = %f \r\n",flexorMVC);
Vigilance88	35:7d9fca0b1545	841
Vigilance88	35:7d9fca0b1545	842	pc.printf("Calibrate_emg() exited \r\n");
Vigilance88	35:7d9fca0b1545	843	pc.printf("Measured time: %f seconds \r\n",calibrate_time);
Vigilance88	35:7d9fca0b1545	844	calibrate_time=0;
Vigilance88	48:a1f97eb8c020	845	/****************************** Done **************************************/
Vigilance88	35:7d9fca0b1545	846
Vigilance88	48:a1f97eb8c020	847	/******************* 4th channel: MVC measurement *************************/
Vigilance88	26:fe3a5469dd6b	848	muscle=4;
Vigilance88	35:7d9fca0b1545	849	pc.printf("\r\n----------------\r\n ");
Vigilance88	35:7d9fca0b1545	850	pc.printf(" Extensor is next.\r\n ");
Vigilance88	35:7d9fca0b1545	851	pc.printf("----------------\r\n ");
Vigilance88	35:7d9fca0b1545	852	wait(1);
Vigilance88	35:7d9fca0b1545	853
Vigilance88	35:7d9fca0b1545	854	pc.printf(" Press any key to begin... "); wait(1);
Vigilance88	35:7d9fca0b1545	855	input=pc.getc();
Vigilance88	35:7d9fca0b1545	856	pc.putc(input);
Vigilance88	35:7d9fca0b1545	857	pc.printf(" \r\n Starting in 3... \r\n"); wait(1);
Vigilance88	35:7d9fca0b1545	858	pc.printf(" \r\n Starting in 2... \r\n"); wait(1);
Vigilance88	35:7d9fca0b1545	859	pc.printf(" \r\n Starting in 1... \r\n"); wait(1);
Vigilance88	48:a1f97eb8c020	860
Vigilance88	35:7d9fca0b1545	861	timer.attach(&emg_mvc,0.002);
Vigilance88	56:5ff9e5c1ed44	862	wait(3);
Vigilance88	35:7d9fca0b1545	863	timer.detach();
Vigilance88	35:7d9fca0b1545	864	pc.printf("\r\n Extensor MVC = %f \r\n",extensMVC);
Vigilance88	25:49ccdc98639a	865
Vigilance88	35:7d9fca0b1545	866	pc.printf("Calibrate_emg() exited \r\n");
Vigilance88	35:7d9fca0b1545	867	pc.printf("Measured time: %f seconds \r\n",calibrate_time);
Vigilance88	35:7d9fca0b1545	868	calibrate_time=0;
Vigilance88	48:a1f97eb8c020	869	/****************************** Done **************************************/
Vigilance88	48:a1f97eb8c020	870
Vigilance88	48:a1f97eb8c020	871	//Stop sampling: detach ticker
Vigilance88	25:49ccdc98639a	872	stop_sampling();
Vigilance88	24:56db31267f10	873
Vigilance88	18:44905b008f44	874	}
Vigilance88	18:44905b008f44	875
Vigilance88	18:44905b008f44	876
Vigilance88	48:a1f97eb8c020	877	//PID motor 1 - Input error and Kp, Kd, Ki, output control signal
Vigilance88	20:0ede3818e08e	878	double pid(double error, double kp, double ki, double kd,double &e_int, double &e_prev)
vsluiter	2:e314bb3b2d99	879	{
Vigilance88	20:0ede3818e08e	880	// Derivative
Vigilance88	24:56db31267f10	881	double e_der = (error-e_prev)/ CONTROL_RATE;
Vigilance88	56:5ff9e5c1ed44	882	e_der = derfilter1.step(e_der); //derfilter1 - seperate 60hz low-pass biquad for this PID
Vigilance88	21:d6a46315d5f5	883	e_prev = error;
Vigilance88	20:0ede3818e08e	884	// Integral
Vigilance88	24:56db31267f10	885	e_int = e_int + CONTROL_RATE * error;
Vigilance88	20:0ede3818e08e	886	// PID
Vigilance88	21:d6a46315d5f5	887	return kperror + kie_int + kd * e_der;
Vigilance88	20:0ede3818e08e	888
Vigilance88	18:44905b008f44	889	}
Vigilance88	18:44905b008f44	890
Vigilance88	48:a1f97eb8c020	891	//PID for motor 2 - needed because of biquadfilter memory variables?
Vigilance88	46:c8c5c455dd51	892	double pid2(double error, double kp, double ki, double kd,double &e_int, double &e_prev)
Vigilance88	46:c8c5c455dd51	893	{
Vigilance88	46:c8c5c455dd51	894	// Derivative
Vigilance88	46:c8c5c455dd51	895	double e_der = (error-e_prev)/ CONTROL_RATE;
Vigilance88	56:5ff9e5c1ed44	896	e_der = derfilter2.step(e_der); //derfilter2 - seperate 60hz low-pass biquad for this PID
Vigilance88	46:c8c5c455dd51	897	e_prev = error;
Vigilance88	46:c8c5c455dd51	898	// Integral
Vigilance88	46:c8c5c455dd51	899	e_int = e_int + CONTROL_RATE * error;
Vigilance88	46:c8c5c455dd51	900	// PID
Vigilance88	46:c8c5c455dd51	901	return kperror + kie_int + kd * e_der;
Vigilance88	46:c8c5c455dd51	902
Vigilance88	46:c8c5c455dd51	903	}
Vigilance88	46:c8c5c455dd51	904
Vigilance88	46:c8c5c455dd51	905
Vigilance88	20:0ede3818e08e	906	//Analyze filtered EMG, calculate reference position from EMG, compare reference position with current position,convert to angles, send error through pid(), send PWM and DIR to motors
Vigilance88	18:44905b008f44	907	void control()
Vigilance88	50:54f71544964c	908	{
Vigilance88	48:a1f97eb8c020	909
Vigilance88	48:a1f97eb8c020	910	/******************* START OF EMG REFERENCE CALCULATION *************************/
Vigilance88	46:c8c5c455dd51	911	if(emg_control==true){
Vigilance88	50:54f71544964c	912	//TODO some idle time with static reference before EMG kicks in. or go to reference in the first 5 seconds.
Vigilance88	46:c8c5c455dd51	913	emg_control_time += CONTROL_RATE;
Vigilance88	46:c8c5c455dd51	914	//if(emg_control_time < 5){
Vigilance88	48:a1f97eb8c020	915	// x=BLA; y=BLA; starting position maybe
Vigilance88	46:c8c5c455dd51	916	//}
Vigilance88	48:a1f97eb8c020	917
Vigilance88	30:a9fdd3202ca2	918	//normalize emg to value between 0-1
Vigilance88	38:c8ac615d0c8f	919	biceps = (biceps_power - bicepsmin) / (bicepsMVC - bicepsmin);
Vigilance88	38:c8ac615d0c8f	920	triceps = (triceps_power - tricepsmin) / (tricepsMVC - tricepsmin);
Vigilance88	38:c8ac615d0c8f	921	flexor = (flexor_power - flexormin) / (flexorMVC - flexormin);
Vigilance88	38:c8ac615d0c8f	922	extens = (extens_power - extensmin) / (extensMVC - extensmin);
Vigilance88	39:e77f844d10d9	923	//make sure values stay between 0-1 over time
Vigilance88	39:e77f844d10d9	924	keep_in_range(&biceps,0,1);
Vigilance88	39:e77f844d10d9	925	keep_in_range(&triceps,0,1);
Vigilance88	39:e77f844d10d9	926	keep_in_range(&flexor,0,1);
Vigilance88	39:e77f844d10d9	927	keep_in_range(&extens,0,1);
Vigilance88	39:e77f844d10d9	928
Vigilance88	38:c8ac615d0c8f	929
Vigilance88	52:8a8c53dc8547	930	//threshold detection! buffer or two thresholds? If average of 100 samples > threshold, then muscle considered on.
Vigilance88	44:145827f5b091	931	movavg1[i]=biceps; //fill array with 100 normalized samples
Vigilance88	44:145827f5b091	932	movavg2[i]=triceps;
Vigilance88	44:145827f5b091	933	movavg3[i]=flexor;
Vigilance88	44:145827f5b091	934	movavg4[i]=extens;
Vigilance88	44:145827f5b091	935	i++;
Vigilance88	44:145827f5b091	936	if(i==window){ //if array full,set i to 0
Vigilance88	44:145827f5b091	937	i=0;
Vigilance88	44:145827f5b091	938	}
Vigilance88	44:145827f5b091	939
Vigilance88	50:54f71544964c	940
Vigilance88	50:54f71544964c	941	//Sum all values in the array. The sum needs to be overwritten or it will continue to sum the next 100 samples on top it
Vigilance88	50:54f71544964c	942	//and will grow out of control.
Vigilance88	50:54f71544964c	943	//So the variable name for the sum in the for loop is not really correct since the average is calculated after the loop.
Vigilance88	50:54f71544964c	944	for(int j = 0; j < window; j++){
Vigilance88	50:54f71544964c	945	biceps_avg += movavg1[j];
Vigilance88	50:54f71544964c	946	triceps_avg += movavg2[j];
Vigilance88	50:54f71544964c	947	flexor_avg += movavg3[j];
Vigilance88	50:54f71544964c	948	extens_avg += movavg4[j];
Vigilance88	44:145827f5b091	949	}
Vigilance88	50:54f71544964c	950	biceps_avg = biceps_avg/window; //divide sum by number of samples -> average
Vigilance88	50:54f71544964c	951	triceps_avg = triceps_avg/window;
Vigilance88	50:54f71544964c	952	flexor_avg = flexor_avg/window;
Vigilance88	50:54f71544964c	953	extens_avg = extens_avg/window;
Vigilance88	46:c8c5c455dd51	954
Vigilance88	50:54f71544964c	955
Vigilance88	48:a1f97eb8c020	956
Vigilance88	48:a1f97eb8c020	957	//Compare muscle amplitudes and determine their influence on x and y reference position.
Vigilance88	53:bf0d97487e84	958	if (biceps_avg>triceps_avg && biceps_avg > 0.2){
Vigilance88	48:a1f97eb8c020	959	xdir = 0;
Vigilance88	53:bf0d97487e84	960	xpower = biceps_avg;}
Vigilance88	53:bf0d97487e84	961	else if (triceps_avg>biceps_avg && triceps_avg>0.2){
Vigilance88	30:a9fdd3202ca2	962	xdir = 1;
Vigilance88	53:bf0d97487e84	963	xpower = triceps_avg;}
Vigilance88	30:a9fdd3202ca2	964	else
Vigilance88	30:a9fdd3202ca2	965	xpower=0;
Vigilance88	30:a9fdd3202ca2	966
Vigilance88	53:bf0d97487e84	967	if (flexor_avg>extens_avg && flexor_avg > 0.2){
Vigilance88	30:a9fdd3202ca2	968	ydir = 0;
Vigilance88	53:bf0d97487e84	969	ypower = flexor_avg;
Vigilance88	30:a9fdd3202ca2	970	}
Vigilance88	53:bf0d97487e84	971	else if (extens_avg>flexor_avg && extens_avg > 0.2){
Vigilance88	30:a9fdd3202ca2	972	ydir = 1;
Vigilance88	53:bf0d97487e84	973	ypower = extens_avg;
Vigilance88	30:a9fdd3202ca2	974	}
Vigilance88	30:a9fdd3202ca2	975	else
Vigilance88	30:a9fdd3202ca2	976	ypower = 0;
Vigilance88	30:a9fdd3202ca2	977
Vigilance88	38:c8ac615d0c8f	978	//power: the longer a signal is active, the further the reference goes. So integrate to determine reference position
Vigilance88	55:726fdab812a9	979	dx = xpower * CONTROL_RATE * 0.15; //last value is a factor to control how fast the reference (so also end effector) changes
Vigilance88	55:726fdab812a9	980	dy = ypower * CONTROL_RATE * 0.15;
Vigilance88	18:44905b008f44	981
Vigilance88	48:a1f97eb8c020	982	//Direction! Sum dx and dy but make sure xdir and ydir are considered.
Vigilance88	48:a1f97eb8c020	983	if (xdir>0) //if x direction of sample is positive, add it to reference position
Vigilance88	48:a1f97eb8c020	984	x += dx;
Vigilance88	48:a1f97eb8c020	985	else //if x direction of sample is negative, substract it from reference position
Vigilance88	30:a9fdd3202ca2	986	x += -dx;
Vigilance88	30:a9fdd3202ca2	987
Vigilance88	48:a1f97eb8c020	988	if (ydir>0) //if y direction of sample is positive, add it to reference position
Vigilance88	30:a9fdd3202ca2	989	y += dy;
Vigilance88	30:a9fdd3202ca2	990	else
Vigilance88	48:a1f97eb8c020	991	y += -dy; //if y direction of sample is negative, substract it from reference position
Vigilance88	48:a1f97eb8c020	992
Vigilance88	48:a1f97eb8c020	993	//now we have x and y -> reference position.
Vigilance88	30:a9fdd3202ca2	994
Vigilance88	39:e77f844d10d9	995	}//end emg_control if
Vigilance88	48:a1f97eb8c020	996	/****************************** END OF EMG REFERENCE CALCULATION **************************************/
Vigilance88	48:a1f97eb8c020	997
Vigilance88	30:a9fdd3202ca2	998
Vigilance88	27:d1814e271a95	999	//Current position - Encoder counts -> current angle -> Forward Kinematics
Vigilance88	49:6515c045cfd6	1000	rpc=(2*PI)/ENCODER_CPR; //radians per count (resolution) - 2pi divided by 4200
Vigilance88	48:a1f97eb8c020	1001	theta1 = Encoder1.getPulses() * rpc; //multiply resolution with number of counts to get current angles
Vigilance88	27:d1814e271a95	1002	theta2 = Encoder2.getPulses() * rpc;
Vigilance88	48:a1f97eb8c020	1003	current_x = l1 * cos(theta1) + l2 * cos(theta1 + theta2); //Forward kinematics for current position
Vigilance88	27:d1814e271a95	1004	current_y = l1 * sin(theta1) + l2 * sin(theta1 + theta2);
Vigilance88	27:d1814e271a95	1005
Vigilance88	27:d1814e271a95	1006
Vigilance88	48:a1f97eb8c020	1007	//calculate error (refpos-currentpos)
Vigilance88	27:d1814e271a95	1008	x_error = x - current_x;
Vigilance88	27:d1814e271a95	1009	y_error = y - current_y;
Vigilance88	27:d1814e271a95	1010
Vigilance88	48:a1f97eb8c020	1011	/****************************** START OF TRIG INVERSE KINEMATICS **************************************/
Vigilance88	38:c8ac615d0c8f	1012	if (control_method==1){
Vigilance88	27:d1814e271a95	1013	//inverse kinematics (refpos to refangle)
Vigilance88	18:44905b008f44	1014
Vigilance88	27:d1814e271a95	1015	costheta2 = (pow(x,2) + pow(y,2) - pow(l1,2) - pow(l2,2)) / (2l1l2) ;
Vigilance88	50:54f71544964c	1016	//absolute in sqrt to avoid imaginary numbers -> bigger steady state error when reference out of workspace
Vigilance88	50:54f71544964c	1017	sintheta2 = sqrt( abs( 1 - pow(costheta2,2) ) );
Vigilance88	27:d1814e271a95	1018
Vigilance88	48:a1f97eb8c020	1019	//Reference angle 2
Vigilance88	56:5ff9e5c1ed44	1020	reftheta2 = atan2(sintheta2,costheta2);
Vigilance88	27:d1814e271a95	1021
Vigilance88	32:76c4d7bb2022	1022	double k1 = l1 + l2*costheta2;
Vigilance88	32:76c4d7bb2022	1023	double k2 = l2*sintheta2;
Vigilance88	32:76c4d7bb2022	1024
Vigilance88	48:a1f97eb8c020	1025	//Reference angle 1
Vigilance88	56:5ff9e5c1ed44	1026	reftheta1 = atan2(y, x) - atan2(k2, k1);
Vigilance88	32:76c4d7bb2022	1027
Vigilance88	32:76c4d7bb2022	1028	/* alternative:
Vigilance88	27:d1814e271a95	1029	costheta1 = ( x * (l1 + l2 * costheta2) + y * l2 * sintheta2 ) / ( pow(x,2) + pow(y,2) );
Vigilance88	30:a9fdd3202ca2	1030	sintheta1 = sqrt( abs( 1 - pow(costheta1,2) ) );
Vigilance88	27:d1814e271a95	1031
Vigilance88	56:5ff9e5c1ed44	1032	reftheta1 = atan2(sintheta1,costheta1);
Vigilance88	32:76c4d7bb2022	1033	*/
Vigilance88	27:d1814e271a95	1034
Vigilance88	27:d1814e271a95	1035	//Angle error
Vigilance88	56:5ff9e5c1ed44	1036	m1_error = reftheta1-theta1;
Vigilance88	56:5ff9e5c1ed44	1037	m2_error = reftheta2-theta2;
Vigilance88	39:e77f844d10d9	1038	}// end control method 1
Vigilance88	48:a1f97eb8c020	1039	/****************************** END OF TRIG INVERSE KINEMATICS **************************************/
Vigilance88	27:d1814e271a95	1040
Vigilance88	48:a1f97eb8c020	1041
Vigilance88	48:a1f97eb8c020	1042	/****************************** START OF DLS INVERSE KINEMATICS **************************************/
Vigilance88	38:c8ac615d0c8f	1043	if(control_method==2){
Vigilance88	37:4d7b7ced20ef	1044	//inverse kinematics (error in position to error in angles)
Vigilance88	57:d6192801fd6d	1045	powlambda2 = pow(lambda,2);
Vigilance88	57:d6192801fd6d	1046	powlambda4 = pow(lambda,4);
Vigilance88	57:d6192801fd6d	1047	dls1= -(l2powlambda2sin(theta1 + theta2) + l1powlambda2sin(theta1) + l1pow(l2,2)pow(cos(theta1 + theta2),2)sin(theta1) - l1pow(l2,2)cos(theta1 + theta2)sin(theta1 + theta2)cos(theta1))/(powlambda4 + 2pow(l2,2)powlambda2pow(cos(theta1 + theta2),2) + 2pow(l2,2)powlambda2pow(sin(theta1 + theta2),2) + pow(l1,2)powlambda2pow(cos(theta1),2) + pow(l1,2)powlambda2pow(sin(theta1),2) + pow(l1,2)pow(l2,2)pow(cos(theta1 + theta2),2)pow(sin(theta1),2) + pow(l1,2)pow(l2,2)pow(sin(theta1 + theta2),2)pow(cos(theta1),2) + 2l1l2powlambda2cos(theta1 + theta2)cos(theta1) + 2l1l2powlambda2sin(theta1 + theta2)sin(theta1) - 2pow(l1,2)pow(l2,2)cos(theta1 + theta2)sin(theta1 + theta2)cos(theta1)*sin(theta1));
Vigilance88	57:d6192801fd6d	1048	dls2= (l2powlambda2cos(theta1 + theta2) + l1powlambda2cos(theta1) + l1pow(l2,2)pow(sin(theta1 + theta2),2)cos(theta1) - l1pow(l2,2)cos(theta1 + theta2)sin(theta1 + theta2)sin(theta1))/(powlambda4 + 2pow(l2,2)powlambda2pow(cos(theta1 + theta2),2) + 2pow(l2,2)powlambda2pow(sin(theta1 + theta2),2) + pow(l1,2)powlambda2pow(cos(theta1),2) + pow(l1,2)powlambda2pow(sin(theta1),2) + pow(l1,2)pow(l2,2)pow(cos(theta1 + theta2),2)pow(sin(theta1),2) + pow(l1,2)pow(l2,2)pow(sin(theta1 + theta2),2)pow(cos(theta1),2) + 2l1l2powlambda2cos(theta1 + theta2)cos(theta1) + 2l1l2powlambda2sin(theta1 + theta2)sin(theta1) - 2pow(l1,2)pow(l2,2)cos(theta1 + theta2)sin(theta1 + theta2)cos(theta1)*sin(theta1));
Vigilance88	57:d6192801fd6d	1049	dls3= -(l2powlambda2sin(theta1 + theta2) - l1pow(l2,2)pow(cos(theta1 + theta2),2)sin(theta1) + pow(l1,2)l2sin(theta1 + theta2)pow(cos(theta1),2) - pow(l1,2)l2cos(theta1 + theta2)cos(theta1)sin(theta1) + l1pow(l2,2)cos(theta1 + theta2)sin(theta1 + theta2)cos(theta1))/(powlambda4 + 2pow(l2,2)powlambda2pow(cos(theta1 + theta2),2) + 2pow(l2,2)powlambda2pow(sin(theta1 + theta2),2) + pow(l1,2)powlambda2pow(cos(theta1),2) + pow(l1,2)powlambda2pow(sin(theta1),2) + pow(l1,2)pow(l2,2)pow(cos(theta1 + theta2),2)pow(sin(theta1),2) + pow(l1,2)pow(l2,2)pow(sin(theta1 + theta2),2)pow(cos(theta1),2) + 2l1l2powlambda2cos(theta1 + theta2)cos(theta1) + 2l1l2powlambda2sin(theta1 + theta2)sin(theta1) - 2pow(l1,2)pow(l2,2)cos(theta1 + theta2)sin(theta1 + theta2)cos(theta1)sin(theta1));
Vigilance88	57:d6192801fd6d	1050	dls4= (l2powlambda2cos(theta1 + theta2) - l1pow(l2,2)pow(sin(theta1 + theta2),2)cos(theta1) + pow(l1,2)l2cos(theta1 + theta2)pow(sin(theta1),2) - pow(l1,2)l2sin(theta1 + theta2)cos(theta1)sin(theta1) + l1pow(l2,2)cos(theta1 + theta2)sin(theta1 + theta2)sin(theta1))/(powlambda4 + 2pow(l2,2)powlambda2pow(cos(theta1 + theta2),2) + 2pow(l2,2)powlambda2pow(sin(theta1 + theta2),2) + pow(l1,2)powlambda2pow(cos(theta1),2) + pow(l1,2)powlambda2pow(sin(theta1),2) + pow(l1,2)pow(l2,2)pow(cos(theta1 + theta2),2)pow(sin(theta1),2) + pow(l1,2)pow(l2,2)pow(sin(theta1 + theta2),2)pow(cos(theta1),2) + 2l1l2powlambda2cos(theta1 + theta2)cos(theta1) + 2l1l2powlambda2sin(theta1 + theta2)sin(theta1) - 2pow(l1,2)pow(l2,2)cos(theta1 + theta2)sin(theta1 + theta2)cos(theta1)sin(theta1));
Vigilance88	37:4d7b7ced20ef	1051
Vigilance88	37:4d7b7ced20ef	1052	q1_error = dls1 * x_error + dls2 * y_error;
Vigilance88	37:4d7b7ced20ef	1053	q2_error = dls3 * x_error + dls4 * y_error;
Vigilance88	37:4d7b7ced20ef	1054
Vigilance88	37:4d7b7ced20ef	1055	//Angle error
Vigilance88	37:4d7b7ced20ef	1056	m1_error = q1_error;
Vigilance88	37:4d7b7ced20ef	1057	m2_error = q2_error;
Vigilance88	39:e77f844d10d9	1058	}//end control method 2
Vigilance88	48:a1f97eb8c020	1059	/****************************** END OF DLS INVERSE KINEMATICS **************************************/
Vigilance88	39:e77f844d10d9	1060
Vigilance88	48:a1f97eb8c020	1061
Vigilance88	48:a1f97eb8c020	1062	/* Set limits to the error!
Vigilance88	48:a1f97eb8c020	1063	motor1: lower limit 40 degrees, upper limit 135
Vigilance88	48:a1f97eb8c020	1064	motor2: lower 43 degrees, upper limit 138 degrees. */
Vigilance88	41:55face19e06b	1065
Vigilance88	48:a1f97eb8c020	1066	//lower limits: Negative error not allowed to go further. NEEDS MORE TESTING
Vigilance88	41:55face19e06b	1067	if (theta1 < theta1_lower){
Vigilance88	41:55face19e06b	1068	if (m1_error > 0)
Vigilance88	41:55face19e06b	1069	m1_error = m1_error;
Vigilance88	39:e77f844d10d9	1070	else
Vigilance88	41:55face19e06b	1071	m1_error = 0; }
Vigilance88	41:55face19e06b	1072	if (theta2 < theta2_lower){
Vigilance88	41:55face19e06b	1073	if (m2_error > 0)
Vigilance88	41:55face19e06b	1074	m2_error = m2_error;
Vigilance88	41:55face19e06b	1075	else
Vigilance88	41:55face19e06b	1076	m2_error = 0;
Vigilance88	41:55face19e06b	1077	}
Vigilance88	39:e77f844d10d9	1078	//upper limit: Positive error not allowed to go further
Vigilance88	41:55face19e06b	1079	if (theta1 > theta1_upper){
Vigilance88	41:55face19e06b	1080	if (m1_error < 0 )
Vigilance88	41:55face19e06b	1081	m1_error = m1_error;
Vigilance88	39:e77f844d10d9	1082	else
Vigilance88	41:55face19e06b	1083	m1_error = 0;
Vigilance88	41:55face19e06b	1084	}
Vigilance88	41:55face19e06b	1085	if (theta2 > theta2_upper){
Vigilance88	41:55face19e06b	1086	if (m2_error < 0 )
Vigilance88	41:55face19e06b	1087	m2_error = m2_error;
Vigilance88	41:55face19e06b	1088	else
Vigilance88	41:55face19e06b	1089	m2_error = 0;
Vigilance88	41:55face19e06b	1090	}
Vigilance88	39:e77f844d10d9	1091
Vigilance88	18:44905b008f44	1092	//PID controller
Vigilance88	24:56db31267f10	1093	u1=pid(m1_error,m1_kp,m1_ki,m1_kd,m1_e_int,m1_e_prev); //motor 1
Vigilance88	46:c8c5c455dd51	1094	u2=pid2(m2_error,m2_kp,m2_ki,m2_kd,m2_e_int,m2_e_prev); //motor 2
Vigilance88	21:d6a46315d5f5	1095
Vigilance88	48:a1f97eb8c020	1096	//keep u between limits, sign = direction, PWM = 0-1
Vigilance88	55:726fdab812a9	1097	keep_in_range(&u1,-0.7,0.7);
Vigilance88	55:726fdab812a9	1098	keep_in_range(&u2,-0.7,0.7);
Vigilance88	21:d6a46315d5f5	1099
Vigilance88	21:d6a46315d5f5	1100	//send PWM and DIR to motor 1
Vigilance88	21:d6a46315d5f5	1101	dir_motor1 = u1>0 ? 1 : 0; //conditional statement dir_motor1 = [condition] ? [if met 1] : [else 0]], same as if else below.
Vigilance88	21:d6a46315d5f5	1102	pwm_motor1.write(abs(u1));
Vigilance88	21:d6a46315d5f5	1103
Vigilance88	21:d6a46315d5f5	1104	//send PWM and DIR to motor 2
Vigilance88	27:d1814e271a95	1105	dir_motor2 = u2>0 ? 0 : 1; //conditional statement, same as if else below
Vigilance88	25:49ccdc98639a	1106	pwm_motor2.write(abs(u2));
Vigilance88	21:d6a46315d5f5	1107
Vigilance88	53:bf0d97487e84	1108
Vigilance88	55:726fdab812a9	1109
Vigilance88	21:d6a46315d5f5	1110	/*if(u1 > 0)
Vigilance88	21:d6a46315d5f5	1111	{
Vigilance88	21:d6a46315d5f5	1112	dir_motor1 = 0;
Vigilance88	21:d6a46315d5f5	1113	else{
Vigilance88	21:d6a46315d5f5	1114	dir_motor1 = 1;
Vigilance88	21:d6a46315d5f5	1115	}
Vigilance88	21:d6a46315d5f5	1116	}
Vigilance88	21:d6a46315d5f5	1117	pwm_motor1.write(abs(u1));
Vigilance88	21:d6a46315d5f5	1118
Vigilance88	21:d6a46315d5f5	1119
Vigilance88	21:d6a46315d5f5	1120	if(u2 > 0)
Vigilance88	21:d6a46315d5f5	1121	{
Vigilance88	21:d6a46315d5f5	1122	dir_motor1 = 1;
Vigilance88	21:d6a46315d5f5	1123	else{
Vigilance88	21:d6a46315d5f5	1124	dir_motor1 = 0;
Vigilance88	21:d6a46315d5f5	1125	}
Vigilance88	21:d6a46315d5f5	1126	}
Vigilance88	21:d6a46315d5f5	1127	pwm_motor1.write(abs(u2));*/
Vigilance88	21:d6a46315d5f5	1128
Vigilance88	18:44905b008f44	1129	}
Vigilance88	18:44905b008f44	1130
Vigilance88	38:c8ac615d0c8f	1131
Vigilance88	26:fe3a5469dd6b	1132	void mainMenu()
Vigilance88	26:fe3a5469dd6b	1133	{
Vigilance88	38:c8ac615d0c8f	1134	//Title Box
Vigilance88	26:fe3a5469dd6b	1135	pc.putc(201);
Vigilance88	26:fe3a5469dd6b	1136	for(int j=0;j<33;j++){
Vigilance88	26:fe3a5469dd6b	1137	pc.putc(205);
Vigilance88	26:fe3a5469dd6b	1138	}
Vigilance88	26:fe3a5469dd6b	1139	pc.putc(187);
Vigilance88	26:fe3a5469dd6b	1140	pc.printf("\n\r");
Vigilance88	28:743485bb51e4	1141	pc.putc(186); pc.printf(" Main Menu "); pc.putc(186);
Vigilance88	26:fe3a5469dd6b	1142	pc.printf("\n\r");
Vigilance88	26:fe3a5469dd6b	1143	pc.putc(200);
Vigilance88	26:fe3a5469dd6b	1144	for(int k=0;k<33;k++){
Vigilance88	26:fe3a5469dd6b	1145	pc.putc(205);
Vigilance88	26:fe3a5469dd6b	1146	}
Vigilance88	26:fe3a5469dd6b	1147	pc.putc(188);
Vigilance88	26:fe3a5469dd6b	1148
Vigilance88	26:fe3a5469dd6b	1149	pc.printf("\n\r");
Vigilance88	26:fe3a5469dd6b	1150	//endbox
Vigilance88	48:a1f97eb8c020	1151
Vigilance88	28:743485bb51e4	1152	wait(1);
Vigilance88	28:743485bb51e4	1153	pc.printf("[C]alibration\r\n"); wait(0.2);
Vigilance88	40:d62f96ed44c0	1154	pc.printf("[T]RIG Control with WASD\r\n"); wait(0.2);
Vigilance88	40:d62f96ed44c0	1155	pc.printf("[D]LS Control with WASD\r\n"); wait(0.2);
Vigilance88	40:d62f96ed44c0	1156	pc.printf("[E]MG Control - DLS \r\n"); wait(0.2);
Vigilance88	28:743485bb51e4	1157	pc.printf("[Q]uit Robot Program\r\n"); wait(0.2);
Vigilance88	28:743485bb51e4	1158	pc.printf("Please make a choice => \r\n");
Vigilance88	26:fe3a5469dd6b	1159	}
Vigilance88	24:56db31267f10	1160
Vigilance88	24:56db31267f10	1161	//Start sampling
Vigilance88	24:56db31267f10	1162	void start_sampling(void)
Vigilance88	24:56db31267f10	1163	{
Vigilance88	24:56db31267f10	1164	sample_timer.attach(&sample_filter,SAMPLE_RATE); //500 Hz EMG
Vigilance88	26:fe3a5469dd6b	1165
Vigilance88	26:fe3a5469dd6b	1166	//Debug LED will be green when sampling is active
Vigilance88	26:fe3a5469dd6b	1167	green=0;
Vigilance88	25:49ccdc98639a	1168	pc.printf("\|\|- started sampling -\|\| \r\n");
Vigilance88	24:56db31267f10	1169	}
Vigilance88	24:56db31267f10	1170
Vigilance88	24:56db31267f10	1171	//stop sampling
Vigilance88	24:56db31267f10	1172	void stop_sampling(void)
Vigilance88	24:56db31267f10	1173	{
Vigilance88	24:56db31267f10	1174	sample_timer.detach();
Vigilance88	26:fe3a5469dd6b	1175
Vigilance88	26:fe3a5469dd6b	1176	//Debug LED will be turned off when sampling stops
Vigilance88	26:fe3a5469dd6b	1177	green=1;
Vigilance88	25:49ccdc98639a	1178	pc.printf("\|\|- stopped sampling -\|\| \r\n");
Vigilance88	24:56db31267f10	1179	}
Vigilance88	24:56db31267f10	1180
Vigilance88	18:44905b008f44	1181	//Start control
Vigilance88	18:44905b008f44	1182	void start_control(void)
Vigilance88	18:44905b008f44	1183	{
Vigilance88	35:7d9fca0b1545	1184	control_timer.attach(&control,CONTROL_RATE); //100 Hz control
Vigilance88	56:5ff9e5c1ed44	1185	servo_timer.attach(&servo_control, SERVO_RATE); //50 Hz control
Vigilance88	35:7d9fca0b1545	1186
Vigilance88	35:7d9fca0b1545	1187	//Debug LED will be blue when control is on. If sampling and control are on -> blue + green = cyan.
Vigilance88	35:7d9fca0b1545	1188	blue=0;
Vigilance88	35:7d9fca0b1545	1189	pc.printf("\|\|- started control -\|\| \r\n");
Vigilance88	35:7d9fca0b1545	1190	}
Vigilance88	35:7d9fca0b1545	1191
Vigilance88	18:44905b008f44	1192	//stop control
Vigilance88	18:44905b008f44	1193	void stop_control(void)
Vigilance88	18:44905b008f44	1194	{
Vigilance88	18:44905b008f44	1195	control_timer.detach();
Vigilance88	56:5ff9e5c1ed44	1196	servo_timer.detach();
Vigilance88	53:bf0d97487e84	1197	pwm_motor1=0; pwm_motor2=0;
Vigilance88	26:fe3a5469dd6b	1198	//Debug LED will be off when control is off
Vigilance88	26:fe3a5469dd6b	1199	blue=1;
Vigilance88	26:fe3a5469dd6b	1200	pc.printf("\|\|- stopped control -\|\| \r\n");
vsluiter	2:e314bb3b2d99	1201	}
vsluiter	0:32bb76391d89	1202
Vigilance88	21:d6a46315d5f5	1203
Vigilance88	26:fe3a5469dd6b	1204	//Clears the putty (or other terminal) window
Vigilance88	26:fe3a5469dd6b	1205	void clearTerminal()
Vigilance88	26:fe3a5469dd6b	1206	{
Vigilance88	26:fe3a5469dd6b	1207	pc.putc(27);
Vigilance88	26:fe3a5469dd6b	1208	pc.printf("[2J"); // clear screen
Vigilance88	26:fe3a5469dd6b	1209	pc.putc(27); // ESC
Vigilance88	26:fe3a5469dd6b	1210	pc.printf("[H"); // cursor to home
Vigilance88	26:fe3a5469dd6b	1211	}
Vigilance88	21:d6a46315d5f5	1212
Vigilance88	30:a9fdd3202ca2	1213
Vigilance88	30:a9fdd3202ca2	1214	void controlMenu()
Vigilance88	30:a9fdd3202ca2	1215	{
Vigilance88	48:a1f97eb8c020	1216	//Title Box
Vigilance88	30:a9fdd3202ca2	1217	pc.putc(201);
Vigilance88	30:a9fdd3202ca2	1218	for(int j=0;j<33;j++){
Vigilance88	30:a9fdd3202ca2	1219	pc.putc(205);
Vigilance88	30:a9fdd3202ca2	1220	}
Vigilance88	30:a9fdd3202ca2	1221	pc.putc(187);
Vigilance88	30:a9fdd3202ca2	1222	pc.printf("\n\r");
Vigilance88	30:a9fdd3202ca2	1223	pc.putc(186); pc.printf(" Control Menu "); pc.putc(186);
Vigilance88	30:a9fdd3202ca2	1224	pc.printf("\n\r");
Vigilance88	30:a9fdd3202ca2	1225	pc.putc(200);
Vigilance88	30:a9fdd3202ca2	1226	for(int k=0;k<33;k++){
Vigilance88	30:a9fdd3202ca2	1227	pc.putc(205);
Vigilance88	30:a9fdd3202ca2	1228	}
Vigilance88	30:a9fdd3202ca2	1229	pc.putc(188);
Vigilance88	30:a9fdd3202ca2	1230
Vigilance88	30:a9fdd3202ca2	1231	pc.printf("\n\r");
Vigilance88	30:a9fdd3202ca2	1232	//endbox
Vigilance88	48:a1f97eb8c020	1233
Vigilance88	38:c8ac615d0c8f	1234	pc.printf("A) Move Arm Left\r\n");
Vigilance88	38:c8ac615d0c8f	1235	pc.printf("D) Move Arm Right\r\n");
Vigilance88	38:c8ac615d0c8f	1236	pc.printf("W) Move Arm Up\r\n");
Vigilance88	38:c8ac615d0c8f	1237	pc.printf("S) Move Arm Down\r\n");
Vigilance88	56:5ff9e5c1ed44	1238	pc.printf("g) Turn debugging on / off \r\n");
Vigilance88	30:a9fdd3202ca2	1239	pc.printf("q) Exit \r\n");
Vigilance88	30:a9fdd3202ca2	1240	pc.printf("Please make a choice => \r\n");
Vigilance88	30:a9fdd3202ca2	1241	}
Vigilance88	30:a9fdd3202ca2	1242
Vigilance88	28:743485bb51e4	1243	void caliMenu(){
Vigilance88	28:743485bb51e4	1244
Vigilance88	48:a1f97eb8c020	1245	//Title Box
Vigilance88	28:743485bb51e4	1246	pc.putc(201);
Vigilance88	28:743485bb51e4	1247	for(int j=0;j<33;j++){
Vigilance88	28:743485bb51e4	1248	pc.putc(205);
Vigilance88	28:743485bb51e4	1249	}
Vigilance88	28:743485bb51e4	1250	pc.putc(187);
Vigilance88	28:743485bb51e4	1251	pc.printf("\n\r");
Vigilance88	28:743485bb51e4	1252	pc.putc(186); pc.printf(" Calibration Menu "); pc.putc(186);
Vigilance88	28:743485bb51e4	1253	pc.printf("\n\r");
Vigilance88	28:743485bb51e4	1254	pc.putc(200);
Vigilance88	28:743485bb51e4	1255	for(int k=0;k<33;k++){
Vigilance88	28:743485bb51e4	1256	pc.putc(205);
Vigilance88	28:743485bb51e4	1257	}
Vigilance88	28:743485bb51e4	1258	pc.putc(188);
Vigilance88	28:743485bb51e4	1259
Vigilance88	28:743485bb51e4	1260	pc.printf("\n\r");
Vigilance88	28:743485bb51e4	1261	//endbox
Vigilance88	28:743485bb51e4	1262
Vigilance88	28:743485bb51e4	1263	pc.printf("[A]rm Calibration\r\n");
Vigilance88	28:743485bb51e4	1264	pc.printf("[E]MG Calibration\r\n");
Vigilance88	28:743485bb51e4	1265	pc.printf("[B]ack to main menu\r\n");
Vigilance88	28:743485bb51e4	1266	pc.printf("Please make a choice => \r\n");
Vigilance88	28:743485bb51e4	1267
Vigilance88	28:743485bb51e4	1268	}
Vigilance88	28:743485bb51e4	1269
Vigilance88	28:743485bb51e4	1270	void titleBox(){
Vigilance88	28:743485bb51e4	1271
Vigilance88	28:743485bb51e4	1272	//Title Box
Vigilance88	28:743485bb51e4	1273	pc.putc(201);
Vigilance88	28:743485bb51e4	1274	for(int j=0;j<33;j++){
Vigilance88	28:743485bb51e4	1275	pc.putc(205);
Vigilance88	28:743485bb51e4	1276	}
Vigilance88	28:743485bb51e4	1277	pc.putc(187);
Vigilance88	28:743485bb51e4	1278	pc.printf("\n\r");
Vigilance88	28:743485bb51e4	1279	pc.putc(186); pc.printf(" BioRobotics M9 - Group 14 "); pc.putc(186);
Vigilance88	28:743485bb51e4	1280	pc.printf("\n\r");
Vigilance88	28:743485bb51e4	1281	pc.putc(186); pc.printf(" Robot powered ON "); pc.putc(186);
Vigilance88	28:743485bb51e4	1282	pc.printf("\n\r");
Vigilance88	28:743485bb51e4	1283	pc.putc(200);
Vigilance88	28:743485bb51e4	1284	for(int k=0;k<33;k++){
Vigilance88	28:743485bb51e4	1285	pc.putc(205);
Vigilance88	28:743485bb51e4	1286	}
Vigilance88	28:743485bb51e4	1287	pc.putc(188);
Vigilance88	28:743485bb51e4	1288
Vigilance88	28:743485bb51e4	1289	pc.printf("\n\r");
Vigilance88	28:743485bb51e4	1290	//endbox
Vigilance88	28:743485bb51e4	1291	}
Vigilance88	28:743485bb51e4	1292
Vigilance88	28:743485bb51e4	1293	void emgInstructions(){
Vigilance88	36:4d4fc200171b	1294	pc.printf("\r\nPrepare the skin before applying electrodes: \n\r");
Vigilance88	28:743485bb51e4	1295	pc.printf("-> Shave electrode locations if needed and clean with alcohol \n\r"); wait(1);
Vigilance88	56:5ff9e5c1ed44	1296	pc.printf(" Check whether EMG signal looks normal. \n\r "); wait(1);
Vigilance88	38:c8ac615d0c8f	1297	pc.printf("\r\n To calibrate the EMG signals we will measure: \n\r ");
Vigilance88	38:c8ac615d0c8f	1298	pc.printf("- Minimum amplitude, while relaxing all muscles. \n\r ");
Vigilance88	48:a1f97eb8c020	1299	pc.printf("- Maximum Voluntary Contraction of each muscle. \n\r"); wait(1);
Vigilance88	56:5ff9e5c1ed44	1300	pc.printf("\r\nFor the MVC you need to flex the mentioned muscle as much as possible for 3 seconds \n\r"); wait(0.5);
Vigilance88	48:a1f97eb8c020	1301	pc.printf("The measurements will begin once you confirm you're ready [Hit any key] \n\r \n\r"); wait(0.5);
Vigilance88	28:743485bb51e4	1302	}
Vigilance88	28:743485bb51e4	1303
Vigilance88	21:d6a46315d5f5	1304	//keeps input limited between min max
Vigilance88	24:56db31267f10	1305	void keep_in_range(double * in, double min, double max)
Vigilance88	18:44905b008f44	1306	{
Vigilance88	18:44905b008f44	1307	in > min ? in < max? : in = max: in = min;
vsluiter	0:32bb76391d89	1308	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	13 Oct 2015
Imports:	5
Forks:	1
Commits:	65
Dependents:	0
Dependencies:	6
Followers:	7

main.cpp@57:d6192801fd6d, 2015-10-27 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning