Lennart van Hoorne / Mbed OS Function_IK

Functie Inverse Kinematica

Dependencies:   HIDScope MODSERIAL

main.cpp@9:24c0d83dec3c, 2018-11-01 (annotated)

Committer:
LennartvanHoorne
Date:
Thu Nov 01 08:52:04 2018 +0000
Revision:
9:24c0d83dec3c
Parent:
8:297208ebcfa6 
nog beter!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
LennartvanHoorne 7:9a389585a9e9 1 # define M_PI 3.14159265358979323846 /* pi */
LennartvanHoorne 7:9a389585a9e9 2
LennartvanHoorne 0:4fc925102d99 3 #include "math.h"
LennartvanHoorne 7:9a389585a9e9 4 #include "stdio.h"
LennartvanHoorne 7:9a389585a9e9 5 #include "iostream"
LennartvanHoorne 0:4fc925102d99 6
LennartvanHoorne 0:4fc925102d99 7 // Constantes
LennartvanHoorne 8:297208ebcfa6 8 //L1 = 0.35;
LennartvanHoorne 8:297208ebcfa6 9 //L2 = 0.30;
LennartvanHoorne 7:9a389585a9e9 10
LennartvanHoorne 7:9a389585a9e9 11 // Variables
LennartvanHoorne 7:9a389585a9e9 12 double V_q1;
LennartvanHoorne 0:4fc925102d99 13
LennartvanHoorne 7:9a389585a9e9 14 double invj00;
LennartvanHoorne 7:9a389585a9e9 15 double invj01;
LennartvanHoorne 7:9a389585a9e9 16 double invj10;
LennartvanHoorne 7:9a389585a9e9 17 double invj11;
LennartvanHoorne 5:205f1de452de 18
LennartvanHoorne 8:297208ebcfa6 19 double vx;
LennartvanHoorne 8:297208ebcfa6 20 double vy;
LennartvanHoorne 8:297208ebcfa6 21
LennartvanHoorne 8:297208ebcfa6 22 double T;
LennartvanHoorne 8:297208ebcfa6 23
LennartvanHoorne 7:9a389585a9e9 24 double q1_new;
LennartvanHoorne 1:f3afd35a7188 25
LennartvanHoorne 0:4fc925102d99 26 //Hoe werken de variabls?
LennartvanHoorne 0:4fc925102d99 27 // q1 en q2 komen uit de Encoder dus die worden geleverd
LennartvanHoorne 0:4fc925102d99 28 // 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)
LennartvanHoorne 0:4fc925102d99 29
LennartvanHoorne 7:9a389585a9e9 30 void IK(double &q1, double &q2, double vx, double vy, double T) {
LennartvanHoorne 0:4fc925102d99 31
LennartvanHoorne 7:9a389585a9e9 32 double Q2;
LennartvanHoorne 7:9a389585a9e9 33 double Q2_new;
LennartvanHoorne 7:9a389585a9e9 34 double V_Q2;
LennartvanHoorne 7:9a389585a9e9 35
LennartvanHoorne 7:9a389585a9e9 36 Q2 = q2 + 90*(M_PI/180);
LennartvanHoorne 7:9a389585a9e9 37
LennartvanHoorne 7:9a389585a9e9 38 invj00 = sin(q1 + Q2)/(0.35*cos(q1 + Q2)*sin(q1) - 0.35*sin(q1 + Q2)*cos(q1));
LennartvanHoorne 7:9a389585a9e9 39 invj01 = -cos(q1 + Q2)/(0.35*cos(q1 + Q2)*sin(q1) - 0.35*sin(q1 + Q2)*cos(q1));
LennartvanHoorne 7:9a389585a9e9 40 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));
LennartvanHoorne 7:9a389585a9e9 41 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));
LennartvanHoorne 7:9a389585a9e9 42
LennartvanHoorne 7:9a389585a9e9 43 V_q1 = invj00*vx + invj01*vy;
LennartvanHoorne 7:9a389585a9e9 44 V_Q2 = invj10*vx + invj11*vy;
LennartvanHoorne 0:4fc925102d99 45
LennartvanHoorne 1:f3afd35a7188 46 // Numerical Integral to make it position controlled
LennartvanHoorne 7:9a389585a9e9 47 q1_new = q1 + V_q1*T;
LennartvanHoorne 1:f3afd35a7188 48 q1 = q1_new;
LennartvanHoorne 7:9a389585a9e9 49 Q2_new = Q2 + V_Q2*T;
LennartvanHoorne 7:9a389585a9e9 50 Q2 = Q2_new;
LennartvanHoorne 7:9a389585a9e9 51
LennartvanHoorne 7:9a389585a9e9 52 q2 = Q2 - 90*(M_PI/180);
LennartvanHoorne 1:f3afd35a7188 53 }
LennartvanHoorne 3:2a437a6ef149 54
LennartvanHoorne 0:4fc925102d99 55
LennartvanHoorne 6:0256de2854d6 56 int main(){
LennartvanHoorne 7:9a389585a9e9 57 double q1 = 0;
LennartvanHoorne 7:9a389585a9e9 58 double q2 = 0;
LennartvanHoorne 7:9a389585a9e9 59 double i;
LennartvanHoorne 7:9a389585a9e9 60
LennartvanHoorne 7:9a389585a9e9 61 for(i = 0; i <= 30; i++)
LennartvanHoorne 7:9a389585a9e9 62 {
LennartvanHoorne 7:9a389585a9e9 63 IK(q1,q2, 0.05, 0, 0.1);
LennartvanHoorne 7:9a389585a9e9 64 printf("q1 = %f, q2 = %f\n",q1,q2);
LennartvanHoorne 7:9a389585a9e9 65 }
LennartvanHoorne 3:2a437a6ef149 66 }