Knopjes kunnen gebruikt worden om de robot the bedienen
Dependencies: FastPWM MODSERIAL QEI mbed
Diff: main.cpp
- Revision:
- 1:f628ab39b9f4
- Parent:
- 0:f81ca7708f95
--- a/main.cpp Thu Nov 01 11:03:18 2018 +0000 +++ b/main.cpp Thu Nov 01 13:17:13 2018 +0000 @@ -73,7 +73,7 @@ // Reference angles of the starting position double q2_0 = -acos((pow(x0,sq)+pow(y0,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3)); double q1_0 = atan(y0/x0)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x0,sq)+pow(y0,sq))/(2.0*L1*sqrt(pow(x0,sq)+pow(y0,sq)))); -double q2_0_enc = (pi-q2_0) + q1_0; +double q2_0_enc = (pi-q2_0) - q1_0; // -------------------------------------------------------------------- // ---------------Read out encoders------------------------------------ @@ -168,8 +168,8 @@ double makeAngleq2(double x, double y){ double q2 = -acos((pow(x,sq)+pow(y,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3)); //angle of the second joint in setpoint configuration double q1 = atan(y/x)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x,sq)+pow(y,sq))/(2.0*L1*sqrt(pow(x,sq)+pow(y,sq)))); //angle of the first joint in the setpoint configuration - double q2_motor = (pi - q2)+q1; //because q2 represents the angle at joint two and not at the motor a calculation has to be done - q2_diff = -(2.0*(q2_motor - q2_0_enc))/(2.0*pi); //the actual amount of radians that the motor has to turn in total to reach the setpoint + double q2_motor = (pi - q2) - q1; //because q2 represents the angle at joint two and not at the motor a calculation has to be done + q2_diff = (2.0*(q2_motor - q2_0_enc)); //the actual amount of radians that the motor has to turn in total to reach the setpoint return q2_diff; }