File content as of revision 1:2bfb79a5ed38:

//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(PA_15);
DigitalIn       zero2(PA_12);
DigitalIn       zero3(PA_11);
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);
    }
    while(Motors[0].distanceToGo() || Motors[1].distanceToGo() || Motors[2].distanceToGo()) {
        for (int i = 0; i < 3; i++) {
            if(zero[i] != 0)
                Motors[i].run();
            else
                Motors[i].moveTo(Motors[i].currentPosition());
        }
    }
    for (int i = 0; i < 3; i++) {
        Motors[i].stop();
        Motors[i].setCurrentPosition(0);
        Motors[i].setMaxSpeed (2000);
        Motors[i].setAcceleration (500);
    }
    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;
}