State machine
Dependencies: mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter
Diff: end_effector_control.h
- Revision:
- 31:e5b3b01d128a
- Child:
- 32:b63b5837bcb1
diff -r a45bbfa6bd22 -r e5b3b01d128a end_effector_control.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/end_effector_control.h Thu Nov 01 13:29:49 2018 +0000 @@ -0,0 +1,35 @@ +#pragma once + +#include "forward_kinematics.h" +#include "inverse_kinematics.h" + +// Moves the end effector by (move_x, move_y) metres. +// Requires current motor angles. +// Last two paramteres will give the new target angles. +void end_effector_control(double move_x, double move_y, double main_current, double sec_current, double &main_target, double &sec_target) { + + double end_x, end_y; + + forward_kinematics(main_current, sec_current, end_x, end_y); + + end_x + move_x; + end_y + move_y; + + double main_potential_target, sec_potential_target; + + inverse_kinematics(end_x, end_y, main_potential_target, sec_potential_target); + + // Check for NAN values. (they are never equal to themselves) + if (main_potential_target != main_potential_target) { + main_potential_target = main_current; // Illogical angle, don't move. + } + + // Check for NAN values. (they are never equal to themselves) + if (sec_potential_target != sec_potential_target) { + sec_potential_target = sec_current; // Illogical angle, don't move. + } + + // Return new angles. + main_target = main_potential_target; + sec_target = sec_potential_target; +} \ No newline at end of file