Forward Kinematics
Dependencies: MODSERIAL Matrix mbed
Diff: main.cpp
- Revision:
- 4:6db7291caa6d
- Parent:
- 3:c006b3e9a41c
- Child:
- 5:65a609067e14
diff -r c006b3e9a41c -r 6db7291caa6d main.cpp --- a/main.cpp Wed Oct 31 08:28:57 2018 +0000 +++ b/main.cpp Wed Oct 31 13:55:56 2018 +0000 @@ -18,6 +18,8 @@ volatile float motor_angle1; // Current angle of motor 1 (left) based on kinematics [rad] volatile float motor_angle2; // Current angle of motor 2 (right) based on kinematics [rad] +DigitalOut safetyLED(LED_GREEN); // Safety check LED + // Useful stuff Ticker ForwardKinematics_ticker; @@ -68,6 +70,29 @@ float Pe_x = L6 - n*cos(rho); // y-coordinate end-effector float Pe_y = n*sin(rho); // x-coordinate end-effector + + // Implementing stops for safety + // 45 < Motor_angle1 < 70 graden + if (0.785398 < motor_angle1 && motor_angle1 < 1.22173) + { + ForwardKinematics_ticker.detach(); + safetyLED = 0; + } + + // -42 < Motor_angle2 < 85 graden + if (-0.733038 < motor_angle2 && motor_angle2 < 1.48353) + { + ForwardKinematics_ticker.detach(); + safetyLED = 0; + } + + // Delta < 170 graden + if (delta < 2.96706) + { + ForwardKinematics_ticker.detach(); + safetyLED = 0; + } + // NEEDS TO RETURN END-EFFECTOR COORDINATES // return Pe_x; // return Pe_y; @@ -76,7 +101,8 @@ int main() { + safetyLED = 1; while (true) { - ForwardKinematics_ticker.attach(ForwardKinematics,2); + ForwardKinematics_ticker.attach(ForwardKinematics,0.5); } } \ No newline at end of file