2nd draft
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed Servo
Fork of robot_mockup by
Diff: main.cpp
- Revision:
- 27:d1814e271a95
- Parent:
- 26:fe3a5469dd6b
- Child:
- 28:743485bb51e4
--- a/main.cpp Sun Oct 18 13:13:07 2015 +0000 +++ b/main.cpp Mon Oct 19 08:34:24 2015 +0000 @@ -228,16 +228,61 @@ //caliMenu(); // Menu function //calibrate_arm(); //Start Calibration - calibrate_emg(); - - //start_control(); //100 Hz control + //calibrate_emg(); + wait(3); + start_control(); //100 Hz control //maybe some stop commands when a button is pressed: detach from timers. //stop_control(); //stop_sampling(); + char c; + pc.printf("entering while loop \r\n"); + while(1) { - + + if( pc.readable() ){ + c = pc.getc(); + switch (c) + { + case '1' : + x = x + 0.01; + //controlMenu(); + //running=false; + break; + + case '2' : + x-=0.01; + //controlMenu(); + //running=false; + break; + + case '3' : + y+=0.01; + //controlMenu(); + //running=false; + break; + + + case '4' : + y-=0.01; + //controlMenu(); + //running=false; + break; + + case 'q' : + pc.printf("Quit"); + //running = false; + break; + }//end switch + 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("PID output: %f and %f \r\n",u1,u2); + pc.printf("----------------------------------------\r\n\n"); + } + //end if } //end of while loop } @@ -282,37 +327,65 @@ } +void controlMenu() +{ + 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) { - red=0; blue=0; //Debug light is purple during arm calibration + pc.printf("Calibrate_arm() entered\r\n"); bool calibrating = true; bool done1 = false; bool done2 = false; + pc.printf("To start arm calibration, press any key =>"); + pc.getc(); + pc.printf("\r\n Calibrating... \r\n"); dir_motor1=1; //cw - dir_motor2=1; //cw - pwm_motor1.write(0.2f); //move upper arm slowly cw - pwm_motor2.write(0.2f); //move forearm slowly cw + dir_motor2=0; //cw + pwm_motor1.write(0.2); //move upper arm slowly cw + while(calibrating){ - + red=0; blue=0; //Debug light is purple during arm calibration + + if(done1==true){ + pwm_motor2.write(0.2); //move forearm slowly cw + } + //when limit switches are hit, stop motor and reset encoder if(shoulder_limit.read() < 0.5){ //polling pwm_motor1.write(0); Encoder1.reset(); done1 = true; + pc.printf("Shoulder Limit hit - shutting down motor 1\r\n"); } if(elbow_limit.read() < 0.5){ //polling pwm_motor2.write(0); Encoder2.reset(); done2 = true; + pc.printf("Elbow Limit hit - shutting down motor 2. \r\n"); } if(done1 && done2){ calibrating=false; //Leave while loop when both limits are reached red=1; blue=1; //Turn debug light off when calibration complete } + + }//end while - }//end while + //TO DO: + //mechanical angle limits -> pulses. If 40 degrees -> counts = floor( 40 / (2*pi/4200) ) + //Encoder1.setPulses(100); //edited QEI library: added setPulses() + //Encoder2.setPulses(100); //edited QEI library: added setPulses() + //pc.printf("Elbow Limit hit - shutting down motor 2. Current pulsecount: %i \r\n",Encoder1.getPulses()); + wait(1); + pc.printf("Arm Calibration Complete\r\n"); } @@ -395,7 +468,7 @@ start_sampling(); muscle=1; - timer.attach(&emg_mvc,0.002); + timer.attach(&emg_mvc,SAMPLE_RATE); wait(5); timer.detach(); @@ -457,24 +530,62 @@ //calculate reference position based on the average emg (integrate) + //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 + theta2 = Encoder2.getPulses() * rpc; + 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) - //inverse kinematics (pos error to angle error) + costheta2 = (pow(x,2) + pow(y,2) - pow(l1,2) - pow(l2,2)) / (2*l1*l2) ; + sintheta2 = sqrt( 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); + + 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 u1=pid(m1_error,m1_kp,m1_ki,m1_kd,m1_e_int,m1_e_prev); //motor 1 u2=pid(m2_error,m2_kp,m2_ki,m2_kd,m2_e_int,m2_e_prev); //motor 2 - keep_in_range(&u1,-1,1); //keep u between -1 and 1, sign = direction, PWM = 0-1 - keep_in_range(&u2,-1,1); + keep_in_range(&u1,-0.6,0.6); //keep u between -1 and 1, sign = direction, PWM = 0-1 + keep_in_range(&u2,-0.6,0.6); //send PWM and DIR to motor 1 dir_motor1 = u1>0 ? 1 : 0; //conditional statement dir_motor1 = [condition] ? [if met 1] : [else 0]], same as if else below. pwm_motor1.write(abs(u1)); //send PWM and DIR to motor 2 - dir_motor2 = u2>0 ? 1 : 0; //conditional statement, same as if else below + dir_motor2 = u2>0 ? 0 : 1; //conditional statement, same as if else below pwm_motor2.write(abs(u2)); /*if(u1 > 0) @@ -545,7 +656,7 @@ //Start control void start_control(void) { - control_timer.attach(&control,SAMPLE_RATE); //100 Hz control + 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;