Biorobotics 7 / Mbed 2 deprecated State_Machine

State machine

Dependencies:   mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter

end_effector_control.h@31:e5b3b01d128a, 2018-11-01 (annotated)

Committer:
brass_phoenix
Date:
Thu Nov 01 13:29:49 2018 +0000
Revision:
31:e5b3b01d128a
Child:
32:b63b5837bcb1
+ Made end effector controll function.

Who changed what in which revision?

UserRevisionLine numberNew 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
brass_phoenix 31:e5b3b01d128a 15 end_x + move_x;
brass_phoenix 31:e5b3b01d128a 16 end_y + move_y;
brass_phoenix 31:e5b3b01d128a 17
brass_phoenix 31:e5b3b01d128a 18 double main_potential_target, sec_potential_target;
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 }