Diff: main.cpp

Revision:

0:1825dad3704f

Child:

1:2bfb79a5ed38

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon May 21 08:05:59 2018 +0000
@@ -0,0 +1,203 @@
+//X-300Y22.5Z-300\r\n - пример команды управления
+#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     // предельные координаты
+
+bool parseXYZ(char* m, float* A);
+float readNum(char* m, int i);
+bool delta_calcInverse(float* A, float* B);
+void MotorsInit();
+void Rx_interrupt();
+
+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};
+DigitalOut      myled(LED1);
+DigitalOut      enb(PA_10);
+Timer           t;
+Serial          pc(PB_10, PB_11);
+
+////////////////////////////////////////////////////////////////////////////////////
+int main()
+{
+    confSysClock();     //Configure system clock (72MHz HSE clock, 48MHz USB clock)
+    t.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();
+        if (Motors[0].distanceToGo() == 0 && Motors[1].distanceToGo() == 0 && Motors[2].distanceToGo() == 0)
+            myled = 1;
+    }
+}
+///////////////////////////////////////////////////////////////////////////////////
+
+void MotorsInit()
+{
+    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].setCurrentPosition(0);
+    }
+    myled = 1;
+    pc.printf("Ready\r\n\r\n");
+}
+
+
+bool parseXYZ(char* m, float* A)
+{
+    for(int i = 0; i < 32; i++) {
+        if (m[i] == '\n' || m[i] == '\r')
+            break;
+        /////////////////////////////////
+        if (m[i] == 'X') {
+            i++;
+            A[0] = readNum(m, i);
+        }
+        ////////////////////////////////
+        if (m[i] == 'Y') {
+            i++;
+            A[1] = readNum(m, i);
+        }
+        //////////////////////////////////
+        if (m[i] == 'Z') {
+            i++;
+            A[2] = readNum(m, i);
+        }
+    }
+    for(int i = 0; i < 3; i++) { // проверка на адекватность
+        if ((A[i] <= -MAXP) || (A[i] >= MAXP))
+            return false;
+    }
+    return true;
+}
+
+float readNum(char* m, int i)
+{
+    char buff[10] = "";
+    int n = 0;
+    while ((isdigit(m[i]) || m[i] == '.' || m[i] == '-') && n < 9) {
+        buff[n] = m[i];
+        i++;
+        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)
+{
+    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;
+}
+
+void Rx_interrupt()
+{
+    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};
+
+    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;
+    }
+    pc.printf("Angle: ");
+    for (int i = 0; i < 3; i++) {
+        pc.printf("%.2f ", angles[i]);
+        Motors[i].moveTo(int(angles[i]*q/1.8));
+    }
+    myled = 0;
+    pc.printf("\r\nSuccess!\r\n\r\n");
+    return;
+}
\ No newline at end of file