// ------------------------------------------------------- // // Inverse Kinematics for 2R planar manipulator with a closed-form solution // Sangsin Park, Ph. D. // Feb. 12. 2018 // ------------------------------------------------------- // // Motor's constants #define JNT1 OUT_A #define JNT2 OUT_B #define FULL_SPEED (100) // Gear's constants #define turnTableTeeth (56) #define spurTeeth (8) // NB: +Angle yields CCW rotation (with eye on top of turntable) #define gearRatio turnTableTeeth/spurTeeth // 7.0 // Links' constants #define L1 (0.168) #define L2 (0.032) // Global variables float theta1_ik = 0.0; float theta2_ik = 0.0; // Function declarations bool IK_2R_Planar_closed(float px, float py); task main() { // button variables bool orangeBtnPushed = FALSE; bool l_ArrowBtnPushed = FALSE; bool r_ArrowBtnPushed = FALSE; bool greyBtnPushed = FALSE; int cnt_OrangeBtn = 0; int cnt_l_ArrowBtn = 0; int cnt_r_ArrowBtn = 0; // reference joint angles int theta1 = 0; int theta2 = 0; float px = 0.0; float py = 0.0; bool IK_ok = FALSE; PlayTone(TONE_B3, 50); TextOut(0, LCD_LINE1, "Grey BTN Quits"); TextOut(0, LCD_LINE2, "Orange BTN Home"); PosRegEnable(JNT1); // Set Port A current angle as zero [deg] PosRegSetMax(JNT1, 0.4*FULL_SPEED, 0); // Set Port A speed limit (40) and default acceleration (0) PosRegEnable(JNT2); // Set Port B current angle as zero [deg] PosRegSetMax(JNT2, 0.4*FULL_SPEED, 0); // Set Port B speed limit (40) and default acceleration (0) while(greyBtnPushed == FALSE) { greyBtnPushed = ButtonPressed(BTNEXIT, FALSE); orangeBtnPushed = ButtonPressed(BTNCENTER, FALSE); l_ArrowBtnPushed = ButtonPressed(BTNLEFT, FALSE); r_ArrowBtnPushed = ButtonPressed(BTNRIGHT, FALSE); cnt_l_ArrowBtn = ButtonCount(BTNLEFT, FALSE); cnt_r_ArrowBtn = ButtonCount(BTNRIGHT, FALSE); PosRegSetAngle(JNT1, theta1); PosRegSetAngle(JNT2, theta2); TextOut(0, LCD_LINE4, FormatNum("JNT 1: %d", theta1/gearRatio)); TextOut(0, LCD_LINE5, FormatNum("JNT 2: %d", theta2/gearRatio)); // Code here for going to home postion after oragne button is pushed // if(orangeBtnPushed == TRUE) { orangeBtnPushed = FALSE; theta1 = 0; theta2 = 0; } // ----------------------------- // if(r_ArrowBtnPushed == TRUE) { r_ArrowBtnPushed = FALSE; IK_ok = IK_2R_Planar_closed(-0.12, 0.12); if(IK_ok == TRUE) { theta1 = theta1_ik*gearRatio; theta2 = theta2_ik*gearRatio; TextOut(0, LCD_LINE6, "Solution."); } else { TextOut(0, LCD_LINE6, "No Solution."); } } } } bool IK_2R_Planar_closed(float px, float py) { // Code here for solving kinematic equations with the algebraic method // // ------------------------------------------------------------------- // }