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@64:21fbff25d80b, 2015-11-03 (annotated)

Committer:: Vigilance88
Date:: Tue Nov 03 15:11:42 2015 +0000
Revision:: 64:21fbff25d80b
Parent:: 63:08357f5c497b

final - final version

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

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@64:21fbff25d80b, 2015-11-03 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning