Gerhard Berman
New reference frame: y=0 is now at table height.
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of
prog_forwardkin_feedback_copy
by 
Elfi Hofmeijer
Diff: main.cpp
- Revision:
- 37:1360978f49ba
- Parent:
- 36:72f0913c5460
--- a/main.cpp Thu Nov 03 09:41:09 2016 +0000
+++ b/main.cpp Thu Nov 03 10:34:51 2016 +0000
@@ -143,143 +143,23 @@
//get joint positions q feedback from encoder
float Encoder1Position = counts1/resolution; //angular position in radians, encoder axis
- float Encoder2Position = counts2/resolution;
+ float Encoder2Position = -1*counts2/resolution;
q1Out = q1start + Encoder1Position*inverse_gear_ratio; //angular position in radians, motor axis
q2Out = q2start + Encoder2Position*inverse_gear_ratio;
-
- /*
- //get end effector position feedback with Brockett
- float Position_x = ((L2 + x0)*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - L0*sin(q1) + (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) - cos(q1)*(L1*sin(q1) + L1*cos(q1)*sin(q2) - L1*cos(q2)*sin(q1)) - sin(q1)*(L1*cos(q1)*cos(q2) - L1*cos(q1) + L1*sin(q1)*sin(q2))); //calculate end effector x-position from motor angles with Brockett, rx
- float Position_y = (L0 - (L2 + x0)*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) - L0*cos(q1) - cos(q1)*(L1*cos(q1)*cos(q2) - L1*cos(q1) + L1*sin(q1)*sin(q2)) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*(L1*sin(q1) + L1*cos(q1)*sin(q2) - L1*cos(q2)*sin(q1))); //calculate end effector y-position from motor angles with Brockett, ry
- */
+
//get end effector position feedback with trigonometry
Position_x = (L1*sin(q1) + L2*sin(q1+q2));
Position_y = (L1*cos(q1) + L2*cos(q1+q2)) + TowerHeight - StampHeight;
- //float PositionVector = sqrt(pow(Position_x,2)+pow(Position_y,2));
-
- /*
- if (Position_y < (0.5*TowerHeight)){
- wait(1.0);
- ReferencePosition_ynew = L1 + TowerHeight - StampHeight; //Reset vertical position after stamping
- }
- else{
- */
- //get velocity vector v = (Pe*- Pe) = [0; dx; dy] from EMG
- biceps_l = !button1.read() * EMGgain; //emg0.read(); //velocity or reference position change, EMG with a gain
- biceps_r = !button2.read() * EMGgain; //emg1.read();
- if (biceps_l > 0 && biceps_r > 0){
- //both arms activated: stamp moves down
- //led1 = 1;
- //led2 = 1;
- ReferencePosition_xnew = ReferencePosition_x;
- ReferencePosition_ynew = y_stampdown; //ReferencePosition_y - dy_stampdown; //into stamping vertical position ~the stamp down action
-
- }
- else if (biceps_l > 0 && biceps_r <= 0){
- //arm 1 activated, move left
- //led1 = 1;
- //led2 = 0;
- ReferencePosition_xnew = ReferencePosition_x - 0.2; //biceps_l;
- ReferencePosition_ynew = y_stampup; //ReferencePosition_y;
- /*
- PositionError_x = ReferencePosition_x - Position_x; //Position error in dx,dy
- PositionError_y = ReferencePosition_y - Position_y; //Position error in dx,dy
-
- dx = PositionError_x;
- dy = PositionError_y;
- q1_dotOut = dy*(((x0 + L1*cos(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (x0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1))) - dx*(((L0 + L1*sin(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (L0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)));
- q2_dotOut = dy*((x0*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(L1*L1*pow(cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - ((x0 + L1*cos(q1))*(pow(L0,2) + pow(x0,2) + 1))/(pow(L1*cos(q1),2)) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - dx*((L0*(L0*L0+L1*sin(q1)*L0+x0*x0+L1*cos(q1)*x0+1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1))-((L0 + L1*sin(q1))*(L0*L0 + x0*x0 + 1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1)));
- */
- }
- else if (biceps_l <= 0 && biceps_r > 0){
- //arm 1 activated, move right
- //led1 = 0;
- //led2 = 1;
- ReferencePosition_xnew = ReferencePosition_x + 0.2; //biceps_r;
- ReferencePosition_ynew = y_stampup; //ReferencePosition_y;
- /*PositionError_x = ReferencePosition_x - Position_x; //Position error in dx,dy
- PositionError_y = ReferencePosition_y - Position_y; //Position error in dx,dy
-
- dx = PositionError_x;
- dy = PositionError_y;
- q1_dotOut = dy*(((x0 + L1*cos(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (x0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1))) - dx*(((L0 + L1*sin(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (L0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)));
- q2_dotOut = dy*((x0*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(L1*L1*pow(cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - ((x0 + L1*cos(q1))*(pow(L0,2) + pow(x0,2) + 1))/(pow(L1*cos(q1),2)) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - dx*((L0*(L0*L0+L1*sin(q1)*L0+x0*x0+L1*cos(q1)*x0+1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1))-((L0 + L1*sin(q1))*(L0*L0 + x0*x0 + 1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1)));
-*/
- }
- else{
- //led1 = 0;
- //led2 = 0;
- ReferencePosition_xnew = ReferencePosition_x;
- ReferencePosition_ynew = y_stampup; //ReferencePosition_y; //
- }
- // }
-
- float PointPositionArm2_x = ReferencePosition_x;
- float PointPositionArm2_y = ReferencePosition_y - TowerHeight + StampHeight;
- float PointVectorArm2 = sqrt(pow(PointPositionArm2_x,2)+pow(PointPositionArm2_y,2));
-
- //check position boundaries
- if (PointVectorArm2 > maxStampDistance){
- ReferencePosition_x = ReferencePosition_x - 0.1;
- ReferencePosition_y = y_stampup;
- pc.printf("Target too far! \r\n");
- }
- else {
- ReferencePosition_x = ReferencePosition_xnew;
- ReferencePosition_y = ReferencePosition_ynew;
- }
-
- //calculate reference joint angles for the new reference position
- float alpha = atan(PointPositionArm2_y/PointPositionArm2_x);
- float beta = acos((L2*L2-L1*L1-pow(PointVectorArm2,2))/(-2*L1*PointVectorArm2));
- q1_refOutNew = PI/2 - (alpha+beta);
- q2_refOutNew = PI - asin(PointVectorArm2*sin(beta)/L2);
-
- //check angle boundaries
- if (q1_refOutNew > q1_refOutMin && q1_refOutNew < q1_refOutMax){
- q1_refOut = q1_refOutNew;
- }
- else {
- q1_refOut = q1_refOut;
- }
- if (q2_refOutNew > q2_refOutMin && q2_refOutNew < q2_refOutMax){
- q2_refOut = q2_refOutNew;
- }
- else {
- q2_refOut = q2_refOut;
- }
-
- //update joint angles
- //q1Out = q1Out + q1_dotOut; //in radians
- //q2Out = q2Out + q2_dotOut;
-
+
pc.baud(115200);
+ pc.printf("Counts1: % f ", counts1);
+ pc.printf("Counts2: % f ", counts2);
pc.printf("posX: %f ",Position_x);
pc.printf("posY: %f ",Position_y);
- pc.printf("refX: %f ",ReferencePosition_xnew);
- pc.printf("refY: %f ",ReferencePosition_ynew);
pc.printf("q1: %f ", q1Out);
- pc.printf("q1ref: %f ", q1_refOut);
- pc.printf("q2: %f ", q2Out);
- pc.printf("q2ref: %f ", q2_refOut);
-
- /*
- pc.printf("dx: %f \r\n", dx);
- pc.printf("dy: %f \r\n", dy);
- pc.printf("q1: %f \r\n", q1Out);
- pc.printf("q1_dot: %f \r\n", q1_dotOut);
- pc.printf("q2: %f \r\n", q2Out);
- pc.printf("q2_dot: %f \r\n", q2_dotOut);
-
- pc.printf("Counts1: %f \r\n", counts1);
- pc.printf("Encoder1: %f \r\n", Encoder1Position);
- pc.printf("Motor1: %f \r\n", q1Out);
- pc.printf("Counts2: %f \r\n", counts2);
- pc.printf("Encoder2: %f \r\n", Encoder2Position);
- pc.printf("Motor2: %f \r\n", q2Out);
- */
-
+ pc.printf("q2: %f \r\n", q2Out);
+
}
@@ -413,8 +293,8 @@
// the joint positions should be, and controls the motor with
// a Feedback controller. This is called from a Ticker.
GetReferenceKinematics1(q1, q2, q1_ref, q2_ref);
- FeedbackControl1( q1_ref, q2_ref, q1, q2, motorValue1, motorValue2);
- SetMotor1(motorValue1, motorValue2);
+ //FeedbackControl1( q1_ref, q2_ref, q1, q2, motorValue1, motorValue2);
+ //SetMotor1(motorValue1, motorValue2);
}
void TimeTrackerF(){
@@ -442,9 +322,9 @@
//int led2val = led2.read();
pc.baud(115200);
pc.printf("Test putty IK");
- counts1 = Encoder1.getPulses(); // gives position of encoder
- counts2 = Encoder2.getPulses(); // gives position of encoder
- wait(10.0);
+ // counts1 = Encoder1.getPulses(); // gives position of encoder
+ // counts2 = Encoder2.getPulses(); // gives position of encoder
+ //wait(10.0);
MeasureTicker.attach(&MeasureTicker_act, 0.1f);
bqc.add(&bq1).add(&bq2);
@@ -457,8 +337,8 @@
MeasureAndControl();
counts1 = Encoder1.getPulses(); // gives position of encoder
counts2 = Encoder2.getPulses(); // gives position of encoder
- //pc.printf("counts1: %i ", counts1);
- //pc.printf("counts2: %i \r\n", counts2);
+ pc.printf("counts1main: %i ", counts1);
+ pc.printf("counts2main: %i ", counts2);
ledblue = 1;
ledgrn = 0;
}