Diff: main.cpp

Revision:

2:d4dad64faadb

Parent:

1:f9c784a35e9c

Child:

3:086f8c1079ff

--- a/main.cpp	Tue Jan 30 18:33:46 2018 +0000
+++ b/main.cpp	Sat Feb 03 19:25:25 2018 +0000
@@ -2,6 +2,9 @@
 #include "mbed.h"
 #include "USBSerial.h"
 #include <string>
+#include "AccelStepper.h"
+//X20Y20Z-300\r - пример команды
+
 
 const float e = 24;     // end effector
 const float f = 75;     // base
@@ -12,48 +15,71 @@
 const float cos120 = -0.5;
 const float sin120 = sqrt(3.0) / 2;
 const float tg30 = 1 / sqrt(3.0);
-const float q = 1;
+const float q = 1; //микрошаг
 
 bool parseXYZ(char* m, float* A);
 int delta_calcInverse(float* A, float* B);
 
+PinName stp[3] = {PB_15,PA_8,PA_9};
+PinName dir[3] = {PB_12,PB_13,PB_14};
+
+AccelStepper Xaxis(1, stp[0], dir[0]);
+AccelStepper Yaxis(1, stp[1], dir[1]);
+AccelStepper Zaxis(1, stp[2], dir[2]);
+AccelStepper Motors[3] = {Xaxis, Yaxis, Zaxis};
+Timer t;
 
 int main()
 {
     confSysClock();     //Configure system clock (72MHz HSE clock, 48MHz USB clock)
     USBSerial usbSerial(0x1f00, 0x2012, 0x0001,  false);
-    DigitalOut  myled(LED1);
+    //DigitalOut  myled(LED1);
+    DigitalOut  enb(PA_10);
+    t.start();
+
+    for (int i = 0; i < 3; i++) {
+        Motors[i].setMaxSpeed(1000);
+        Motors[i].setAcceleration(100);
+        Motors[i].setMinPulseWidth(10000);
+        //Motors[i].moveTo(500);
+    }
+
+    usbSerial.printf("Ready\r\n");
 
     while(1) {
-        myled = 0;      // turn the LED on
+
+        enb = 1;
+        /*myled = 0;      // turn the LED on
         wait_ms(200);   // 200 millisecond
         myled = 1;      // turn the LED off
         wait_ms(1000);  // 1000 millisecond
-        usbSerial.printf("Blink\r\n");
+        usbSerial.printf("Blink\r\n");*/
 
-        char message[32];
         if (usbSerial.available()) {
+            char message[32];
             wait_ms(2);
             usbSerial.scanf("%s", message);
             strcat(message, "\r");
             usbSerial.printf("%s", message);
 
-            float A[32];
-            bool p = parseXYZ(message, A);
+            float point[3];
+            bool p = parseXYZ(message, point);
             if(p == false)
                 return;
-            usbSerial.printf("%.2f \r", A[0]);
-            usbSerial.printf("%.2f \r", A[1]);
-            usbSerial.printf("%.2f \r", A[2]);
+            usbSerial.printf("%.2f \r", point[0]);
+            usbSerial.printf("%.2f \r", point[1]);
+            usbSerial.printf("%.2f \r", point[2]);
 
-            float B[3];
-            if (!delta_calcInverse(A, B)) {
+            float angles[3];
+            if (!delta_calcInverse(point, angles)) {
                 for (int i = 0; i < 3; i++) {
-                    usbSerial.printf("%.2f \r", B[i]);
-                    //Motors[i].moveTo(B[i]*q/1.8);
+                    usbSerial.printf("%.2f \r", angles[i]);
+                    Motors[i].moveTo(angles[i]*q/1.8);
                 }
             }
         }
+        for (int i = 0; i < 3; i++)
+            Motors[i].runToPosition();
     }
 }