group14 - k9
2nd draft
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed Servo
Fork of
robot_mockup
by 
Martijn Kern
Revision 64:21fbff25d80b, committed 2015-11-03
- Comitter:
- Vigilance88
- Date:
- Tue Nov 03 15:11:42 2015 +0000
- Parent:
- 63:08357f5c497b
- Commit message:
- final - final version
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Fri Oct 30 08:00:09 2015 +0000
+++ b/main.cpp Tue Nov 03 15:11:42 2015 +0000
@@ -31,20 +31,19 @@
DigitalOut blue(LED_BLUE);
//EMG shields
-AnalogIn emg1(A0); //Analog input - Biceps EMG
-AnalogIn emg2(A1); //Analog input - Triceps EMG
-AnalogIn emg3(A2); //Analog input - Flexor EMG
-AnalogIn emg4(A3); //Analog input - Extensor EMG
+AnalogIn emg1(A0); //Analog input - Right Flexor EMG
+AnalogIn emg2(A1); //Analog input - Right Extensor EMG
+AnalogIn emg3(A2); //Analog input - Left Flexor EMG
+AnalogIn emg4(A3); //Analog input - Left Extensor EMG
Ticker sample_timer; //Ticker for EMG sampling
Ticker control_timer; //Ticker for control loop
Ticker servo_timer; //Ticker for servo control
Ticker debug_timer; //Ticker for debug printf
+
+//Turn hidscope off if not needed anymore
//HIDScope scope(2); //Scope 4 channels
-// AnalogIn potmeter(A4); //potmeters
-// AnalogIn potmeter2(A5); //
-
//Encoders
QEI Encoder1(D13,D12,NC,32); //channel A and B from encoder, counts = Encoder.getPulses();
QEI Encoder2(D10,D9,NC,32); //channel A and B from encoder,
@@ -63,7 +62,7 @@
//Other buttons
InterruptIn debugbtn(PTA4); //using FRDM buttons - debug on or off
-DigitalIn button2(PTC6); //using FRDM buttons
+DigitalIn button2(PTC6); //using FRDM buttons - not used
/*Text colors ASCII code: Set Attribute Mode <ESC>[{attr1};...;{attrn}m
@@ -87,7 +86,8 @@
/*--------------------------------------------------------------------------------------------------------------------
---- DECLARE VARIABLES -----------------------------------------------------------------------------------------------
--------------------------------------------------------------------------------------------------------------------*/
-volatile bool debug = true;
+//Debugging on or off
+volatile bool debug = true; //default is on
//EMG variables: raw EMG - filtered EMG - maximum voluntary contraction - max amplitude during relaxation.
double emg_biceps; double biceps_power; double bicepsMVC = 0; double bicepsmin=0;
@@ -102,8 +102,8 @@
double dx, dy; //Integral
double emg_control_time; //Elapsed time in EMG control
-//Threshold moving average
-const int window=30; //100 samples
+//Threshold moving average window
+const int window=30; //30 samples
int i=0; //movavg array index
double movavg1[window]; //moving average arrays with size of window
double movavg2[window];
@@ -122,7 +122,7 @@
double m2_error=0; double m2_e_int=0; double m2_e_prev=0; //Error, integrated error, previous error motor 2
const double m2_kp=1; const double m2_ki=0.01; const double m2_kd=0.05; //Proportional, integral and derivative gains.
-//Calibration bools, checks if elbow/shoulder limits are hit
+//Calibration bools, checks if elbow/shoulder limits are hit and if calibration is complete
volatile bool done1 = false;
volatile bool done2 = false;
volatile bool calibrating = false;
@@ -172,7 +172,7 @@
const double low_a1 = -1.968902268531908;
const double low_a2 = 0.9693784555043481;
-//Derivative lowpass filter 60 Hz - remove error noise
+//Derivative lowpass filter 60 Hz - remove derivative error noise
const double derlow_b0 = 0.027859766117136;
const double derlow_b1 = 0.0557195322342721;
const double derlow_b2 = 0.027859766117136;
@@ -190,13 +190,13 @@
//Reference position
double x; double y;
-//Select whether to use Trig or DLS method, emg true or false
+//Select whether to use Trig (1) or DLS method (2), emg control true or false
int control_method;
bool emg_control;
//Inverse Kinematics - Trig / Gonio method.
//First convert reference position to angle needed, then compare that angle to current angle.
-double reftheta1; double reftheta2; //reference angles
+double reftheta1; double reftheta2; //reference angles
double costheta1; double sintheta1; //helper variables
double costheta2; double sintheta2; //
@@ -207,11 +207,11 @@
double q1_error, q2_error; //Angle errors
double x_error, y_error; //Position errors
double lambda=0.1; //Damping constant
-double powlambda2, powlambda4;
-double powl1, powl2;
-double sintheta1theta2, costheta1theta2;
+double powlambda2, powlambda4; //helper variables to reduce calculation time
+double powl1, powl2; //
+double sintheta1theta2, costheta1theta2; //
-//Mechanical Limits
+//Mechanical Limits (pulse counts and radians)
int theta1_cal, theta2_cal; //Pulse counts at mechanical limits.
double theta1_lower=0.698132, theta1_upper=2.35619; //motor1: lower limit 40 degrees, upper limit 135
double theta2_lower=0.750492, theta2_upper=2.40855; //motor2: lower limit 43 degrees, upper limit 138 degrees.
@@ -222,6 +222,8 @@
//Using biquadFilter library
//Syntax: biquadFilter filter(a1, a2, b0, b1, b2); coefficients. Call with: filter.step(u), with u signal to be filtered.
+//Each biquadFilter object can only be used by one signal - memory variables are unique for each emg.
+//This means 4 biquads for each muscle.
//Biceps
biquadFilter highpass( high_a1 , high_a2 , high_b0 , high_b1 , high_b2 ); // removes DC and movement artefacts
biquadFilter notch1( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 ); // removes 49-51 Hz power interference
@@ -246,7 +248,7 @@
biquadFilter notch2_4( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 ); //
biquadFilter lowpass4( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // EMG envelope
-//PID filter (lowpass 60 Hz)
+//PID filter (lowpass 60 Hz, 6*crossoverfreq)
biquadFilter derfilter1( derlow_a1 , derlow_a2 , derlow_b0 , derlow_b1 , derlow_b2 ); // derivative filter
biquadFilter derfilter2( derlow_a1 , derlow_a2 , derlow_b0 , derlow_b1 , derlow_b2 ); // derivative filter
@@ -255,39 +257,40 @@
--------------------------------------------------------------------------------------------------------------------*/
void sample_filter(void); //Sampling and filtering
-void control(); //Control - reference -> error -> pid -> motor output
-void servo_control(); //Mouse alignment with servo
+void control(); //Control loop - reference -> error -> pid -> motor output
+void servo_control(); //Mouse alignment feed forward for servo
void calibrate_emg(); //Instructions + measurement of Min and MVC of each muscle
void emg_mvc(); //Helper funcion for storing MVC value
void emg_min(); //Helper function for storing Min value
void calibrate_arm(void); //Calibration of the arm with limit switches
void start_sampling(void); //Attaches the sample_filter function to a 500Hz ticker
void stop_sampling(void); //Stops sample_filter
-void start_control(void); //Attaches the control function to a 100Hz ticker
-void stop_control(void); //Stops control function
+void start_control(void); //Attaches the control function to a 100Hz ticker and the servo_control to a 50Hz ticker
+void stop_control(void); //Stops control and servo control
//Keeps the input between min and max value
void keep_in_range(double * in, double min, double max);
-//Reusable PID controller, previous and integral error need to be passed by reference
+//Reusable PID controller, previous and integral error need to be passed by reference.
+//Need two because of different derivative filter biquads.
double pid(double error, double kp, double ki, double kd,double &e_int, double &e_prev);
double pid2(double error, double kp, double ki, double kd,double &e_int, double &e_prev);
//Menu functions
-void debugging();
-void debug_trigger();
-void mainMenu();
-void caliMenu();
-void controlMenu();
-void controlButtons();
-void clearTerminal();
-void emgInstructions();
-void titleBox();
+void debugging(); //Prints useful debug parameters if debugging is turned on.
+void debug_trigger(); //Triggers debug on or off - attach to 1 hz ticker
+void mainMenu(); //Prints the main menu
+void caliMenu(); //Prints the calibration menu
+void controlMenu(); //Prints the control menu with WASD button control
+void controlButtons(); //
+void clearTerminal(); //Clears the putty window
+void emgInstructions(); //Optional - prints instructions for preparing the skin for EMG
+void titleBox(); //Prints a fancy box. To view correctly putty translation-character set needs to be set to CP437.
//Limit switches - power off motors if switches hit (rising edge interrupt)
-void calibrate(void);
-void shoulder();
-void elbow();
+void calibrate(void); //Rotates arm clockwise slowly untill switches are hit
+void shoulder(); //Functions attached to buttons - when hit: encoder pulses are set to mechanical limit angles and motor turned off.
+void elbow(); //
/*--------------------------------------------------------------------------------------------------------------------
---- MAIN LOOP -------------------------------------------------------------------------------------------------------
@@ -300,7 +303,7 @@
servoPwm.Enable(602,20000); //Start position servo, PWM period in usecs
- //Set PwmOut frequency to x Hz
+ //Set PwmOut frequency to 50 Hz
pwm_motor1.period(0.02);
pwm_motor2.period(0.02);
@@ -313,7 +316,7 @@
mainMenu();
char command=0;
-
+ //Main menu:
while(command != 'Q' && command != 'q')
{
if(pc.readable()){
@@ -321,23 +324,25 @@
switch(command){
+ //User chooses 'c'
case 'c':
case 'C': {
pc.printf("\n\r => You chose calibration.\r\n\n");
caliMenu();
char command2=0;
-
+ //Calibration menu:
while(command2 != 'B' && command2 != 'b'){
command2 = pc.getc();
switch(command2){
+ //user chooses 'a'
case 'a':
case 'A':
pc.printf("\n\r => Arm Calibration Starting... please wait \n\r");
calibrate_arm(); wait(1);
caliMenu();
break;
-
+ //user chooses 'e'
case 'e':
case 'E':
pc.printf("\n\r => EMG Calibration Starting... please wait \n\r");
@@ -349,7 +354,7 @@
pc.printf("\n\r------------------------- \n\r");
caliMenu();
break;
-
+ //user chooses 'b'
case 'b':
case 'B':
pc.printf("\n\r => Going back to main menu.. \n\r");
@@ -360,6 +365,8 @@
}//end while
break;
}//end case c C
+
+ //user chooses 't'
case 't':
case 'T':
pc.printf("=> You chose TRIG button control \r\n\n"); wait(1);
@@ -372,6 +379,7 @@
}
controlButtons();
break;
+ //user chooses 'd'
case 'd':
case 'D':
pc.printf("=> You chose DLS button control \r\n\n"); wait(1);
@@ -384,6 +392,7 @@
}
controlButtons();
break;
+ //user chooses 'e'
case 'e':
case 'E':
pc.printf("=> You chose EMG DLS control \r\n\n"); wait(1);
@@ -396,22 +405,12 @@
controlButtons();
break;
- case 'R':
- red=!red;
- pc.printf("=> Red LED triggered \n\r");
- break;
- case 'G':
- green=!green;
- pc.printf("=> Green LED triggered \n\r");
- break;
- case 'B':
- blue=!blue;
- pc.printf("=> Blue LED triggered \n\r");
- break;
+ //user chooses 'q'
case 'q':
case 'Q':
break;
+ //other inputs
default:
pc.printf("=> Invalid Input \n\r");
break;
@@ -434,7 +433,6 @@
--------------------------------------------------------------------------------------------------------------------*/
//Debug function: prints important variables to check if things are calculating correctly - find errors
-
void debug_trigger(){
debug=!debug;
printf("Debug triggered: %s \r\n", debug ? "ON" : "OFF");
@@ -443,7 +441,7 @@
void debugging()
{
if(debug==true){
- //Debugging values:
+ //Choose which debugging values to show:
pc.printf("\r\nRef pos: %f and %f \r\n",x,y);
pc.printf("Cur pos: %f and %f \r\n",current_x,current_y);
//pc.printf("Pos Error: %f and %f \r\n",x_error,y_error);
@@ -474,17 +472,15 @@
servopos = (2100/PI)*theta3 + 600;
servoPwm.SetPosition(servopos);
- //old:
- //theta3 = 1.5*PI - deltatheta1 - deltatheta2;
- //servopos = -(2100/PI)*theta3 + 2700;
}
//Use WASD keys to change reference position, x is a and d, y is w and s.
void controlButtons()
{
controlMenu();
- debug_timer.attach(&debugging,1);
+ debug_timer.attach(&debugging,1); //debug printing at 1 hz
char c=0;
+ //control menu
while(c != 'Q' && c != 'q') {
@@ -492,22 +488,26 @@
c = pc.getc();
switch (c)
{
+ //user chooses 'd'
case 'd' :
x = x + 0.01;
break;
+ //user chooses 'a'
case 'a' :
x-=0.01;
break;
-
+
+ //user chooses 'w'
case 'w' :
y+=0.01;
break;
+ //user chooses 's'
case 's' :
y-=0.01;
@@ -516,6 +516,7 @@
case 'g' :
debug=true;
break;
+ //user chooses 'q'
case 'q' :
case 'Q' :
pc.printf("=> Quitting control... \r\n"); wait(1);
@@ -545,8 +546,8 @@
emg_flexor = emg3.read(); //Flexor
emg_extens = emg4.read(); //Extensor
- //Filter: high-pass -> notch -> rectify -> lowpass or moving average
- // Can we use same biquadFilter (eg. highpass) for other muscles or does each muscle need its own biquad?
+ //Filter: high-pass -> notch -> rectify -> lowpass
+ //each muscle need its own biquad per filter
biceps_power = highpass.step(emg_biceps); triceps_power = highpass2.step(emg_triceps); flexor_power = highpass3.step(emg_flexor); extens_power = highpass4.step(emg_extens);
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);
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);
@@ -569,7 +570,7 @@
}
-//Send arm to mechanical limits, and set encoder to 0. Then send arm to starting position.
+//Send arm to mechanical limits, and set encoder to these angles.
void calibrate_arm(void)
{
pc.printf("Calibrate_arm() entered\r\n");
@@ -600,7 +601,7 @@
x = l1 * cos(theta1_lower) + l2 * cos(theta1_lower + theta2_lower);
y = l1 * sin(theta1_lower) + l2 * sin(theta1_lower + theta2_lower);
- //x = 0.2220;
+ //x = 0.2220; position at limits
//y = 0.4088;
}
}
@@ -786,7 +787,6 @@
if(muscle==1){
if(biceps_power>bicepsMVC){
- //pc.printf("+ ");
pc.printf("%s+ %s",GREEN_,_GREEN);
bicepsMVC=biceps_power;
}
@@ -829,6 +829,7 @@
}
+//Minimum measurement during relaxation
void emg_min()
{
if(biceps_power>bicepsmin){
@@ -866,7 +867,7 @@
}
-//PID for motor 2 - needed because of biquadfilter memory variables?
+//PID for motor 2 - needed because of biquadfilter memory variables
double pid2(double error, double kp, double ki, double kd,double &e_int, double &e_prev)
{
// Derivative
@@ -881,7 +882,8 @@
}
-//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
+//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
void control()
{
@@ -928,7 +930,7 @@
emg_control_time += CONTROL_RATE;
- //Move mouse to starting position - bottom right corner - when EMG control starts. After 5 seconds reference can be changed with EMG.
+ //Move mouse to starting position - bottom right corner of used workspace - when EMG control starts. After 5 seconds reference can be changed with EMG.
if(emg_control_time < 5){
x=0; y=0.3;
@@ -1010,7 +1012,7 @@
//Reference angle 1
reftheta1 = atan2(y, x) - atan2(k2, k1);
- /* alternative:
+ /* alternative, but extra square root
costheta1 = ( x * (l1 + l2 * costheta2) + y * l2 * sintheta2 ) / ( pow(x,2) + pow(y,2) );
sintheta1 = sqrt( abs( 1 - pow(costheta1,2) ) );
@@ -1027,17 +1029,20 @@
/******************************** START OF DLS INVERSE KINEMATICS ****************************************/
if(control_method==2){
//inverse kinematics (error in position to error in angles)
- powlambda2 = pow(lambda,2);
- powlambda4 = pow(lambda,4);
- powl2 = pow(l2,2);
- powl1 = pow(l1,2);
- sintheta1theta2 = sin(theta1 + theta2);
- costheta1theta2 = cos(theta1 + theta2);
+ powlambda2 = pow(lambda,2); //help functions to reduce amount of calculations
+ powlambda4 = pow(lambda,4); //
+ powl2 = pow(l2,2); //
+ powl1 = pow(l1,2); //
+ sintheta1theta2 = sin(theta1 + theta2); //
+ costheta1theta2 = cos(theta1 + theta2); //
+
+ //calculate DLS matrix
dls1= -(l2*powlambda2*sintheta1theta2 + l1*powlambda2*sin(theta1) + l1*powl2*pow(costheta1theta2,2)*sin(theta1) - l1*powl2*costheta1theta2*sintheta1theta2*cos(theta1))/(powlambda4 + 2*powl2*powlambda2*pow(costheta1theta2,2) + 2*powl2*powlambda2*pow(sintheta1theta2,2) + powl1*powlambda2*pow(cos(theta1),2) + powl1*powlambda2*pow(sin(theta1),2) + powl1*powl2*pow(costheta1theta2,2)*pow(sin(theta1),2) + powl1*powl2*pow(sintheta1theta2,2)*pow(cos(theta1),2) + 2*l1*l2*powlambda2*costheta1theta2*cos(theta1) + 2*l1*l2*powlambda2*sintheta1theta2*sin(theta1) - 2*powl1*powl2*costheta1theta2*sintheta1theta2*cos(theta1)*sin(theta1));
dls2= (l2*powlambda2*costheta1theta2 + l1*powlambda2*cos(theta1) + l1*powl2*pow(sintheta1theta2,2)*cos(theta1) - l1*powl2*costheta1theta2*sintheta1theta2*sin(theta1))/(powlambda4 + 2*powl2*powlambda2*pow(costheta1theta2,2) + 2*powl2*powlambda2*pow(sintheta1theta2,2) + powl1*powlambda2*pow(cos(theta1),2) + powl1*powlambda2*pow(sin(theta1),2) + powl1*powl2*pow(costheta1theta2,2)*pow(sin(theta1),2) + powl1*powl2*pow(sintheta1theta2,2)*pow(cos(theta1),2) + 2*l1*l2*powlambda2*costheta1theta2*cos(theta1) + 2*l1*l2*powlambda2*sintheta1theta2*sin(theta1) - 2*powl1*powl2*costheta1theta2*sintheta1theta2*cos(theta1)*sin(theta1));
dls3= -(l2*powlambda2*sintheta1theta2 - l1*powl2*pow(costheta1theta2,2)*sin(theta1) + powl1*l2*sintheta1theta2*pow(cos(theta1),2) - powl1*l2*costheta1theta2*cos(theta1)*sin(theta1) + l1*powl2*costheta1theta2*sintheta1theta2*cos(theta1))/(powlambda4 + 2*powl2*powlambda2*pow(costheta1theta2,2) + 2*powl2*powlambda2*pow(sintheta1theta2,2) + powl1*powlambda2*pow(cos(theta1),2) + powl1*powlambda2*pow(sin(theta1),2) + powl1*powl2*pow(costheta1theta2,2)*pow(sin(theta1),2) + powl1*powl2*pow(sintheta1theta2,2)*pow(cos(theta1),2) + 2*l1*l2*powlambda2*costheta1theta2*cos(theta1) + 2*l1*l2*powlambda2*sintheta1theta2*sin(theta1) - 2*powl1*powl2*costheta1theta2*sintheta1theta2*cos(theta1)*sin(theta1));
dls4= (l2*powlambda2*costheta1theta2 - l1*powl2*pow(sintheta1theta2,2)*cos(theta1) + powl1*l2*costheta1theta2*pow(sin(theta1),2) - powl1*l2*sintheta1theta2*cos(theta1)*sin(theta1) + l1*powl2*costheta1theta2*sintheta1theta2*sin(theta1))/(powlambda4 + 2*powl2*powlambda2*pow(costheta1theta2,2) + 2*powl2*powlambda2*pow(sintheta1theta2,2) + powl1*powlambda2*pow(cos(theta1),2) + powl1*powlambda2*pow(sin(theta1),2) + powl1*powl2*pow(costheta1theta2,2)*pow(sin(theta1),2) + powl1*powl2*pow(sintheta1theta2,2)*pow(cos(theta1),2) + 2*l1*l2*powlambda2*costheta1theta2*cos(theta1) + 2*l1*l2*powlambda2*sintheta1theta2*sin(theta1) - 2*powl1*powl2*costheta1theta2*sintheta1theta2*cos(theta1)*sin(theta1));
+ //calculate angle errors from position error
q1_error = dls1 * x_error + dls2 * y_error;
q2_error = dls3 * x_error + dls4 * y_error;
@@ -1052,7 +1057,7 @@
motor1: lower limit 40 degrees, upper limit 135
motor2: lower 43 degrees, upper limit 138 degrees. */
- //lower limits: Negative error not allowed to go further. NEEDS MORE TESTING
+ //lower limits: Negative error not allowed to go further.
if (theta1 < theta1_lower){
if (m1_error > 0)
m1_error = m1_error;
@@ -1082,7 +1087,7 @@
u1=pid(m1_error,m1_kp,m1_ki,m1_kd,m1_e_int,m1_e_prev); //motor 1
u2=pid2(m2_error,m2_kp,m2_ki,m2_kd,m2_e_int,m2_e_prev); //motor 2
- //keep u between limits, sign = direction, PWM = 0-1
+ //keep PWM between limits, sign = direction
keep_in_range(&u1,-0.3,0.3);
keep_in_range(&u2,-0.3,0.3);
@@ -1093,8 +1098,7 @@
//send PWM and DIR to motor 2
dir_motor2 = u2>0 ? 0 : 1; //conditional statement, same as if else below
pwm_motor2.write(abs(u2));
-
-
+
/*if(u1 > 0)
{
@@ -1117,7 +1121,7 @@
}
-
+//Prints the main menu
void mainMenu()
{
//Title Box
@@ -1199,7 +1203,7 @@
pc.printf("[H"); // cursor to home
}
-
+//Prints control menu
void controlMenu()
{
//Title Box
@@ -1229,6 +1233,7 @@
pc.printf("Please make a choice => \r\n");
}
+//prints calibration menu
void caliMenu(){
//Title Box
@@ -1255,7 +1260,7 @@
pc.printf("Please make a choice => \r\n");
}
-
+//prints square title box
void titleBox(){
//Title Box
@@ -1279,6 +1284,7 @@
//endbox
}
+//prints emg instructions
void emgInstructions(){
pc.printf("\r\nPrepare the skin before applying electrodes: \n\r");
pc.printf("-> Shave electrode locations if needed and clean with alcohol \n\r"); wait(1);