2nd draft
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed Servo
Fork of robot_mockup by
Diff: main.cpp
- Revision:
- 35:7d9fca0b1545
- Parent:
- 34:d6ec7c634763
- Child:
- 36:4d4fc200171b
--- a/main.cpp Thu Oct 22 12:59:55 2015 +0000 +++ b/main.cpp Thu Oct 22 14:18:41 2015 +0000 @@ -84,10 +84,10 @@ --------------------------------------------------------------------------------------------------------------------*/ //EMG variables: raw EMG - filtered EMG - maximum voluntary contraction -double emg_biceps; double biceps_power; double bicepsMVC = 0; -double emg_triceps; double triceps_power; double tricepsMVC = 0; -double emg_flexor; double flexor_power; double flexorMVC = 0; -double emg_extens; double extens_power; double extensMVC = 0; +double emg_biceps; double biceps_power; double bicepsMVC = 0; double bicepsmin; +double emg_triceps; double triceps_power; double tricepsMVC = 0; double tricepsmin; +double emg_flexor; double flexor_power; double flexorMVC = 0; double flexormin; +double emg_extens; double extens_power; double extensMVC = 0; double extensmin; int muscle; //Muscle selector for MVC measurement double calibrate_time; //Elapsed time for each MVC measurement @@ -95,10 +95,10 @@ //PID variables double u1; double u2; //Output of PID controller (PWM value for motor 1 and 2) double m1_error=0; double m1_e_int=0; double m1_e_prev=0; //Error, integrated error, previous error -const double m1_kp=0.001; const double m1_ki=0.0125; const double m1_kd=0.1; //Proportional, integral and derivative gains. +const double m1_kp=0.1; const double m1_ki=0.0125; const double m1_kd=0.02; //Proportional, integral and derivative gains. double m2_error=0; double m2_e_int=0; double m2_e_prev=0; //Error, integrated error, previous error -const double m2_kp=0.001; const double m2_ki=0.0125; const double m2_kd=0.1; //Proportional, integral and derivative gains. +const double m2_kp=0.1; const double m2_ki=0.0125; const double m2_kd=0.02; //Proportional, integral and derivative gains. //Calibration, checks if elbow/shoulder are done bool done1 = false; @@ -212,7 +212,9 @@ 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 start_dlscontrol(void); void stop_control(void); //Stops control function +void emg_min(); //Keeps the input between min and max value void keep_in_range(double * in, double min, double max); @@ -317,9 +319,9 @@ }//end while break; - case 's': - case 'S': - pc.printf("=> You chose control \r\n\n"); + case 't': + case 'T': + pc.printf("=> You chose TRIG control \r\n\n"); wait(1); start_sampling(); wait(1); @@ -327,6 +329,16 @@ wait(1); controlButtons(); break; + case 'd': + case 'D': + pc.printf("=> You chose DLS control \r\n\n"); + wait(1); + start_sampling(); + wait(1); + start_dlscontrol(); + wait(1); + controlButtons(); + break; case 'R': red=!red; pc.printf("=> Red LED triggered \n\r"); @@ -414,7 +426,7 @@ 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("Current angles: %f and %f \r\n",theta1,theta2); - pc.printf("Error in angles: %f and %f \r\n",dtheta1,dtheta2); + 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\n"); } @@ -586,6 +598,29 @@ } +void emg_min() +{ + + if(biceps_power<bicepsmin){ + bicepsmin=biceps_power; + } + + if(triceps_power<tricepsmin){ + tricepsmin=triceps_power; + } + + if(flexor_power<flexormin){ + flexormin=flexor_power; + } + + if(extens_power < extensmin){ + extensmin = extens_power; + } + + calibrate_time = calibrate_time + 0.002; + +} + //EMG calibration void calibrate_emg() { @@ -593,6 +628,27 @@ pc.printf("Testcode calibration \r\n"); wait(1); + pc.printf("Starting minimum measurement, relax all muscles."); + wait(1); + pc.printf(" Press any key to begin... "); wait(1); + char input; + input=pc.getc(); + pc.printf(" \r\n Starting in 3... \r\n"); wait(1); + pc.printf(" \r\n Starting in 2... \r\n"); wait(1); + pc.printf(" \r\n Starting in 1... \r\n"); wait(1); + + start_sampling(); + timer.attach(&emg_min,SAMPLE_RATE); + wait(5); + timer.detach(); + pc.printf("\r\n Measurement complete."); wait(1); + pc.printf("\r\n Biceps min = %f \r\n",bicepsmin); wait(1); + pc.printf("\r\n Triceps min = %f \r\n",tricepsmin); wait(1); + pc.printf("\r\n Flexor min = %f \r\n",flexormin); wait(1); + pc.printf("\r\n Extensor min = %f \r\n",extensmin); wait(1); + + calibrate_time=0; + pc.printf("\r\n Now we will measure maximum amplitudes \r\n"); wait(1); pc.printf("+ means current sample is higher than stored MVC\r\n"); pc.printf("- means current sample is lower than stored MVC\r\n"); wait(2); @@ -601,7 +657,6 @@ pc.printf("----------------\r\n "); wait(1); pc.printf(" Press any key to begin... "); wait(1); - char input; input=pc.getc(); pc.putc(input); pc.printf(" \r\n Starting in 3... \r\n"); wait(1); @@ -634,7 +689,6 @@ pc.printf(" \r\n Starting in 2... \r\n"); wait(1); pc.printf(" \r\n Starting in 1... \r\n"); wait(1); start_sampling(); - muscle=1; timer.attach(&emg_mvc,0.002); wait(5); timer.detach(); @@ -646,9 +700,50 @@ //Flexor: muscle=3; + pc.printf("\r\n----------------\r\n "); + pc.printf(" Flexor is next.\r\n "); + pc.printf("----------------\r\n "); + wait(1); + + pc.printf(" Press any key to begin... "); wait(1); + input=pc.getc(); + pc.putc(input); + pc.printf(" \r\n Starting in 3... \r\n"); wait(1); + pc.printf(" \r\n Starting in 2... \r\n"); wait(1); + pc.printf(" \r\n Starting in 1... \r\n"); wait(1); + start_sampling(); + timer.attach(&emg_mvc,0.002); + wait(5); + timer.detach(); + pc.printf("\r\n Flexor MVC = %f \r\n",flexorMVC); + + pc.printf("Calibrate_emg() exited \r\n"); + pc.printf("Measured time: %f seconds \r\n",calibrate_time); + calibrate_time=0; + //Extensor: + muscle=4; + pc.printf("\r\n----------------\r\n "); + pc.printf(" Extensor is next.\r\n "); + pc.printf("----------------\r\n "); + wait(1); + + pc.printf(" Press any key to begin... "); wait(1); + input=pc.getc(); + pc.putc(input); + pc.printf(" \r\n Starting in 3... \r\n"); wait(1); + pc.printf(" \r\n Starting in 2... \r\n"); wait(1); + pc.printf(" \r\n Starting in 1... \r\n"); wait(1); + start_sampling(); + timer.attach(&emg_mvc,0.002); + wait(5); + timer.detach(); + pc.printf("\r\n Extensor MVC = %f \r\n",extensMVC); + pc.printf("Calibrate_emg() exited \r\n"); + pc.printf("Measured time: %f seconds \r\n",calibrate_time); + calibrate_time=0; //Stop sampling, detach ticker stop_sampling(); @@ -675,12 +770,13 @@ /* //normalize emg to value between 0-1 - biceps = (biceps - bicepsmin) / (bicepsmvc - bicepsmin) - biceps = (biceps - bicepsmin) / (bicepsmvc - bicepsmin) - biceps = (biceps - bicepsmin) / (bicepsmvc - bicepsmin) - biceps = (biceps - bicepsmin) / (bicepsmvc - bicepsmin) + norm_biceps = (biceps_power - bicepsmin) / (bicepsMVC - bicepsmin) + norm_triceps = (triceps_power - tricepsmin) / (tricepsMVC - tricepsmin) + norm_flexor = (flexor_power - flexormin) / (flexorMVC - flexormin) + norm_extens = (extens_power - extensmin) / (extensMVC - extensmin) //threshold detection! buffer? + TODO //analyze emg (= velocity) if (biceps>triceps && biceps > 0.1) @@ -972,7 +1068,8 @@ //endbox wait(1); pc.printf("[C]alibration\r\n"); wait(0.2); - pc.printf("[S]tart Control with buttons\r\n"); wait(0.2); + pc.printf("[T]RIG Control with buttons\r\n"); wait(0.2); + pc.printf("[D]LS Control with buttons\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); @@ -1003,6 +1100,15 @@ //Start control void start_control(void) { + control_timer.attach(&control,CONTROL_RATE); //100 Hz control + + //Debug LED will be blue when control is on. If sampling and control are on -> blue + green = cyan. + blue=0; + pc.printf("||- started control -|| \r\n"); +} + +void start_dlscontrol(void) +{ control_timer.attach(&dlscontrol,CONTROL_RATE); //100 Hz control //Debug LED will be blue when control is on. If sampling and control are on -> blue + green = cyan. @@ -1010,6 +1116,7 @@ pc.printf("||- started control -|| \r\n"); } + //stop control void stop_control(void) {