RobotArm_ Biorobotics project.
Dependencies: PID QEI RemoteIR Servo mbed
Fork of Biorobotics_Motor_Control by
main.cpp
- Committer:
- jordymorsinkhof
- Date:
- 2017-10-27
- Revision:
- 5:74784836db3d
- Parent:
- 4:c45eaa904b09
File content as of revision 5:74784836db3d:
#include "mbed.h" #include "QEI.h" #include "PID.h" #include "ReceiverIR.h" #include "Servo.h" #include <iostream> #include <string> #include <math.h> ReceiverIR ir_rx(D2); AnalogIn PotMeter1(A0); AnalogIn PotMeter2(A1); InterruptIn Button(PTA4); PwmOut MotorThrottle1(D5); PwmOut MotorThrottle2(D6); DigitalOut MotorDirection1(D4); DigitalOut MotorDirection2(D7); DigitalOut servo(D3); DigitalIn EndSwitch1(D9); DigitalIn EndSwitch2(D8); QEI EncoderMotor1(D12,D13,NC,4200); QEI EncoderMotor2(D10,D11,NC,4200); Ticker ControlTicker; Ticker GetRefTicker; float Interval=0.02f; float pi=3.14159265359; PID controller1(100.0f,0.0f,0.001f, Interval); PID controller2(100.0f,0.0f,0.002f, Interval); //G: elsewhere given values 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]; RawSerial pc(PTB17,PTB16); //Function INPUT: q1, q2. OUTPUT: Xcurr, Ycurr void Brock(float q1, float q2, float& Xcurr, float& Ycurr) { //Position of end-effector in base frame when q1 = q2 = 0 to use in He0(0) float He1[9]; He1[0] = cos(q1 + q2); He1[1] = -sin(q1 +q2); He1[2] = L2*cos(q1 + q2) + L1*cos(q1); He1[3] = sin(q1 + q2); He1[4] = cos(q1 + q2); He1[5] = L2*sin(q1 + q2) + L1*sin(q1); He1[6] = 0; He1[7] = 0; He1[8] = 1; // print He1 to check the matrix /* pc.printf("He1 = [\n\r"); for (int i=0; i<=8; i++){ pc.printf("%.2f ",He1[i]); if (i==2){ pc.printf("\n\r");} else if (i==5){ 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] - 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); Xcurr = He1[2]; Ycurr = He1[5]; //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++){ pc.printf("%.2f ",He1[i]); if (i==2){ pc.printf("\n\r");} else if (i==5){ pc.printf("\n\r");} } pc.printf("]\n\r" ); */ } //Function INPUT: q1, q2, Xset, Yset, Xcurr, Ycurr. OUTPUT: tau1, tau2 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.2; //stiffness //Current position = VARIABLE EXPRESSED IN BASE FRAME 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("Posx , Posy: %f %f \r\n",rxe,rye); //Define transposed Jacobian J_T [3x2] float J_T1 = 1; float J_T2 = ry1; float J_T3 = rx1; float J_T4 = 1; 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); //Define wrench Wspr = (tau Fx Fy) = (rxFx - rxFy Fx Fy) 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); tau1 = J_T1*Wspr1+J_T2*Wspr2+J_T3*Wspr3; tau2 = J_T4*Wspr1+J_T5*Wspr2+J_T6*Wspr3; //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); } void ControlFunction(float AngleRef1,float AngleRef2){ 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(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(); float OutPut2=controller2.compute(); // Direction handling float Direction1=0; float Direction2=0; if(OutPut1<0){ Direction1=1; } if(OutPut2<0){ Direction2=1; } OutPut1=fabs(OutPut1); OutPut2=fabs(OutPut2); // Output schrijven MotorThrottle1.write(OutPut1); MotorThrottle2.write(OutPut2); 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(){ } void LayDown(){ } void RemoteController(){ int bitcount; bitcount = ir_rx.getData(&format, buf, sizeof(buf) * 8); int state = buf[2]; switch(state) { case 22: //1 pc.printf("1\n\r"); break; case 25: //1 pc.printf("2\n\r"); break; case 13: //1 pc.printf("3\n\r"); break; case 12: //1 pc.printf("4\n\r"); break; case 24: //1 pc.printf("5\n\r"); ControlTicker.detach(); MotorThrottle1=0.0f; MotorThrottle2=0.0f; PickUp(); //ControlTicker.attach(&ControlLoop, Interval); break; case 94: //1 pc.printf("6\n\r"); break; case 8: //1 pc.printf("7\n\r"); break; case 28: //1 pc.printf("8\n\r"); ControlTicker.detach(); MotorThrottle1=0.0f; MotorThrottle2=0.0f; LayDown(); //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; /* GYset = GYset + 1; */ //pc.printf("%f\n\r", PosRef2); break; case 21: //1 pc.printf("Onder\n\r"); //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; /* GXset = GXset + 1; */ //pc.printf("%f\n\r", PosRef1); break; case 67: //1 pc.printf("Rechts\n\r"); //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); break; default: break; } } // Give Reference Position void DeterminePosRef(){ 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\r\n"); 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); pc.printf("Homing \r\n"); HomingLoop(); 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.1); } } while(1){} }