group14 - k9
2nd draft
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed Servo
Fork of
robot_mockup
by 
Martijn Kern
Diff: main.cpp
- Revision:
- 56:5ff9e5c1ed44
- Parent:
- 55:726fdab812a9
- Child:
- 57:d6192801fd6d
--- a/main.cpp Tue Oct 27 15:28:31 2015 +0000
+++ b/main.cpp Tue Oct 27 19:56:59 2015 +0000
@@ -14,8 +14,9 @@
//Define important constants in memory
#define PI 3.14159265
#define SAMPLE_RATE 0.002 //500 Hz EMG sample rate
-#define CONTROL_RATE 0.005 //100 Hz Control rate
-#define ENCODER_CPR 4200 //both motor encoders have 64 (X4), 32 (X2) counts per revolution of motor shaft
+#define CONTROL_RATE 0.005 //100 Hz Control rate
+#define SERVO_RATE 0.05 //50 Hz Servo Control rate
+#define ENCODER_CPR 4200 //both motor encoders have 64 (X4), 32 (X2) counts per revolution of motor shaft
//gearbox 1:131.25 -> 4200 counts per revolution of the output shaft (X2),
#define PWM_PERIOD 0.0001 //10k Hz pwm motor frequency. Higher -> too hot, lower -> motor doesnt respond correctly
/*--------------------------------------------------------------------------------------------------------------------
@@ -37,7 +38,9 @@
Ticker sample_timer; //Ticker for EMG sampling
Ticker control_timer; //Ticker for control loop
-//HIDScope scope(4); //Scope 4 channels
+Ticker servo_timer; //Ticker for servo control
+Ticker debug_timer;
+HIDScope scope(4); //Scope 4 channels
// AnalogIn potmeter(A4); //potmeters
// AnalogIn potmeter2(A5); //
@@ -84,6 +87,7 @@
/*--------------------------------------------------------------------------------------------------------------------
---- DECLARE VARIABLES -----------------------------------------------------------------------------------------------
--------------------------------------------------------------------------------------------------------------------*/
+volatile bool debug = true;
//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;
@@ -179,6 +183,7 @@
const double l1 = 0.25; const double l2 = 0.25; //Arm lengths
double current_x; double current_y; //Current position
double theta1; double theta2; double theta3; //Current angles
+double deltatheta1; double deltatheta2; //Change in angles compared to mechanical lower limit - for servo
double servopos; //servo position in usec
double rpc; //Encoder resolution: radians per count
@@ -191,7 +196,7 @@
//Inverse Kinematics - Trig / Gonio method.
//First convert reference position to angle needed, then compare that angle to current angle.
-double dtheta1; double dtheta2; //reference angles
+double reftheta1; double reftheta2; //reference angles
double costheta1; double sintheta1; //helper variables
double costheta2; double sintheta2; //
@@ -248,7 +253,7 @@
void sample_filter(void); //Sampling and filtering
void control(); //Control - reference -> error -> pid -> motor output
-void dlscontrol(); //Damped Least Squares method
+void servo_control(); //Mouse alignment with 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
@@ -266,6 +271,7 @@
double pid2(double error, double kp, double ki, double kd,double &e_int, double &e_prev);
//Menu functions
+void debugging();
void mainMenu();
void caliMenu();
void controlMenu();
@@ -282,19 +288,13 @@
/*--------------------------------------------------------------------------------------------------------------------
---- MAIN LOOP -------------------------------------------------------------------------------------------------------
--------------------------------------------------------------------------------------------------------------------*/
-void servotest(){
- //Servo alignment
- theta3 = 1.5*PI - (theta1) - theta2;
- servopos = -(2100/PI)*theta3 + 2700;
- servoPwm.SetPosition(servopos);
-}
-Ticker test;
+
int main()
{
pc.baud(115200); //serial baudrate
red=1; green=1; blue=1; //Make sure debug LEDs are off
- servoPwm.Enable(1650,20000); //Start position servo, PWM period in usecs
+ servoPwm.Enable(600,20000); //Start position servo, PWM period in usecs
//theta1_cal = floor(theta1_lower * (4200/(2*PI)));
//Encoder1.setPulses(theta1_cal); //edited QEI library: added setPulses()
@@ -332,14 +332,13 @@
char command2=0;
- while(command2 != 'B' && command2 != 'b'){
- command2 = pc.getc();
+ while(command2 != 'B' && command2 != 'b'){
+ command2 = pc.getc();
switch(command2){
case 'a':
case 'A':
pc.printf("\n\r => Arm Calibration Starting... please wait \n\r");
- calibrate_arm();
- wait(1);
+ calibrate_arm(); wait(1);
caliMenu();
break;
@@ -362,49 +361,49 @@
break;
}//end switch
- }//end while
- break;
+ }//end while
+ break;
}//end case c C
case 't':
case 'T':
- pc.printf("=> You chose TRIG button control \r\n\n");
- wait(1);
- start_sampling();
- wait(1);
+ pc.printf("=> You chose TRIG button control \r\n\n"); wait(1);
+ start_sampling(); wait(1);
emg_control=false;
control_method=1;
- start_control();
- wait(1);
+ start_control(); wait(1);
+ if(debug)
+ {
+ debug_timer.attach(&debugging,1);
+ }
controlButtons();
break;
case 'd':
case 'D':
- pc.printf("=> You chose DLS button control \r\n\n");
- wait(1);
- start_sampling();
- wait(1);
+ pc.printf("=> You chose DLS button control \r\n\n"); wait(1);
+ start_sampling(); wait(1);
emg_control=false;
control_method=2;
-
- test.attach(&servotest,0.02);
- start_control();
- wait(1);
+ start_control(); wait(1);
+ if(debug)
+ {
+ debug_timer.attach(&debugging,1);
+ }
controlButtons();
break;
case 'e':
case 'E':
- pc.printf("=> You chose EMG DLS control \r\n\n");
- wait(1);
- start_sampling();
- wait(1);
+ pc.printf("=> You chose EMG DLS control \r\n\n"); wait(1);
+ start_sampling(); wait(1);
emg_control_time = 0;
emg_control=true;
- test.attach(&servotest,0.02);
control_method=2;
+ start_control(); wait(1);
+ if(debug)
+ {
+ debug_timer.attach(&debugging,1);
+ }
+ controlButtons();
- start_control();
- wait(1);
- controlButtons();
break;
case 'R':
red=!red;
@@ -430,8 +429,6 @@
} // end while loop
-
-
//When end of while loop reached (user chose quit program).
pc.printf("\r\n------------------------------ \n\r");
@@ -445,32 +442,52 @@
---- FUNCTIONS -------------------------------------------------------------------------------------------------------
--------------------------------------------------------------------------------------------------------------------*/
+//Debug function: prints important variables to check if things are calculating correctly - find errors
+void debugging()
+{
+ //Debugging values:
+ 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);
+ pc.printf("Cur angles: %f and %f \r\n",theta1,theta2);
+ pc.printf("DLS1: %f and DLS2 %f and DLS3 %f and DLS4: %f \r\n",dls1,dls2,dls3,dls4);
+ pc.printf("Error angles: %f and %f \r\n",q1_error,q2_error);
+ pc.printf("PID output: %f and %f \r\n",u1,u2);
+ pc.printf("----------------------------------------\r\n");
+ pc.printf("Buffer1: %f \r\n",biceps_avg);
+ pc.printf("Buffer2: %f \r\n",triceps_avg);
+ pc.printf("Buffer3: %f \r\n",flexor_avg);
+ pc.printf("Buffer4: %f \r\n",extens_avg);
+ pc.printf("----------------------------------------\r\n");
+ pc.printf("Theta3: %f \r\n",theta3);
+ pc.printf("Servopos us: %f \r\n",servopos);
+ pc.printf("----------------------------------------\r\n");
+
+
+}
+
+//Calculates how much servo needs to move to keep mouse aligned
+void servo_control(){
+ //Servo alignment
+ //When shoulder or elbow angle increases from starting position --> servo needs to turn counterclockwise to keep mouse aligned.
+ deltatheta1 = theta1 - theta1_lower;
+ deltatheta2 = theta2 - theta2_lower;
+ theta3 = deltatheta1 + deltatheta2;
+ servopos = -(2100/PI)*theta3 + 2700;
+ 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();
char c=0;
while(c != 'Q' && c != 'q') {
- //Debugging values:
- if(c!='q' && c!='Q'){
- /*pc.printf("Reference position: %f and %f \r\n",x,y);
- pc.printf("Current position: %f and %f \r\n",current_x,current_y);
- pc.printf("Pos Error: %f and %f \r\n",x_error,y_error);
- pc.printf("Current angles: %f and %f \r\n",theta1,theta2);
- pc.printf("DLS1: %f and DLS2 %f and DLS3 %f and DLS4: %f \r\n",dls1,dls2,dls3,dls4);
- pc.printf("Error in angles: %f and %f \r\n",q1_error,q2_error);
- pc.printf("PID output: %f and %f \r\n",u1,u2);
- pc.printf("----------------------------------------\r\n");
- pc.printf("Buffer 1: %f \r\n",biceps_avg);
- pc.printf("Buffer 2: %f \r\n",triceps_avg);
- pc.printf("Buffer 3: %f \r\n",flexor_avg);
- pc.printf("Buffer 4: %f \r\n",extens_avg);
- pc.printf("----------------------------------------\r\n");
- pc.printf("Theta 3: %f \r\n",theta3);
- pc.printf("Servoposition (us): %f \r\n",servopos);
- pc.printf("----------------------------------------\r\n");
- wait(1); */
- }//end if
+
if( pc.readable() ){
c = pc.getc();
@@ -497,11 +514,15 @@
break;
+ case 'g' :
+ debug=true;
+ break;
case 'q' :
case 'Q' :
pc.printf("=> Quitting control... \r\n"); wait(1);
stop_sampling();
stop_control();
+ debug_timer.detach();
pc.printf("Sampling and Control detached \n\r"); wait(1);
pc.printf("Returning to Main Menu \r\n\n"); wait(1);
mainMenu();
@@ -735,7 +756,7 @@
pc.printf(" \r\n Starting in 1... \r\n"); wait(1);
timer.attach(&emg_min,SAMPLE_RATE);
- wait(5);
+ wait(3);
timer.detach();
pc.printf("\r\n Measurement complete."); wait(1);
pc.printf("\r\n Biceps min = %f \r\n",bicepsmin); wait(1);
@@ -764,7 +785,7 @@
muscle=1;
timer.attach(&emg_mvc,SAMPLE_RATE);
- wait(5);
+ wait(3);
timer.detach();
pc.printf("\r\n Measurement complete."); wait(1);
@@ -789,7 +810,7 @@
pc.printf(" \r\n Starting in 1... \r\n"); wait(1);
timer.attach(&emg_mvc,0.002);
- wait(5);
+ wait(3);
timer.detach();
pc.printf("\r\n Triceps MVC = %f \r\n",tricepsMVC);
@@ -813,7 +834,7 @@
pc.printf(" \r\n Starting in 1... \r\n"); wait(1);
timer.attach(&emg_mvc,0.002);
- wait(5);
+ wait(3);
timer.detach();
pc.printf("\r\n Flexor MVC = %f \r\n",flexorMVC);
@@ -837,7 +858,7 @@
pc.printf(" \r\n Starting in 1... \r\n"); wait(1);
timer.attach(&emg_mvc,0.002);
- wait(5);
+ wait(3);
timer.detach();
pc.printf("\r\n Extensor MVC = %f \r\n",extensMVC);
@@ -857,7 +878,7 @@
{
// Derivative
double e_der = (error-e_prev)/ CONTROL_RATE;
- e_der = derfilter1.step(e_der); //derfilter1 - seperate 7hz low-pass biquad for this PID
+ e_der = derfilter1.step(e_der); //derfilter1 - seperate 60hz low-pass biquad for this PID
e_prev = error;
// Integral
e_int = e_int + CONTROL_RATE * error;
@@ -871,7 +892,7 @@
{
// Derivative
double e_der = (error-e_prev)/ CONTROL_RATE;
- e_der = derfilter2.step(e_der); //derfilter2 - seperate 7hz low-pass biquad for this PID
+ e_der = derfilter2.step(e_der); //derfilter2 - seperate 60hz low-pass biquad for this PID
e_prev = error;
// Integral
e_int = e_int + CONTROL_RATE * error;
@@ -995,24 +1016,24 @@
sintheta2 = sqrt( abs( 1 - pow(costheta2,2) ) );
//Reference angle 2
- dtheta2 = atan2(sintheta2,costheta2);
+ reftheta2 = atan2(sintheta2,costheta2);
double k1 = l1 + l2*costheta2;
double k2 = l2*sintheta2;
//Reference angle 1
- dtheta1 = atan2(y, x) - atan2(k2, k1);
+ reftheta1 = atan2(y, x) - atan2(k2, k1);
/* alternative:
costheta1 = ( x * (l1 + l2 * costheta2) + y * l2 * sintheta2 ) / ( pow(x,2) + pow(y,2) );
sintheta1 = sqrt( abs( 1 - pow(costheta1,2) ) );
- dtheta1 = atan2(sintheta1,costheta1);
+ reftheta1 = atan2(sintheta1,costheta1);
*/
//Angle error
- m1_error = dtheta1-theta1;
- m2_error = dtheta2-theta2;
+ m1_error = reftheta1-theta1;
+ m2_error = reftheta2-theta2;
}// end control method 1
/******************************** END OF TRIG INVERSE KINEMATICS ****************************************/
@@ -1131,9 +1152,6 @@
pc.printf("[D]LS Control with WASD\r\n"); wait(0.2);
pc.printf("[E]MG Control - DLS \r\n"); wait(0.2);
pc.printf("[Q]uit Robot Program\r\n"); wait(0.2);
- pc.printf("[R]ed LED\r\n"); wait(0.2);
- pc.printf("[G]reen LED\r\n"); wait(0.2);
- pc.printf("[B]lue LED\r\n"); wait(0.2);
pc.printf("Please make a choice => \r\n");
}
@@ -1161,6 +1179,7 @@
void start_control(void)
{
control_timer.attach(&control,CONTROL_RATE); //100 Hz control
+ servo_timer.attach(&servo_control, SERVO_RATE); //50 Hz control
//Debug LED will be blue when control is on. If sampling and control are on -> blue + green = cyan.
blue=0;
@@ -1171,6 +1190,7 @@
void stop_control(void)
{
control_timer.detach();
+ servo_timer.detach();
pwm_motor1=0; pwm_motor2=0;
//Debug LED will be off when control is off
blue=1;
@@ -1212,6 +1232,7 @@
pc.printf("D) Move Arm Right\r\n");
pc.printf("W) Move Arm Up\r\n");
pc.printf("S) Move Arm Down\r\n");
+ pc.printf("g) Turn debugging on / off \r\n");
pc.printf("q) Exit \r\n");
pc.printf("Please make a choice => \r\n");
}
@@ -1269,11 +1290,11 @@
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);
- pc.printf("\r\n Check whether EMG signal looks normal. \n\r "); wait(1);
+ pc.printf(" Check whether EMG signal looks normal. \n\r "); wait(1);
pc.printf("\r\n To calibrate the EMG signals we will measure: \n\r ");
pc.printf("- Minimum amplitude, while relaxing all muscles. \n\r ");
pc.printf("- Maximum Voluntary Contraction of each muscle. \n\r"); wait(1);
- pc.printf("\r\nFor the MVC you need to flex the mentioned muscle as much as possible for 5 seconds \n\r"); wait(0.5);
+ pc.printf("\r\nFor the MVC you need to flex the mentioned muscle as much as possible for 3 seconds \n\r"); wait(0.5);
pc.printf("The measurements will begin once you confirm you're ready [Hit any key] \n\r \n\r"); wait(0.5);
}