Gerhard Berman
All working, but Position_x and Position_y don't start at value 0, which causes spontaneous motor action.
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of
prog_inversekin
by 
Gerhard Berman
Revision 18:d2cfd07ae88a, committed 2016-10-24
- Comitter:
- GerhardBerman
- Date:
- Mon Oct 24 09:07:07 2016 +0000
- Parent:
- 17:91d20d362e72
- Commit message:
- All working, but Position_x and Position_y don't start at value 0, which causes spontaneous motor action.
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 91d20d362e72 -r d2cfd07ae88a main.cpp
--- a/main.cpp Fri Oct 21 10:39:46 2016 +0000
+++ b/main.cpp Mon Oct 24 09:07:07 2016 +0000
@@ -15,6 +15,7 @@
after testing to get proper action.
- Set angle boundaries!!
- Set robot constants (lengths etc.)
+- Set EMGgain and thresholds
*/
//set pins
@@ -39,6 +40,14 @@
//HIDScope scope(4);
//set initial conditions
+float biceps_l = 0;
+float biceps_r = 0;
+float ReferencePosition_x = 0;
+float ReferencePosition_y = 0;
+float Position_x = 0;
+float Position_y = 0;
+float PositionError_x = 0;
+float PositionError_y = 0;
float error1_prev = 0;
float error2_prev = 0;
float IntError1 = 0;
@@ -50,16 +59,18 @@
float q2_dot = 0.0;
float motorValue1 = 0.0;
float motorValue2 = 0.0;
-
-//set constant or variable values
int counts1 = 0;
int counts2 = 0;
int counts1Prev = 0;
int counts2Prev = 0;
+
+//set constant or variable values
+float EMGgain = 1.0;
double DerivativeCounts;
float x0 = 1.0;
float L0 = 1.0;
float L1 = 1.0;
+float L2 = 1.0;
float dx;
float dy;
float dy_stampdown = 0.05; //5 cm movement downward to stamp
@@ -99,63 +110,98 @@
float Encoder1Position = counts1/resolution; //position in radians, encoder axis
float Encoder2Position = counts2/resolution;
- float Motor1Position = Encoder1Position*inverse_gear_ratio; //position in radians, motor axis
- float Motor2Position = Encoder2Position*inverse_gear_ratio;
+ q1Out = Encoder1Position*inverse_gear_ratio; //position in radians, motor axis
+ q2Out = Encoder2Position*inverse_gear_ratio;
+ 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 velocity vector v = (Pe*- Pe) = [0; dx; dy] from EMG
- float biceps1 = !button1.read();
- float biceps2 = !button2.read();
- if (biceps1 > 0 && biceps2 > 0){
+ biceps_l = EMGgain * !button1.read(); //emg0.read(); //velocity or reference position change, EMG with a gain
+ biceps_r = EMGgain * !button2.read(); //emg1.read();
+ if (biceps_l > 0 && biceps_r > 0){
//both arms activated: stamp moves down
//led1 = 1;
//led2 = 1;
- dx = 0;
- dy = 1; //dy_stampdown; //into stamping vertical position?? ~the stamp down action
+ ReferencePosition_x = ReferencePosition_x;
+ ReferencePosition_y = ReferencePosition_y - dy_stampdown; //into stamping vertical position ~the stamp down action
+
+ /*
+ wait(1); //lift stamp after stamping
+ ReferencePosition_y = ReferencePosition_y - dy_stampdown;
+ PositionError_y = ReferencePosition_y - Position_y; //Position error in dx,dy
+ 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 left
+ //led1 = 1;
+ //led2 = 0;
+ ReferencePosition_x = ReferencePosition_x - biceps_l;
+ ReferencePosition_y = ReferencePosition_y;
/*
- wait(1);
- dy = -(dy_stampdown); //reset vertical position
+ 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 (biceps1 > 0 && biceps2 <= 0){
- //arm 1 activated, move left
- //led1 = 1;
- //led2 = 0;
- dx = 1; //-biceps1;
- dy = 0;
+ else if (biceps_l <= 0 && biceps_r > 0){
+ //arm 1 activated, move right
+ //led1 = 0;
+ //led2 = 1;
+ ReferencePosition_x = ReferencePosition_x + biceps_r;
+ ReferencePosition_y = 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 (biceps1 <= 0 && biceps2 > 0){
- //arm 1 activated, move left
- //led1 = 0;
- //led2 = 1;
- dx = 1; //biceps2;
- dy = 0;
- 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;
- dx=0;
- dy=0;
- q1_dotOut = 0;
- q2_dotOut = 0;
+ ReferencePosition_x = ReferencePosition_x;
+ ReferencePosition_y = ReferencePosition_y;
}
- //get joint angles change q_dot = Jpseudo * TwistEndEff (Matlab)
+ //calculate joint angle differences
+ 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)));
+
//update joint angles
- q1Out = q1Out + q1_dotOut; //in radians
- q2Out = q2Out + q2_dotOut;
+ //q1Out = q1Out + q1_dotOut; //in radians
+ //q2Out = q2Out + q2_dotOut;
pc.baud(115200);
+ pc.printf("Reference_x: %f \r\n", ReferencePosition_x);
+ pc.printf("Position_x: %f \r\n", Position_x);
+ pc.printf("PositionError_x: %f \r\n", PositionError_x);
+ pc.printf("dx: %f \r\n", dx);
+ pc.printf("q1dotOut: %f \r\n", q1_dotOut);
+ pc.printf("q1Out: %f \r\n", q1Out);
+
+ pc.printf("Reference_y: %f \r\n", ReferencePosition_y);
+ pc.printf("Position_y: %f \r\n", Position_y);
+ pc.printf("PositionError_y: %f \r\n", PositionError_y);
+ pc.printf("dy: %f \r\n", dy);
+ pc.printf("q2dotOut: %f \r\n", q2_dotOut);
+ pc.printf("q2Out: %f \r\n", q2Out);
+ /*
pc.printf("dx: %f \r\n", dx);
pc.printf("dy: %f \r\n", dy);
pc.printf("q1: %f \r\n", q1Out);
@@ -165,19 +211,21 @@
pc.printf("Counts1: %f \r\n", counts1);
pc.printf("Encoder1: %f \r\n", Encoder1Position);
- pc.printf("Motor1: %f \r\n", Motor1Position);
+ 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", Motor2Position);
+ pc.printf("Motor2: %f \r\n", q2Out);
+ */
}
+
void FeedForwardControl1(float q1_dot, float q2_dot, float &motorValue1Out, float &motorValue2Out){
//float Encoder1Position = counts1/resolution; //position in radians, encoder axis
//float Position1 = Encoder1Position*inverse_gear_ratio; //position in radians, motor axis
// linear feedback control
- float error1 = q1_dot; //referencePosition1 - Position1; // proportional error in radians
- float error2 = q2_dot; //referencePosition1 - Position1; // proportional error in radians
+ float error1 = q1_dot; //referencePosition1 - Position1; // proportional angular error in radians
+ float error2 = q2_dot; //referencePosition1 - Position1; // proportional angular error in radians
float Kp = 1; //potMeter2.read();
float IntError1 = IntError1 + error1*t_sample; // integrated error in radians
@@ -211,14 +259,14 @@
error1_prev = error1;
error2_prev = error1;
- float biceps1 = !button1.read();
- float biceps2 = !button2.read();
+ //float biceps_l = !button1.read();
+ //float biceps_r = !button2.read();
/*
scope.set(0,q1);
scope.set(1,q2);
- scope.set(2,biceps1);
- scope.set(3,biceps2);
+ scope.set(2,biceps_l);
+ scope.set(3,biceps_r);
scope.send();
*/
}
@@ -292,7 +340,7 @@
//int led2val = led2.read();
pc.baud(115200);
pc.printf("Test putty IK");
- MeasureTicker.attach(&MeasureTicker_act, 1.0f);
+ MeasureTicker.attach(&MeasureTicker_act, 0.1f);
bqc.add(&bq1).add(&bq2);
while(1)
@@ -302,6 +350,8 @@
MeasureAndControl();
counts1 = Encoder1.getPulses(); // gives position of encoder
counts2 = Encoder2.getPulses(); // gives position of encoder
+ pc.printf("counts1: %i \r\n", counts1);
+ pc.printf("counts2: %i \r\n", counts2);
}
/*
if (BiQuadTicker_go){