State machine
Dependencies: mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter
end_effector_control.h@51:e0e4d7e3de93, 2018-11-05 (annotated)
- Committer:
- MAHCSnijders
- Date:
- Mon Nov 05 16:03:39 2018 +0000
- Revision:
- 51:e0e4d7e3de93
- Parent:
- 50:5d2176b93a65
Fixed comments
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
brass_phoenix | 31:e5b3b01d128a | 1 | #pragma once |
brass_phoenix | 31:e5b3b01d128a | 2 | |
brass_phoenix | 31:e5b3b01d128a | 3 | #include "forward_kinematics.h" |
brass_phoenix | 31:e5b3b01d128a | 4 | #include "inverse_kinematics.h" |
brass_phoenix | 31:e5b3b01d128a | 5 | |
brass_phoenix | 31:e5b3b01d128a | 6 | // Moves the end effector by (move_x, move_y) metres. |
brass_phoenix | 31:e5b3b01d128a | 7 | // Requires current motor angles. |
brass_phoenix | 31:e5b3b01d128a | 8 | // Last two paramteres will give the new target angles. |
brass_phoenix | 31:e5b3b01d128a | 9 | void end_effector_control(double move_x, double move_y, double main_current, double sec_current, double &main_target, double &sec_target) { |
brass_phoenix | 31:e5b3b01d128a | 10 | |
brass_phoenix | 31:e5b3b01d128a | 11 | double end_x, end_y; |
brass_phoenix | 31:e5b3b01d128a | 12 | |
brass_phoenix | 31:e5b3b01d128a | 13 | forward_kinematics(main_current, sec_current, end_x, end_y); |
brass_phoenix | 31:e5b3b01d128a | 14 | |
MAHCSnijders | 50:5d2176b93a65 | 15 | end_x += move_x; // If EMG signals are received this is (+ or -) 0.01 meters |
brass_phoenix | 32:b63b5837bcb1 | 16 | end_y += move_y; |
brass_phoenix | 31:e5b3b01d128a | 17 | |
MAHCSnijders | 50:5d2176b93a65 | 18 | double main_potential_target, sec_potential_target; // Target if inverse kinematics does not give a NAN-value |
brass_phoenix | 31:e5b3b01d128a | 19 | |
brass_phoenix | 31:e5b3b01d128a | 20 | inverse_kinematics(end_x, end_y, main_potential_target, sec_potential_target); |
brass_phoenix | 31:e5b3b01d128a | 21 | |
brass_phoenix | 31:e5b3b01d128a | 22 | // Check for NAN values. (they are never equal to themselves) |
brass_phoenix | 31:e5b3b01d128a | 23 | if (main_potential_target != main_potential_target) { |
brass_phoenix | 31:e5b3b01d128a | 24 | main_potential_target = main_current; // Illogical angle, don't move. |
brass_phoenix | 31:e5b3b01d128a | 25 | } |
brass_phoenix | 31:e5b3b01d128a | 26 | |
brass_phoenix | 31:e5b3b01d128a | 27 | // Check for NAN values. (they are never equal to themselves) |
brass_phoenix | 31:e5b3b01d128a | 28 | if (sec_potential_target != sec_potential_target) { |
brass_phoenix | 31:e5b3b01d128a | 29 | sec_potential_target = sec_current; // Illogical angle, don't move. |
brass_phoenix | 31:e5b3b01d128a | 30 | } |
brass_phoenix | 31:e5b3b01d128a | 31 | |
brass_phoenix | 31:e5b3b01d128a | 32 | // Return new angles. |
brass_phoenix | 31:e5b3b01d128a | 33 | main_target = main_potential_target; |
brass_phoenix | 31:e5b3b01d128a | 34 | sec_target = sec_potential_target; |
brass_phoenix | 31:e5b3b01d128a | 35 | } |