File content as of revision 3:086f8c1079ff:

#include "stm32f103c8t6.h"
#include "mbed.h"
#include <string>
#include "AccelStepper.h"
//X20Y20Z-300\r - пример команды

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 = 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);

Timer t;

int main()
{
    confSysClock();     //Configure system clock (72MHz HSE clock, 48MHz USB clock)
    pc.baud(9600);
    t.start();

    MotorsInit();
    float angles[3] = {0, 0, 0};
    float point[3] = {0, 0, 0};
    char message[32] = "";
    char c = '\0';
    while(1) {

        for (int i = 0; i < 3; i++)
            Motors[i].run();
        if (Motors[0].distanceToGo() == 0)
            myled = 1;

        if (pc.readable()) {
            wait_ms(300);
            strcpy(message, "");
            pc.scanf("%s\r\n", message);
            strcat(message, "\n");
            pc.printf("%s", message);

            bool p = parseXYZ(message, point);
            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);

            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;
            }
        }
    }
}

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] = "";
    int i = 0;
    if (m[i] != 'X')
        return false;
    i++;
    while (m[i] != 'Y') {
        if (m[i] == '\n')
            return false;
        strcat(buff, &m[i]);
        i++;
    }
    A[0] = atof(buff);

    strcpy(buff,"");
    i++;
    while (m[i] != 'Z') {
        if (m[i] == '\n')
            return false;
        strcat(buff, &m[i]);
        i++;
    }
    A[1] = atof(buff);

    strcpy(buff,"");
    i++;
    while (m[i] != '\n') {
        if (i > 32)
            return false;
        strcat(buff, &m[i]);
        i++;
    }
    A[2] = atof(buff);
    return true;
}

int delta_calcAngleYZ(float x0, float y0, float z0, float &theta)
{
    float y1 = -0.5 * tg30 * f; // f/2 * tg 30
    y0 -= 0.5 * tg30    * e;    // shift center to edge
    // z = a + b*y
    float a = (x0 * x0 + y0 * y0 + z0 * z0 + rf * rf - re * re - y1 * y1) / (2 * z0);
    float b = (y1 - y0) / z0;
    // discriminant
    float d = -(a + b * y1) * (a + b * y1) + rf * (b * b * rf + rf);
    if (d < 0) return -1; // non-existing point
    float yj = (y1 - a * b - sqrt(d)) / (b * b + 1); // choosing outer point
    float zj = a + b * yj;
    theta = 180.0 * atan(-zj / (y1 - yj)) / pi + ((yj > y1) ? 180.0 : 0.0);
    return 0;
}

int delta_calcInverse(float* A, float* B)
{
    float x0 = A[0];
    float y0 = A[1];
    float z0 = A[2];
    int status = delta_calcAngleYZ(x0, y0, z0, B[0]);
    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;
}