RobotArm_ Biorobotics project.
Dependencies: PID QEI RemoteIR Servo mbed
Fork of Biorobotics_Motor_Control by
Diff: main.cpp
- Revision:
- 4:c45eaa904b09
- Parent:
- 3:98ea6afa0cf2
diff -r 98ea6afa0cf2 -r c45eaa904b09 main.cpp --- a/main.cpp Thu Oct 26 19:48:03 2017 +0000 +++ b/main.cpp Fri Oct 27 11:45:13 2017 +0000 @@ -31,27 +31,25 @@ float Interval=0.02f; float pi=3.14159265359; -PID controller1(20.0f,0.0f,0.001f, Interval); -PID controller2(20.0f,0.0f,0.002f, Interval); +PID controller1(100.0f,0.0f,0.001f, Interval); +PID controller2(100.0f,0.0f,0.002f, Interval); //G: elsewhere given values -float Gq1 = 0; //from homing -float Gq2 = 0; //from homing -float GXset = 47.5; //from EMG in cm +float GXset = 40.5; //from EMG in cm float GYset = 11; //from EMG in cm //Constant robot parameters const float L1 = 27.5; //length arm 1 in cm const float L2 = 32; //length arm 2 in cm +//Motor angles in radialen +float q1set = 0.25f*pi; +float q2set = -0.5f*pi; RemoteIR::Format format; uint8_t buf[4]; -Serial pc(PTB17,PTB16); - -float PosRef1=1.0f; -float PosRef2=1.0f; +RawSerial pc(PTB17,PTB16); //Function INPUT: q1, q2. OUTPUT: Xcurr, Ycurr void Brock(float q1, float q2, float& Xcurr, float& Ycurr) @@ -71,6 +69,7 @@ He1[8] = 1; // print He1 to check the matrix + /* pc.printf("He1 = [\n\r"); for (int i=0; i<=8; i++){ @@ -82,16 +81,16 @@ pc.printf("\n\r");} } pc.printf("]\n\r" ); - + */ //Translation from base frame to board frame SO FROM NOW ON ALL EXPRESSED IN BOARD FRAME - He1[2] = He1[2] - 12; //12 = x distance from base frame to board frame - He1[5] = He1[5] + 11; //11 = y distance from base frame to board frame + He1[2] = He1[2] - 14.2; //12 = x distance from base frame to board frame + He1[5] = He1[5] + 11.9; //11 = y distance from base frame to board frame - pc.printf("The old value of Xcurr was %f, old Ycurr was %f\n\r", Xcurr, Ycurr); + //pc.printf("The old value of Xcurr was %f, old Ycurr was %f\n\r", Xcurr, Ycurr); Xcurr = He1[2]; Ycurr = He1[5]; - pc.printf("The new value of Xcurr is %f, new Ycurr is %f\n\r", Xcurr, Ycurr); - + //pc.printf("The new value of Xcurr is %f, new Ycurr is %f\n\r", Xcurr, Ycurr); + /* // print He0 to check the matrix pc.printf("\n\r He0 = [\n\r"); for (int i=0; i<=8; i++){ @@ -103,6 +102,7 @@ pc.printf("\n\r");} } pc.printf("]\n\r" ); + */ } @@ -110,118 +110,62 @@ void ErrorToTau(float q1, float q2, float Xcurr, float Ycurr, float Xset, float Yset, float& tau1, float& tau2) { //The parameters k and b are free controller parameters that will determine how fast the arm moves towards the setpoint. - float k = 0.5; //stiffness + float k = 0.2; //stiffness //Current position = VARIABLE EXPRESSED IN BASE FRAME - float rx1 = 0; //x coordinate of joint 1 - float ry1 = 0; //y coordinate of joint 1 - float rx2 = cos(q1)*L1; //x coordinate of joint 2 - float ry2 = sin(q1)*L1; //y coordinate of joint 2 + float rx1 = -14.2; //x coordinate of joint 1 + float ry1 = 11.9; //y coordinate of joint 1 + float rx2 = cos(q1)*L1-14.2; //x coordinate of joint 2 + float ry2 = sin(q1)*L1+11.9; //y coordinate of joint 2 float rxe = rx2 + cos(q1+q2)*L2; //x coordinate of end-effector float rye = ry2 + sin(q1+q2)*L2; //y coordinate of end-effector - pc.printf("Current position: rx1 = %f, ry1 = %f, rx2 = %f, ry2 = %f, rxe = %f, rye = %f\n\r", rx1, ry1, rx2, ry2, rxe, rye); - + //pc.printf("Current position: rx1 = %f, ry1 = %f, rx2 = %f, ry2 = %f, rxe = %f, rye = %f\n\r", rx1, ry1, rx2, ry2, rxe, rye); + //pc.printf("Posx , Posy: %f %f \r\n",rxe,rye); //Define transposed Jacobian J_T [3x2] float J_T1 = 1; - float J_T2 = ry2; - float J_T3 = -rx2; + float J_T2 = ry1; + float J_T3 = rx1; float J_T4 = 1; - float J_T5 = rye; - float J_T6 = -rxe; + float J_T5 = ry2; + float J_T6 = -rx2; //Define spring force float Fsprx = k*(Xset - Xcurr); float Fspry = k*(Yset - Ycurr); - pc.printf("The new value of Fsprx is %f, new Fspry is %f\n\r", Fsprx, Fspry); + //pc.printf("The new value of Fsprx is %f, new Fspry is %f\n\r", Fsprx, Fspry); //Define wrench Wspr = (tau Fx Fy) = (rxFx - rxFy Fx Fy) - float Wspr1 = Ycurr*Fsprx - Xcurr*Fspry; + float Wspr1 = -Ycurr*Fsprx + Xcurr*Fspry; float Wspr2 = Fsprx; float Wspr3 = Fspry; - + //pc.printf("Fx , Fy: %f %f \r\n",Fsprx,Fspry); + //End-effector wrench to forces and torques on the joints using the Jacobian (transposed) - pc.printf("The old value of tau1 was %f, old tau2 was %f\n\r", tau1, tau2); + //pc.printf("The old value of tau1 was %f, old tau2 was %f\n\r", tau1, tau2); tau1 = J_T1*Wspr1+J_T2*Wspr2+J_T3*Wspr3; tau2 = J_T4*Wspr1+J_T5*Wspr2+J_T6*Wspr3; - pc.printf("The new value of tau1 is %f, new tau2 is %f\n\r", tau1, tau2); + //pc.printf("tau Rene: %f %f\r\n",tau1,tau2); + tau1 = Fspry*(L2*cos(q1+q2)+L1*cos(q1)) - Fsprx*(L2*sin(q1+q2)+L1*sin(q1)); + tau2 = Fspry*L2*cos(q1+q2) - Fsprx*L2*sin(q1+q2); + //pc.printf("tau Bram: %f %f\r\n",tau1,tau2); + //pc.printf("The new value of tau1 is %f, new tau2 is %f\n\r", tau1, tau2); + //pc.printf("Tau1, Tau2: %f %f \r\n",tau1,tau2); } - -//Overall function which only needs inputs -void SuperFunction(float q1, float q2, float Xset, float Yset, float& q1set, float& q2set) -{ - //start values - float tau1; //previous values are irrelevant - float tau2; //previous values are irrelevant - float omg1; //previous values are irrelevant - float omg2; //previous values are irrelevant - float Xcurr; //new value is calculated, old replaced - float Ycurr; //new value is calculated, old replaced - - float T = 0.01; //Loop period - float b = 200; //damping - - // Call function to calculate Xcurr and Ycurr from q1 and q2 - Brock(q1, q2, Xcurr, Ycurr); - - // Call function to calculate tau1 and tau2 from X,Ycurr and X,Yset - ErrorToTau(q1, q2, Xcurr, Ycurr, Xset, Yset, tau1, tau2); - - //torque to joint velocity - pc.printf("The old value of omg1 was %f, old omg2 was %f\n\r", omg1, omg2); - omg1 = tau1/b; - omg2 = tau2/b; - pc.printf("The new value of omg1 is %f, new omg2 is %f\n\r", omg1, omg2); +void ControlFunction(float AngleRef1,float AngleRef2){ - //joint velocity to angles q1 and q2, where you define new q1 and q2 based on previous q1 and q2 - q1set = q1set + omg1*T; - q2set = q2set + omg2*T; -} - - -void HomingLoop(){ - - EndSwitch1.mode(PullUp); - EndSwitch2.mode(PullUp); - - MotorThrottle1=0.1f; - MotorThrottle2=0.2f; - - MotorDirection1=1; - MotorDirection2=1; - - bool dummy1=0; - bool dummy2=0; - - while(EndSwitch1.read()|EndSwitch2.read()){ - if((EndSwitch1.read()==0)&&(dummy1==0)){ - MotorThrottle1=0.0f; - dummy1=1; - } - if((EndSwitch2.read()==0)&&(dummy2==0)){ - MotorThrottle2=0.0f; - dummy2=1; - } - } - MotorThrottle1=0.0f; - MotorThrottle2=0.0f; - EncoderMotor1.reset(); - EncoderMotor2.reset(); -} - -void ControlLoop(){ - - float Pos1 = EncoderMotor1.getPulses()/4200.0f*2.0f*pi; //Motorpos. in radians - float Pos2 = EncoderMotor2.getPulses()/4200.0f*2.0f*pi; //Motorpos. in radians - float error1 = PosRef1 - Pos1; - float error2 = PosRef2 - Pos2; + float Angle1 = (EncoderMotor1.getPulses()/4200.0f)*(2.0f*pi)/3.0f; //Motorpos. in radians + float Angle2 = (EncoderMotor2.getPulses()/4200.0f)*(2.0f*pi)/1.8f; //Motorpos. in radians // PID input - controller1.setSetPoint(PosRef1); - controller2.setSetPoint(PosRef2); - - controller1.setProcessValue(Pos1); - controller2.setProcessValue(Pos2); + controller1.setSetPoint(AngleRef1); + controller2.setSetPoint(AngleRef2); + + //pc.printf("ANGLEREF: %f %f \r\n",AngleRef1,AngleRef2); + //pc.printf("ANGLE : %f %f \r\n\n",Angle1,Angle2); + + controller1.setProcessValue(Angle1); + controller2.setProcessValue(Angle2); // PID output float OutPut1=controller1.compute(); @@ -247,6 +191,77 @@ MotorDirection1=Direction1; MotorDirection2=Direction2; } + float omg1; //previous values are irrelevant + float omg2; //previous values are irrelevant +//Overall function which only needs inputs +void LoopFunction() +{ + float q1 = EncoderMotor1.getPulses()/4200.0f*2.0f*pi/3.0f; //Motorpos. in radians + float q2 = EncoderMotor2.getPulses()/4200.0f*2.0f*pi/1.8f; //Motorpos. in radians + + //start values + float tau1; //previous values are irrelevant + float tau2; //previous values are irrelevant + + float Xcurr; //new value is calculated, old replaced + float Ycurr; //new value is calculated, old replaced + + float b = 200; //damping + + // Call function to calculate Xcurr and Ycurr from q1 and q2 + Brock(q1, q2, Xcurr, Ycurr); + + // Call function to calculate tau1 and tau2 from X,Ycurr and X,Yset + ErrorToTau(q1, q2, Xcurr, Ycurr, GXset, GYset, tau1, tau2); + + //torque to joint velocity + //pc.printf("The old value of omg1 was %f, old omg2 was %f\n\r", omg1, omg2); + omg1 = tau1/b; + omg2 = tau2/b; + //pc.printf("The new value of omg1 is %f, new omg2 is %f\n\r", omg1, omg2); + + //joint velocity to angles q1 and q2, where you define new q1 and q2 based on previous q1 and q2 + q1set = q1set + omg1*Interval; + q2set = q2set + omg2*Interval; + + //pc.printf("q1set , q2set: %f %f \r\n",q1set,q2set); + //pc.printf("q1 , q2 : %f %f \r\n",q1,q2); + + ControlFunction(q1set,q2set); +} + + +void HomingLoop(){ + + EndSwitch1.mode(PullUp); + EndSwitch2.mode(PullUp); + + MotorThrottle1=0.2f; + MotorThrottle2=0.1f; + + MotorDirection1=1; + MotorDirection2=1; + + bool dummy1=0; + bool dummy2=0; + + while(EndSwitch1.read()|EndSwitch2.read()){ + if((EndSwitch1.read()==0)&&(dummy1==0)){ + MotorThrottle1=0.0f; + dummy1=1; + } + if((EndSwitch2.read()==0)&&(dummy2==0)){ + MotorThrottle2=0.0f; + dummy2=1; + } + } + MotorThrottle1=0.0f; + MotorThrottle2=0.0f; + EncoderMotor1.reset(0.29/2/pi*4200*3.0f); + EncoderMotor2.reset((3.3715-2*pi)/2/pi*4200*1.8f); +} + + void PickUp(){ @@ -256,7 +271,7 @@ void LayDown(){ - } +} void RemoteController(){ @@ -286,7 +301,7 @@ MotorThrottle1=0.0f; MotorThrottle2=0.0f; PickUp(); - ControlTicker.attach(&ControlLoop, Interval); + //ControlTicker.attach(&ControlLoop, Interval); break; case 94: //1 pc.printf("6\n\r"); @@ -300,38 +315,50 @@ MotorThrottle1=0.0f; MotorThrottle2=0.0f; LayDown(); - ControlTicker.attach(&ControlLoop, Interval); + //ControlTicker.attach(&ControlLoop, Interval); break; case 90: //1 pc.printf("9\n\r"); break; case 70: //1 pc.printf("Boven\n\r"); - PosRef2=PosRef2-0.1f; - pc.printf("%f\n\r", PosRef2); + //PosRef2=PosRef2-0.1f; + /* + GYset = GYset + 1; + */ + //pc.printf("%f\n\r", PosRef2); break; case 21: //1 pc.printf("Onder\n\r"); - PosRef2=PosRef2+0.1f; - pc.printf("%f\n\r", PosRef2); + //PosRef2=PosRef2+0.1f; + /* + GYset = GYset - 1; + */ + //pc.printf("%f\n\r", PosRef2); break; case 68: //1 pc.printf("Links\n\r"); - PosRef1=PosRef1+0.1f; - pc.printf("%f\n\r", PosRef1); + //PosRef1=PosRef1+0.1f; + /* + GXset = GXset + 1; + */ + //pc.printf("%f\n\r", PosRef1); break; case 67: //1 pc.printf("Rechts\n\r"); - PosRef1=PosRef1-0.1f; - pc.printf("%f\n\r", PosRef1); + //PosRef1=PosRef1-0.1f; + /* + GXset = GXset - 1; + */ + //pc.printf("%f\n\r", PosRef1); break; case 64: //1 pc.printf("OK\n\r"); - ControlTicker.detach(); - MotorThrottle1=0.0f; - MotorThrottle2=0.0f; - HomingLoop(); - ControlTicker.attach(&ControlLoop, Interval); + //ControlTicker.detach(); + //MotorThrottle1=0.0f; + //MotorThrottle2=0.0f; + //HomingLoop(); + //ControlTicker.attach(&ControlLoop, Interval); break; default: @@ -341,45 +368,47 @@ - +// Give Reference Position void DeterminePosRef(){ - PosRef1=2*pi*PotMeter1.read(); // Reference in Rad - PosRef2=2*pi*PotMeter2.read(); // Reference in Rad + GXset=40*PotMeter1.read(); // Reference in Rad + GYset=40*PotMeter2.read(); // Reference in Rad + pc.printf("RefX , RefY: %f %f\r\n",GXset,GYset); + float posx=0; + float posy=0; + Brock(EncoderMotor1.getPulses()/4200.0f*2.0f*pi/3.0f,EncoderMotor2.getPulses()/4200.0f*2.0f*pi/1.8f,posx,posy); + pc.printf("posx posy : %f %f\r\n",posx,posy); + pc.printf("q1set,q2set: %f %f\r\n",q1set,q2set); + pc.printf("q1 ,q2 : %f %f\r\n",EncoderMotor1.getPulses()/4200.0f*2.0f*pi/3.0f,EncoderMotor2.getPulses()/4200.0f*2.0f*pi/1.8f); + pc.printf("w1 ,w2 : %f %f\r\n",omg1,omg2); + pc.printf("\n"); + //ControlFunction(GXset,GYset); } int main() { pc.baud(115200); - pc.printf("startup"); + pc.printf("startup\r\n"); - controller1.setInputLimits(0.5f,2.0f*pi); - controller2.setInputLimits(0.5f,2.0f*pi); + controller1.setInputLimits(-2.0f*pi,2.0f*pi); + controller2.setInputLimits(-2.0f*pi,2.0f*pi); controller1.setOutputLimits(-0.15f,0.15f); controller2.setOutputLimits(-0.5f,0.5f); - - float q1set; //becomes new value after calling function again - float q2set; //becomes new value after calling function again - SuperFunction(Gq1, Gq2, GXset, GYset, q1set, q2set); - pc.printf("The new value of q1set is %f, the new q2set is %f\n\r", q1set, q2set); - SuperFunction(Gq1, Gq2, GXset, GYset, q1set, q2set); - pc.printf("The new value of q1set is %f, the new q2set is %f\n\r", q1set, q2set); - - + + pc.printf("Homing \r\n"); HomingLoop(); - - - ControlTicker.attach(&ControlLoop, Interval); - //GetRefTicker.attach(&DeterminePosRef, 0.1f); - + pc.printf("Starting Tickers \r\n"); + ControlTicker.attach(&LoopFunction,Interval); + GetRefTicker.attach(&DeterminePosRef,0.5f); while(1) { if (ir_rx.getState() == ReceiverIR::Received) { - pc.printf("received"); + + RemoteController(); - wait(0.01); + wait(0.1); } }