Diff: main.cpp

Revision:

0:06ccc77eaf35

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jul 25 11:50:40 2018 +0000
@@ -0,0 +1,42 @@
+#include "mbed.h"
+#include "stepper.h"
+#include <cmath>
+
+stepperMotor motorX(PC_5, PC_4, PD_2, PC_8, 0.0002);
+stepperMotor motorY(PA_5, PA_4, PA_2, PA_6, 0.0002);
+stepperMotor motorZ(PC_3, PC_2, PC_13, PC_6, 0.0002);
+
+double height;
+double a;
+double b;
+
+void init()
+{
+    int steps = -12000;
+    motorX.enable(steps);
+    while(motorX.remain != 0);
+    motorY.enable(steps);
+    while(motorY.remain != 0);
+    motorZ.enable(steps);
+    while(motorZ.remain != 0);
+}
+
+int moveArm(double x, double y, double z, bool hand, double &alpha, double &beta, double &lambda)
+{
+    //double alpha, beta, lambda;
+    double c = sqrt(y*y + (x-height)*(x-height));
+    alpha = atan((x-height) / y)) + acos((a*a + c*c - b*b) / (2*a*c)) + M_PI_2;
+    beta = acos((a*a + b*b - c*c) / (2*a*b));
+    lambda = atan(z / y);
+    return 0;
+}
+
+int main()
+{
+    //stepperMotor rightMotor(PC_5, PC_4, PD_2, 0.0002);
+    motorX.enable();
+    motorY.enable();
+    motorZ.enable();
+    init();
+    return 0;
+}
\ No newline at end of file