Functie Inverse Kinematica
Dependencies: HIDScope MODSERIAL
main.cpp
- Committer:
- LennartvanHoorne
- Date:
- 2018-11-01
- Revision:
- 9:24c0d83dec3c
- Parent:
- 8:297208ebcfa6
File content as of revision 9:24c0d83dec3c:
# define M_PI 3.14159265358979323846 /* pi */ #include "math.h" #include "stdio.h" #include "iostream" // Constantes //L1 = 0.35; //L2 = 0.30; // Variables double V_q1; double invj00; double invj01; double invj10; double invj11; double vx; double vy; double T; double q1_new; //Hoe werken de variabls? // q1 en q2 komen uit de Encoder dus die worden geleverd // This is the function for the Inverse Kinematics We start with a inverse Jacobian so we can determine q_dot (rotation speed of the motors) void IK(double &q1, double &q2, double vx, double vy, double T) { double Q2; double Q2_new; double V_Q2; Q2 = q2 + 90*(M_PI/180); invj00 = sin(q1 + Q2)/(0.35*cos(q1 + Q2)*sin(q1) - 0.35*sin(q1 + Q2)*cos(q1)); invj01 = -cos(q1 + Q2)/(0.35*cos(q1 + Q2)*sin(q1) - 0.35*sin(q1 + Q2)*cos(q1)); invj10 = -(0.3*sin(q1 + Q2) + 0.35*sin(q1))/(0.35*0.3*cos(q1 + Q2)*sin(q1) - 0.35*0.3*sin(q1 + Q2)*cos(q1)); invj11 = (0.3*cos(q1 + Q2) + 0.35*cos(q1))/(0.35*0.3*cos(q1 + Q2)*sin(q1) - 0.35*0.3*sin(q1 + Q2)*cos(q1)); V_q1 = invj00*vx + invj01*vy; V_Q2 = invj10*vx + invj11*vy; // Numerical Integral to make it position controlled q1_new = q1 + V_q1*T; q1 = q1_new; Q2_new = Q2 + V_Q2*T; Q2 = Q2_new; q2 = Q2 - 90*(M_PI/180); } int main(){ double q1 = 0; double q2 = 0; double i; for(i = 0; i <= 30; i++) { IK(q1,q2, 0.05, 0, 0.1); printf("q1 = %f, q2 = %f\n",q1,q2); } }