2nd draft
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed Servo
Fork of robot_mockup by
Diff: main.cpp
- Revision:
- 30:a9fdd3202ca2
- Parent:
- 29:948b0b14f6be
- Child:
- 31:7b8b8459bddc
--- a/main.cpp Wed Oct 21 09:19:23 2015 +0000 +++ b/main.cpp Wed Oct 21 10:30:03 2015 +0000 @@ -229,7 +229,7 @@ int main() { pc.baud(115200); //serial baudrate - red=1; green=1; blue=1; //Make sure debug LEDS are off + red=1; green=1; blue=1; //Make sure debug LEDs are off //Set PwmOut frequency to 10k Hz //pwm_motor1.period(PWM_PERIOD); @@ -245,8 +245,9 @@ //calibrate_emg(); + //set initial reference position + x=0.5; y=0; - x=0.5; y=0; //start_control(); //100 Hz control //maybe some stop commands when a button is pressed: detach from timers. @@ -414,7 +415,7 @@ //end if } //end of while loop - } +} //Sample and Filter void sample_filter(void) @@ -455,32 +456,6 @@ } -void controlMenu() -{ - //Title Box - pc.putc(201); - for(int j=0;j<33;j++){ - pc.putc(205); - } - pc.putc(187); - pc.printf("\n\r"); - pc.putc(186); pc.printf(" Control Menu "); pc.putc(186); - pc.printf("\n\r"); - pc.putc(200); - for(int k=0;k<33;k++){ - pc.putc(205); - } - pc.putc(188); - - pc.printf("\n\r"); - //endbox - pc.printf("1) Move Arm Left\r\n"); - pc.printf("2) Move Arm Right\r\n"); - pc.printf("3) Move Arm Up\r\n"); - pc.printf("4) Move Arm Down\r\n"); - pc.printf("q) Exit \r\n"); - pc.printf("Please make a choice => \r\n"); -} //Send arm to mechanical limits, and set encoder to 0. Then send arm to starting position. void calibrate_arm(void) @@ -680,10 +655,74 @@ //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() { - //analyze emg (= velocity, averages?) + /* + + //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) + + //analyze emg (= velocity) + if (biceps>triceps && biceps > 0.1) + xdir = 0; + xpower = biceps; + else if (triceps>biceps && triceps>0.1) + xdir = 1; + xpower = triceps; + else + xpower=0; + + if (flexor>extensor && flexor > 0.1){ + ydir = 0; + ypower = flexor; + } + else if (extensor>flexor && extensor > 0.1){ + ydir = 1; + ypower = extensor; + } + else + ypower = 0; + + //We have power: the longer a signal is active, the further you go. So integrate to determine reference position + dx = xpower * CONTROL_RATE; + dy = ypower * CONTROL_RATE; - //calculate reference position based on the average emg (integrate) + //But: direction! Sum dx and dy but make sure xdir and ydir are considered. + if (xdir>0) + x += dx; + else + x += -dx; + + if (ydir>0) + y += dy; + else + y += -dy; + + //now we have x and y -> reference position. + + //Set limits to the error! + //lower limit: Negative error not allowed to go further. + if (theta1 < limitangle) + if (error1 > 0) + error1 = error1; + else + error1 = 0; + if (theta2 < limitangle) + same as above + + //upper limit: Positive error not allowed to go further + if (theta1 > limitangle) + if (error1 < 0 ) + error1 = error1; + else + error1 = 0; + if (theta2 > limitangle) + same as above + + */ + //Current position - Encoder counts -> current angle -> Forward Kinematics rpc=(2*PI)/ENCODER1_CPR; //radians per count (resolution) - 2pi divided by 4200 theta1 = Encoder1.getPulses() * rpc; //multiply resolution with number of counts @@ -691,40 +730,30 @@ current_x = l1 * cos(theta1) + l2 * cos(theta1 + theta2); current_y = l1 * sin(theta1) + l2 * sin(theta1 + theta2); - //pc.printf("Calculated current position: x = %f and y = %f \r\n",current_x,current_y); - - - //pc.printf("X is %f and Y is %f \r\n",x,y); //calculate error (refpos-currentpos) currentpos = forward kinematics x_error = x - current_x; y_error = y - current_y; - //pc.printf("X error is %f and Y error is %f \r\n",x_error,y_error); //inverse kinematics (refpos to refangle) costheta2 = (pow(x,2) + pow(y,2) - pow(l1,2) - pow(l2,2)) / (2*l1*l2) ; - sintheta2 = sqrt( 1 - pow(costheta2,2) ); + sintheta2 = sqrt( abs( 1 - pow(costheta2,2) ) ); - //pc.printf("costheta2 = %f and sostheta2 = %f \r\n",costheta2,sostheta2); dtheta2 = atan2(sintheta2,costheta2); costheta1 = ( x * (l1 + l2 * costheta2) + y * l2 * sintheta2 ) / ( pow(x,2) + pow(y,2) ); - sintheta1 = sqrt( 1 - pow(costheta1,2) ); - - //pc.printf("costheta1 = %f and sostheta1 = %f \r\n",costheta1,sostheta1); + sintheta1 = sqrt( abs( 1 - pow(costheta1,2) ) ); dtheta1 = atan2(sintheta1,costheta1); //Angle error - m1_error = dtheta1-theta1; m2_error = dtheta2-theta2; - //pc.printf("m1 error is %f and m2 error is %f \r\n",m1_error,m2_error); //PID controller @@ -847,6 +876,34 @@ pc.printf("[H"); // cursor to home } + +void controlMenu() +{ + //Title Box + pc.putc(201); + for(int j=0;j<33;j++){ + pc.putc(205); + } + pc.putc(187); + pc.printf("\n\r"); + pc.putc(186); pc.printf(" Control Menu "); pc.putc(186); + pc.printf("\n\r"); + pc.putc(200); + for(int k=0;k<33;k++){ + pc.putc(205); + } + pc.putc(188); + + pc.printf("\n\r"); + //endbox + pc.printf("1) Move Arm Left\r\n"); + pc.printf("2) Move Arm Right\r\n"); + pc.printf("3) Move Arm Up\r\n"); + pc.printf("4) Move Arm Down\r\n"); + pc.printf("q) Exit \r\n"); + pc.printf("Please make a choice => \r\n"); +} + void caliMenu(){ //Title Box