Patrick Zieverink / Mbed 2 deprecated biorobotics_four_scorers_21_10

leuk leuk

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

kinematics.cpp@17:615c5d8b3710, 2019-10-11 (annotated)

Committer:
joostbonekamp
Date:
Fri Oct 11 09:51:35 2019 +0000
Revision:
17:615c5d8b3710
Parent:
15:9a1f34bc9958 
Added EMG interface and filtering, using HIDScope to display the result. Removed all errors, code actually compiles now.

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 */