State machine
Dependencies: mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter
Diff: inverse_kinematics.h
- Revision:
- 51:e0e4d7e3de93
- Parent:
- 50:5d2176b93a65
diff -r 5d2176b93a65 -r e0e4d7e3de93 inverse_kinematics.h --- a/inverse_kinematics.h Mon Nov 05 10:14:31 2018 +0000 +++ b/inverse_kinematics.h Mon Nov 05 16:03:39 2018 +0000 @@ -32,9 +32,9 @@ float q2 = PI - alfa; // Angle between L1 and L2 // Delta < 175 graden SAFETY LIMITATION - if (r > max_r_angle) // If delta is larger than 175 degrees then r is this + if (r > max_r_length) // If delta is larger than 175 degrees then r is this { - r = max_r_angle; + r = max_r_length; } // Calculation of motor_angle2 @@ -43,7 +43,7 @@ dest_sec_angle = zeta - beta; - // Determining angle delta for safety + // Determining angle delta for safety (not necessary for calculation, for debugging purposes) float J1x_0 = L0 + L6 + L1*cos(dest_sec_angle); // x-coordinate of joint 1 in frame 0 float J1y_0 = L1*sin(dest_sec_angle); // y-coordinate of joint 1 in frame 0 @@ -52,7 +52,7 @@ // Implementing stops for safety - // 45 < Motor_angle1 < 70 graden + // 45 < Motor_angle1 < 160 graden if (dest_main_angle < main_arm_min_angle) // If motor_angle is smaller than 45 degrees { dest_main_angle = main_arm_min_angle;