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@56:5ff9e5c1ed44, 2015-10-27 (annotated)

Committer:: Vigilance88
Date:: Tue Oct 27 19:56:59 2015 +0000
Revision:: 56:5ff9e5c1ed44
Parent:: 55:726fdab812a9
Child:: 57:d6192801fd6d

added debug printf function

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

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@56:5ff9e5c1ed44, 2015-10-27 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning