Diff: end_effector_control.h

Revision:

31:e5b3b01d128a

Child:

32:b63b5837bcb1

--- /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