Revision 1:2b8f94cdaaea, committed 2018-07-08

Comitter:

yuliyasm

Date:

Sun Jul 08 20:46:00 2018 +0000

Parent:

0:1825dad3704f

Commit message:

1

Changed in this revision

--- a/main.cpp	Mon May 21 08:05:59 2018 +0000
+++ b/main.cpp	Sun Jul 08 20:46:00 2018 +0000
@@ -1,24 +1,14 @@
-//X-300Y22.5Z-300\r\n - пример команды управления
+//X300Y500Z1000\r\n - пример команды управления (3 угла в градусах)
 #include "stm32f103c8t6.h"
 #include "mbed.h"
 #include "AccelStepper.h"
 #include <ctype.h>
 
-const float r = 25;     // end effector
-const float R = 80;     // base
-const float l = 225.5;
-const float L = 100;
 const int q = 8;        // микрошаг
-
-#define pi      3.14159
-#define cos120  -0.5
-#define sin120  0.866
-#define tg30    0.57735
-#define MAXP    500     // предельные координаты
+#define MSpeed  2000    // максимальная скорость
 
 bool parseXYZ(char* m, float* A);
 float readNum(char* m, int i);
-bool delta_calcInverse(float* A, float* B);
 void MotorsInit();
 void Rx_interrupt();
 
@@ -28,28 +18,32 @@
 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};
+
 DigitalOut      myled(LED1);
 DigitalOut      enb(PA_10);
 Timer           t;
+Timer           t_my;
 Serial          pc(PB_10, PB_11);
 
+bool flags[3] = {true, true, true};
 ////////////////////////////////////////////////////////////////////////////////////
 int main()
 {
     confSysClock();     //Configure system clock (72MHz HSE clock, 48MHz USB clock)
     t.start();
+    t_my.start();
     pc.baud(9600);
     pc.attach(&Rx_interrupt, Serial::RxIrq);
     pc.printf("Hello\r\n");
     MotorsInit();
-
     while(1) {
-        for (int i = 0; i < 3; i++)
-            Motors[i].run();
+        for (int i = 0; i < 3; i++) {
+            Motors[i].runSpeedToPosition();
+            if (Motors[i].distanceToGo() == 0 && flags[i] == false) {
+                flags[i] = true;
+                pc.printf("%d - finish in %d msec\r\n", i, t_my.read_ms());
+            }
+        }
         if (Motors[0].distanceToGo() == 0 && Motors[1].distanceToGo() == 0 && Motors[2].distanceToGo() == 0)
             myled = 1;
     }
@@ -60,24 +54,9 @@
 {
     enb = 0;
     myled = 0;
-
     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(Motors[0].distanceToGo() && Motors[1].distanceToGo() && Motors[2].distanceToGo()) {
-        for (int i = 0; i < 3; i++) {
-            if(zero[i] != 0)
-                Motors[i].runSpeedToPosition();
-            else
-                Motors[i].moveTo(Motors[i].currentPosition());
-        }
-    }
-    for (int i = 0; i < 3; i++) {
-        Motors[i].stop();
+        Motors[i].setMinPulseWidth (20);
+        Motors[i].setMaxSpeed (MSpeed*2);
         Motors[i].setCurrentPosition(0);
     }
     myled = 1;
@@ -106,10 +85,6 @@
             A[2] = readNum(m, i);
         }
     }
-    for(int i = 0; i < 3; i++) { // проверка на адекватность
-        if ((A[i] <= -MAXP) || (A[i] >= MAXP))
-            return false;
-    }
     return true;
 }
 
@@ -122,51 +97,27 @@
         i++;
         n++;
     }
+    if (n == 0) {
+        buff[n] = '0';
+        n++;
+    }
     buff[n] = '\0';
-    if (n == 0)
-        return (MAXP);
     return(atof(buff));
 }
 
 
-bool delta_calcAngleYZ(float x0, float y0, float z0, float &theta)
-{
-    float y1 = -R; // f/2 * tg 30
-    y0 = y0-r;    // shift center to edge
-    // z = a + b*y
-    float a = (x0*x0 + y0*y0 + z0*z0 + L*L - l*l - y1*y1) / (2 * z0);
-    float b = (y1 - y0) / z0;
-    // discriminant
-    float D = -(a + b * y1)*(a + b * y1) + L*L * (b*b + 1);
-    if (D < 0) return false; // non-existing point
-    float yj = (y1 - a * b - sqrt(D)) / (b*b + 1); // choosing outer point
-    float zj = a + b * yj;
-    theta = atan(-zj / (y1 - yj)) + ((yj > y1) ? pi : 0);
-    theta = 180*theta/pi;
-    return true;
-}
-
-bool delta_calcInverse(float* A, float* B)
+void constTime()
 {
-    if (A[2] > 0)
-        return false;
-    float x0 = A[0];
-    float y0 = A[1];
-    float z0 = A[2];
-    int status = delta_calcAngleYZ(x0, y0, z0, B[0]);
-    if (status) {
-        x0 = A[0]*cos120 + A[1]*sin120;
-        y0 = A[1]*cos120 - A[0]*sin120;
-        z0 = A[2];
-        status = delta_calcAngleYZ(x0, y0, z0, B[1]);
-    } // rotate coords to +120 deg
-    if (status) {
-        x0 = A[0]*cos120 - A[1]*sin120;
-        y0 = A[1]*cos120 + A[0]*sin120;
-        z0 = A[2];
-        status = delta_calcAngleYZ(x0, y0, z0, B[2]);
-    } // rotate coords to -120 deg
-    return status;
+    float DeltaMax = 0;
+    for (int i = 0; i < 3; i++) {
+        if(abs(Motors[i].distanceToGo()) > DeltaMax)
+            DeltaMax = abs(Motors[i].distanceToGo());
+    }
+    float k = MSpeed/DeltaMax;
+    for (int i = 0; i < 3; i++) {
+        Motors[i].setSpeed(k*abs(Motors[i].distanceToGo()));
+        pc.printf("\r\nSpeed %d", int(k*abs(Motors[i].distanceToGo())));
+    }
 }
 
 void Rx_interrupt()
@@ -174,30 +125,21 @@
     char message[32] = "";
     pc.scanf("%[^\n]s", message);
     strcat(message, "\n");
-    pc.printf("%s", message);
-
-    float point[3] = {MAXP, MAXP, MAXP};
-    float angles[3] = {0, 0, 0};
+    pc.printf("\r\n%s", message);
 
-    bool p = parseXYZ(message, point);
-    if(p == false) {
-        pc.printf("Parse fail\r\n");
-        return;
-    }
-    pc.printf("Point: %.2f ", point[0]);
-    pc.printf("%.2f ", point[1]);
-    pc.printf("%.2f\r\n", point[2]);
-    p = delta_calcInverse(point, angles);
-    if(p == false) {
-        pc.printf("Kinematic fail\r\n");
-        return;
-    }
+    float angles[3] = {0, 0, 0};
+    parseXYZ(message, angles);
     pc.printf("Angle: ");
     for (int i = 0; i < 3; i++) {
         pc.printf("%.2f ", angles[i]);
-        Motors[i].moveTo(int(angles[i]*q/1.8));
+        Motors[i].move(int(angles[i]*q/1.8));
+        flags[i] = false;
     }
+    constTime();
     myled = 0;
-    pc.printf("\r\nSuccess!\r\n\r\n");
+    t_my.stop();
+    t_my.reset();
+    t_my.start();
+    pc.printf("\r\nWait!\r\n");
     return;
 }
\ No newline at end of file