Patrick Zieverink / Mbed 2 deprecated biorobotics_four_scorers_21_10

leuk leuk

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

kinematics.cpp@23:35732c84d6ae, 2019-10-21 (annotated)

Committer:
PatrickZieverink
Date:
Mon Oct 21 10:14:36 2019 +0000
Revision:
23:35732c84d6ae
Parent:
17:615c5d8b3710 
PID coefficienten erin geknald

Who changed what in which revision?

UserRevisionLine numberNew contents of line
joostbonekamp 17:615c5d8b3710 1 /*
joostbonekamp 14:4cf17b10e504 2 void kinematics1() //
joostbonekamp 14:4cf17b10e504 3 {
joostbonekamp 14:4cf17b10e504 4 double length_1;
joostbonekamp 15:9a1f34bc9958 5 double theta; //hoek tussen coordinatenstels, hidde vragen
joostbonekamp 14:4cf17b10e504 6 double length_2;
joostbonekamp 14:4cf17b10e504 7
joostbonekamp 14:4cf17b10e504 8 class H_matrix //rows_columns (als je het niet ziet, gebruik dat de onderste rij 0 0 1 is)
joostbonekamp 14:4cf17b10e504 9 {
joostbonekamp 14:4cf17b10e504 10 public:
joostbonekamp 14:4cf17b10e504 11 int h1_1_1 = 1;
joostbonekamp 14:4cf17b10e504 12 int h1_1_2 = 0;
joostbonekamp 14:4cf17b10e504 13 float h1_1_3 = sin(theta*PI/180)*(length_1+length_2); //sinus en cosinus zijn standaard gedefinieërd in radians, maar theta is hier dus in graden
joostbonekamp 14:4cf17b10e504 14 int h1_2_1 = 0;
joostbonekamp 14:4cf17b10e504 15 int h1_2_2 = 1;
joostbonekamp 14:4cf17b10e504 16 float h1_2_3 = cos(theta*PI/180)*(length_1+length_2);
joostbonekamp 14:4cf17b10e504 17 int h1_3_1 = 0;
joostbonekamp 14:4cf17b10e504 18 int h1_3_2 = 0;
joostbonekamp 14:4cf17b10e504 19 int h1_3_3 = 1;
joostbonekamp 14:4cf17b10e504 20 }
joostbonekamp 14:4cf17b10e504 21 H_matrix H;
joostbonekamp 14:4cf17b10e504 22
joostbonekamp 14:4cf17b10e504 23 class Position_vector1 //positie vector gezien vanuit het uiteinde van de complete arm (coördinatiestelsel draait niet mee met de arm)
joostbonekamp 14:4cf17b10e504 24 {
joostbonekamp 14:4cf17b10e504 25 public:
joostbonekamp 14:4cf17b10e504 26 float p1_1_1
joostbonekamp 14:4cf17b10e504 27 float p1_2_1
joostbonekamp 14:4cf17b10e504 28 float p1_3_1
joostbonekamp 14:4cf17b10e504 29 }
joostbonekamp 14:4cf17b10e504 30 class Position_vector2
joostbonekamp 14:4cf17b10e504 31 {
joostbonekamp 14:4cf17b10e504 32 public:
joostbonekamp 14:4cf17b10e504 33 float p2_1_1 = h1_1_1*p1_1_1 + h1_1_2*p1_2_1 + h1_1_3*p1_3_1;
joostbonekamp 14:4cf17b10e504 34 float p2_2_1 = h1_2_1*p1_1_1 + h1_2_2*p1_2_1 + h1_2_3*p1_3_1;
joostbonekamp 14:4cf17b10e504 35 float p2_3_1 = h1_3_1*p1_1_1 + h1_3_2*p1_2_1 + h1_3_3*p1_3_1;
joostbonekamp 14:4cf17b10e504 36 }
joostbonekamp 14:4cf17b10e504 37 }
joostbonekamp 14:4cf17b10e504 38
joostbonekamp 14:4cf17b10e504 39
joostbonekamp 14:4cf17b10e504 40
joostbonekamp 14:4cf17b10e504 41
joostbonekamp 14:4cf17b10e504 42 void kinematics2() //
joostbonekamp 14:4cf17b10e504 43 {
joostbonekamp 14:4cf17b10e504 44 double length_1;
joostbonekamp 14:4cf17b10e504 45 volatile double theta;
joostbonekamp 14:4cf17b10e504 46 volatile double length_2;
joostbonekamp 14:4cf17b10e504 47
joostbonekamp 14:4cf17b10e504 48 class H_matrix2 //rows_columns (als je het niet ziet, gebruik dat de onderste rij 0 0 1 is)
joostbonekamp 14:4cf17b10e504 49 {
joostbonekamp 14:4cf17b10e504 50 public:
joostbonekamp 14:4cf17b10e504 51 int h2_1_1 = 1;
joostbonekamp 14:4cf17b10e504 52 int h2_1_2 = 0;
joostbonekamp 14:4cf17b10e504 53 float h2_1_3 = -sin(theta*PI/180)*(length_1+length_2); //sinus en cosinus zijn standaard gedefinieërd in radians, maar theta is hier dus in graden
joostbonekamp 14:4cf17b10e504 54 int h2_2_1 = 0;
joostbonekamp 14:4cf17b10e504 55 int h2_2_2 = 1;
joostbonekamp 14:4cf17b10e504 56 float h2_2_3 = -cos(theta*PI/180)*(length_1+length_2);
joostbonekamp 14:4cf17b10e504 57 int h2_3_1 = 0;
joostbonekamp 14:4cf17b10e504 58 int h2_3_2 = 0;
joostbonekamp 14:4cf17b10e504 59 int h2_3_3 = 1;
joostbonekamp 14:4cf17b10e504 60 }
joostbonekamp 14:4cf17b10e504 61 H_matrix H;
joostbonekamp 14:4cf17b10e504 62
joostbonekamp 14:4cf17b10e504 63 class Position_vector3 //positie vector gezien vanuit het onderste draaipunt
joostbonekamp 14:4cf17b10e504 64 {
joostbonekamp 14:4cf17b10e504 65 public:
joostbonekamp 14:4cf17b10e504 66 float p3_1_1;
joostbonekamp 14:4cf17b10e504 67 float p3_2_1;
joostbonekamp 14:4cf17b10e504 68 float p3_3_1;
joostbonekamp 14:4cf17b10e504 69 }
joostbonekamp 14:4cf17b10e504 70 class Position_vector4
joostbonekamp 14:4cf17b10e504 71 {
joostbonekamp 14:4cf17b10e504 72 public:
joostbonekamp 14:4cf17b10e504 73 float p4_1_1 = h2_1_1*p3_1_1 + h2_1_2*p3_2_1 + h2_1_3*p3_3_1;
joostbonekamp 14:4cf17b10e504 74 float p4_2_1 = h2_2_1*p1_1_1 + h2_2_2*p1_2_1 + h2_2_3*p3_3_1;
joostbonekamp 14:4cf17b10e504 75 float p4_3_1 = h2_3_1*p1_1_1 + h2_3_2*p1_2_1 + h2_3_3*p3_3_1;
joostbonekamp 14:4cf17b10e504 76 }
joostbonekamp 14:4cf17b10e504 77 H_matrix2 H2
joostbonekamp 17:615c5d8b3710 78 }
joostbonekamp 17:615c5d8b3710 79 */