Arthur de Lange / Mbed 2 deprecated Kinematics_Project_G5

Kinematica

Dependencies:   Matrix MatrixMath mbed

main.cpp@0:cc8312f7a22d, 2017-10-26 (annotated)

Committer:
arthurdelange
Date:
Thu Oct 26 10:43:31 2017 +0000
Revision:
0:cc8312f7a22d
Begin

Who changed what in which revision?

UserRevisionLine numberNew contents of line
arthurdelange 0:cc8312f7a22d 1 #include "mbed.h"
arthurdelange 0:cc8312f7a22d 2 #include "Matrix.h"
arthurdelange 0:cc8312f7a22d 3 #include "MatrixMath.h"
arthurdelange 0:cc8312f7a22d 4
arthurdelange 0:cc8312f7a22d 5 // constantes
arthurdelange 0:cc8312f7a22d 6 float L1 = 0.250;
arthurdelange 0:cc8312f7a22d 7 float L2 = 0.355;
arthurdelange 0:cc8312f7a22d 8 float L3 = 0.150;
arthurdelange 0:cc8312f7a22d 9
arthurdelange 0:cc8312f7a22d 10 float T = 0.5; // Sample tijd EMG meting
arthurdelange 0:cc8312f7a22d 11
arthurdelange 0:cc8312f7a22d 12 Matrix Jda(2,3);
arthurdelange 0:cc8312f7a22d 13
arthurdelange 0:cc8312f7a22d 14 // Reference position
arthurdelange 0:cc8312f7a22d 15 float q1_1 = 0;
arthurdelange 0:cc8312f7a22d 16 float q2_1 = 0;
arthurdelange 0:cc8312f7a22d 17 float q1_2
arthurdelange 0:cc8312f7a22d 18 float q2_2
arthurdelange 0:cc8312f7a22d 19
arthurdelange 0:cc8312f7a22d 20 // EMG Input
arthurdelange 0:cc8312f7a22d 21 float vx
arthurdelange 0:cc8312f7a22d 22 float vy
arthurdelange 0:cc8312f7a22d 23
arthurdelange 0:cc8312f7a22d 24
arthurdelange 0:cc8312f7a22d 25 // Calculating q_set
arthurdelange 0:cc8312f7a22d 26 void Kinematics ()
arthurdelange 0:cc8312f7a22d 27 {
arthurdelange 0:cc8312f7a22d 28 q1_1 = q1_2;
arthurdelange 0:cc8312f7a22d 29 q2_1 = q2_2;
arthurdelange 0:cc8312f7a22d 30
arthurdelange 0:cc8312f7a22d 31 Jda << -L1*cos(q1_1)-L2*sin(q2_1)) << -L2*sin(q2_1)
arthurdelange 0:cc8312f7a22d 32 <<-L1*sin(q1_1)+L2*cos(q2_1)+L3) << L2*cos(q2_1))+L3;
arthurdelange 0:cc8312f7a22d 33
arthurdelange 0:cc8312f7a22d 34
arthurdelange 0:cc8312f7a22d 35
arthurdelange 0:cc8312f7a22d 36 int main()
arthurdelange 0:cc8312f7a22d 37 {
arthurdelange 0:cc8312f7a22d 38
arthurdelange 0:cc8312f7a22d 39 }