State machine
Dependencies: mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter
end_effector_control.h
- Committer:
- MAHCSnijders
- Date:
- 2018-11-05
- Revision:
- 51:e0e4d7e3de93
- Parent:
- 50:5d2176b93a65
File content as of revision 51:e0e4d7e3de93:
#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; // If EMG signals are received this is (+ or -) 0.01 meters end_y += move_y; double main_potential_target, sec_potential_target; // Target if inverse kinematics does not give a NAN-value 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; }