Diff: main.cpp

Revision:

3:086f8c1079ff

Parent:

2:d4dad64faadb

--- a/main.cpp	Sat Feb 03 19:25:25 2018 +0000
+++ b/main.cpp	Mon May 07 11:41:10 2018 +0000
@@ -1,88 +1,118 @@
 #include "stm32f103c8t6.h"
 #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
-const float re = 300;
+const float e = 20;     // end effector
+const float f = 80;     // base
+const float re = 225;
 const float rf = 100;
 
 const float pi = 3.14;
 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 = 8; //микрошаг
 
 bool parseXYZ(char* m, float* A);
 int delta_calcInverse(float* A, float* B);
+void MotorsInit();
 
 PinName stp[3] = {PB_15,PA_8,PA_9};
 PinName dir[3] = {PB_12,PB_13,PB_14};
+AccelStepper axis1(1, stp[0], dir[0]);
+AccelStepper axis2(1, stp[1], dir[1]);
+AccelStepper axis3(1, stp[2], dir[2]);
+AccelStepper Motors[3] = {axis1, axis2, axis3};
+DigitalIn  zero1(PB_10);
+DigitalIn  zero2(PB_1);
+DigitalIn  zero3(PB_0);
+DigitalIn zero[3] = {zero1, zero2, zero3};
+Serial      pc(PA_2, PA_3);
+DigitalOut  myled(LED1);
+DigitalOut  enb(PA_10);
 
-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  enb(PA_10);
+    pc.baud(9600);
     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");
-
+    MotorsInit();
+    float angles[3] = {0, 0, 0};
+    float point[3] = {0, 0, 0};
+    char message[32] = "";
+    char c = '\0';
     while(1) {
 
-        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");*/
+        for (int i = 0; i < 3; i++)
+            Motors[i].run();
+        if (Motors[0].distanceToGo() == 0)
+            myled = 1;
 
-        if (usbSerial.available()) {
-            char message[32];
-            wait_ms(2);
-            usbSerial.scanf("%s", message);
-            strcat(message, "\r");
-            usbSerial.printf("%s", message);
+        if (pc.readable()) {
+            wait_ms(300);
+            strcpy(message, "");
+            pc.scanf("%s\r\n", message);
+            strcat(message, "\n");
+            pc.printf("%s", message);
 
-            float point[3];
             bool p = parseXYZ(message, point);
-            if(p == false)
-                return;
-            usbSerial.printf("%.2f \r", point[0]);
-            usbSerial.printf("%.2f \r", point[1]);
-            usbSerial.printf("%.2f \r", point[2]);
+            if(p == false) {
+                pc.printf("Parse fail");
+                continue;
+            }
+            pc.printf("%.2f ", point[0]);
+            pc.printf("%.2f ", point[1]);
+            pc.printf("%.2f \n", point[2]);
+            delta_calcInverse(point, angles);
 
-            float angles[3];
-            if (!delta_calcInverse(point, angles)) {
-                for (int i = 0; i < 3; i++) {
-                    usbSerial.printf("%.2f \r", angles[i]);
-                    Motors[i].moveTo(angles[i]*q/1.8);
-                }
+            for (int i = 0; i < 3; i++) {
+                pc.printf("%.2f ", angles[i]);
+                Motors[i].moveTo(int(angles[i]*q/1.8));
+                myled = 0;
+                //Motors[i].runToPosition();
+                //myled = 1;
             }
         }
-        for (int i = 0; i < 3; i++)
-            Motors[i].runToPosition();
     }
 }
 
+void MotorsInit()
+{
+    enb = 0;
+    myled = 0;
+    pc.printf("Hello\r\n");
+
+    for (int i = 0; i < 3; i++) {
+        Motors[i].setMinPulseWidth (100);
+        Motors[i].setMaxSpeed (500);
+        Motors[i].setAcceleration (100);
+        Motors[i].moveTo(-90*q/1.8);
+        Motors[i].setSpeed (50);
+    }
+
+    while(zero[0] || zero[1] || zero[2]) {
+        for (int i = 0; i < 3; i++) {
+            if(zero[i] != 0)
+                Motors[i].runSpeedToPosition();
+            if (!Motors[i].distanceToGo())
+                goto setup;
+        }
+    }
+setup:
+    for (int i = 0; i < 3; i++) {
+        Motors[i].stop();
+        Motors[i].setCurrentPosition(0);
+    }
+    myled = 1;
+    pc.printf("Ready\r\n");
+}
+
+
 bool parseXYZ(char* m, float* A)
 {
     char buff[32] = "";
@@ -90,7 +120,9 @@
     if (m[i] != 'X')
         return false;
     i++;
-    while (m[i] != 'Y' && i < 32) {
+    while (m[i] != 'Y') {
+        if (m[i] == '\n')
+            return false;
         strcat(buff, &m[i]);
         i++;
     }
@@ -98,7 +130,9 @@
 
     strcpy(buff,"");
     i++;
-    while (m[i] != 'Z' && i < 32) {
+    while (m[i] != 'Z') {
+        if (m[i] == '\n')
+            return false;
         strcat(buff, &m[i]);
         i++;
     }
@@ -106,12 +140,13 @@
 
     strcpy(buff,"");
     i++;
-    while (m[i] != '\r' && i < 32) {
+    while (m[i] != '\n') {
+        if (i > 32)
+            return false;
         strcat(buff, &m[i]);
         i++;
     }
     A[2] = atof(buff);
-
     return true;
 }
 
@@ -131,8 +166,6 @@
     return 0;
 }
 
-// inverse kinematics: (x0, y0, z0) -> (theta1, theta2, theta3)
-// returned status: 0=OK, -1=non-existing position
 int delta_calcInverse(float* A, float* B)
 {
     float x0 = A[0];
@@ -142,4 +175,4 @@
     if (status == 0) status = delta_calcAngleYZ(x0 * cos120 + y0 * sin120, y0 * cos120 - x0 * sin120, z0, B[1]); // rotate coords to +120 deg
     if (status == 0) status = delta_calcAngleYZ(x0 * cos120 - y0 * sin120, y0 * cos120 + x0 * sin120, z0, B[2]); // rotate coords to -120 deg
     return status;
-}
+}
\ No newline at end of file