File content as of revision 0:06ccc77eaf35:

#include "mbed.h"
#include "stepper.h"
#include <cmath>

stepperMotor motorX(PC_5, PC_4, PD_2, PC_8, 0.0002);
stepperMotor motorY(PA_5, PA_4, PA_2, PA_6, 0.0002);
stepperMotor motorZ(PC_3, PC_2, PC_13, PC_6, 0.0002);

double height;
double a;
double b;

void init()
{
    int steps = -12000;
    motorX.enable(steps);
    while(motorX.remain != 0);
    motorY.enable(steps);
    while(motorY.remain != 0);
    motorZ.enable(steps);
    while(motorZ.remain != 0);
}

int moveArm(double x, double y, double z, bool hand, double &alpha, double &beta, double &lambda)
{
    //double alpha, beta, lambda;
    double c = sqrt(y*y + (x-height)*(x-height));
    alpha = atan((x-height) / y)) + acos((a*a + c*c - b*b) / (2*a*c)) + M_PI_2;
    beta = acos((a*a + b*b - c*c) / (2*a*b));
    lambda = atan(z / y);
    return 0;
}

int main()
{
    //stepperMotor rightMotor(PC_5, PC_4, PD_2, 0.0002);
    motorX.enable();
    motorY.enable();
    motorZ.enable();
    init();
    return 0;
}