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?
User | Revision | Line number | New 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 | } |