File content as of revision 1:f9c784a35e9c:

#include "stm32f103c8t6.h"
#include "mbed.h"
#include "USBSerial.h"
#include <string>

const float e = 24;     // end effector
const float f = 75;     // base
const float re = 300;
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;

bool parseXYZ(char* m, float* A);
int delta_calcInverse(float* A, float* B);


int main()
{
    confSysClock();     //Configure system clock (72MHz HSE clock, 48MHz USB clock)
    USBSerial usbSerial(0x1f00, 0x2012, 0x0001,  false);
    DigitalOut  myled(LED1);

    while(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");

        char message[32];
        if (usbSerial.available()) {
            wait_ms(2);
            usbSerial.scanf("%s", message);
            strcat(message, "\r");
            usbSerial.printf("%s", message);

            float A[32];
            bool p = parseXYZ(message, A);
            if(p == false)
                return;
            usbSerial.printf("%.2f \r", A[0]);
            usbSerial.printf("%.2f \r", A[1]);
            usbSerial.printf("%.2f \r", A[2]);

            float B[3];
            if (!delta_calcInverse(A, B)) {
                for (int i = 0; i < 3; i++) {
                    usbSerial.printf("%.2f \r", B[i]);
                    //Motors[i].moveTo(B[i]*q/1.8);
                }
            }
        }
    }
}

bool parseXYZ(char* m, float* A)
{
    char buff[32] = "";
    int i = 0;
    if (m[i] != 'X')
        return false;
    i++;
    while (m[i] != 'Y' && i < 32) {
        strcat(buff, &m[i]);
        i++;
    }
    A[0] = atof(buff);

    strcpy(buff,"");
    i++;
    while (m[i] != 'Z' && i < 32) {
        strcat(buff, &m[i]);
        i++;
    }
    A[1] = atof(buff);

    strcpy(buff,"");
    i++;
    while (m[i] != '\r' && i < 32) {
        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;
}

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