File content as of revision 0:e7aedcbababd:

#include "mbed.h"
#include "stepper.h"
#include "Servo.h"
#include <string>
#include "DigitDisplay.h"
#include "ds1307.h"
#define ACCEL_ON  1
#define ACCEL_OFF 0
#define SPEED_X 300 
#define SPEED_Y 300 
#define X_STEPS_PER_MM  86
#define Y_STEPS_PER_MM  86
#define PI  3.14
#define Resolution 500
#define CURVE_SECTION_MM 5
#define Curve_feq_MM 5
//Communication
Serial pc(SERIAL_TX,SERIAL_RX);
Serial Nucleo(D8,D2);
DS1307 thistime(D14,D15);
//clock
DigitalOut buzz(D6);

DigitDisplay display(D7, D5);

Ticker tick;
int year = 1;
int  month = 1;
int  date =1;
int  day = 1;
int  hour   = 17;
int  minute = 28;
int  second = 0;
int Coundown = 30;
//Servo
Servo Pen_1(A0);
//Servo Pen_2(A1);

//Stepper
stepper x(PB_13,PB_14);
stepper y(PA_12,PC_5);

//Limit Switch
DigitalIn X_MIN(PC_0,PullUp);
DigitalIn X_MAX(PC_1,PullUp);
DigitalIn Y_MIN(PB_0,PullUp);
DigitalIn Y_MAX(PA_4,PullUp);

struct FloatPoint {
    float x;
    float y;
    float sum;
};

FloatPoint current_steps;
FloatPoint target_steps;
FloatPoint delta_steps;

FloatPoint current_mm;
FloatPoint target_mm;
FloatPoint delta_mm;

FloatPoint fp;
FloatPoint MAX;
FloatPoint MIN;



float z;
float curve_section = CURVE_SECTION_MM;
int Set = 0;
bool x_direction;
bool y_direction;

long max_delta;
long x_counter;
long y_counter;

int Mode_G = 0;
int Mode_$ = 0;
int SIZE = 0;
int s;
int state_pen;
float i,j;

int serch(char ch,string cmd);
void Taonoi_Begin ();
bool Taonoican_Step_x(long current, long target, int8_t direction_x);
bool Taonoican_Step_y(long current, long target, int8_t direction_y);
void Taonoi_Move();
void calculate_deltas();
void Curve (float i, float j);
void Taonoi_Square(float X_MAX, float X_MIN, float Y_MAX, float Y_MIN);
void Taonoi_Around(float X_MAX, float X_MIN, float Y_MAX, float Y_MIN);
void Taonoi_Ellipse(float X_MAX, float X_MIN, float Y_MAX, float Y_MIN);
void beatup();
void percent_coundown();


int main()
{
    pc.baud(115200);
    Nucleo.baud(9600);
    pc.printf("Hello\n");
    string data  ;

    Taonoi_Begin();
    //Taonoi_Around(50,0,70,0);
    //Taonoi_Square(50,0,70,0);
    //Taonoi_Oval(50,0,100,30);
    MAX.x = 0;
    MAX.y = 0;
    MIN.x = 200;
    MIN.x = 200;
    thistime.gettime( &second, &minute, &hour, &day, &date, &month, &year);
    display.write(0, hour / 10);
    display.write(1, hour % 10);
    display.write(2, minute / 10);
    display.write(3, minute % 10);
    display.setColon(true);
    tick.attach(&beatup, 0.5);

    while(1) {
        fp.x = 0.0;
        fp.y = 0.0;

        if(Nucleo.readable()) {
            char ch = Nucleo.getc();
            if(ch!='\r') {
                pc.putc(ch);
                data.push_back(ch);
            } else {
                if(data.find('G') != std::string::npos) {
                    Mode_G = serch('G',data);
                }
                if(data.find('$') != std::string::npos) {
                    Mode_$ = serch('$',data);
                }
                if(data.find('X') != std::string::npos) {
                    //target_steps.x = serch('X',data);
                    target_mm.x = serch('X',data);
                    fp.x = target_mm.x;
                }
                if(data.find('Y') != std::string::npos) {
                    target_mm.y = serch('Y',data);
                    fp.y = target_mm.y;
                }
                if(data.find('Z') != std::string::npos) {
                    z = serch('Z',data);
                }
                if(data.find('I') != std::string::npos) {
                    i = serch('I',data);
                }

                if(data.find('J') != std::string::npos) {
                    j = serch('J',data);
                }
                if(data.find('S') != std::string::npos) {
                    s = serch('S',data);
                    if(s==1)
                        SIZE = 0;
                    else if (s==2)
                        SIZE = 20;
                    else if (s==3)
                        SIZE = 40;
                }
                data.clear();
                if(Mode_G == 2 || Mode_G == 3) {
                    Pen_1 = z;
                    if(state_pen == 1 && z == 0)
                        wait(0.5);
                    else if (state_pen == 0 && z == 1)
                        wait(0.5);
                    state_pen = z;
                    Curve(i,j);
                    Nucleo.putc('#');
                    Mode_G = 0;
                } else {
                    calculate_deltas();
                    Pen_1 = z;
                    if(state_pen == 1 && z == 0)
                        wait(0.5);
                    else if (state_pen == 0 && z == 1)
                        wait(0.5);
                    state_pen = z;
                    Taonoi_Move();
                    Nucleo.putc('#');
                }
                if(Mode_$ == 1) {
                    Taonoi_Square(MAX.x,MIN.x,MAX.y,MIN.y);
                    Nucleo.putc('$');
                    Mode_$ = 0;
                } else if(Mode_$ == 2) {
                    Taonoi_Around(MAX.x,MIN.x,MAX.y,MIN.y);
                    Nucleo.putc('$');
                    Mode_$ = 0;
                } else if(Mode_$ == 3) {
                    Taonoi_Ellipse(MAX.x,MIN.x,MAX.y,MIN.y);
                    Nucleo.putc('$');
                    Mode_$ = 0;

                }
            }
        }

    }
}

int serch(char ch,string cmd)
{
    size_t pos = cmd.find(ch);
    size_t epos = cmd.find(' ');
    string temp = cmd.substr(pos+1,epos-pos);
    //pc.printf("\n===: %s\n\n",temp.c_str());
    return atoi(temp.c_str());

}
void Taonoi_Begin()
{
    Pen_1 = 1;
    target_mm.x = -400;
    target_mm.y = -400;
    calculate_deltas();
    Taonoi_Move();
    current_steps.x = 0;
    current_steps.y = 0;
    target_mm.x = 50;//50;
    target_mm.y = 100;//100;
    calculate_deltas();
    Taonoi_Move();
    current_steps.x = 0;
    current_steps.y = 0;
    target_mm.x = 0;
    target_mm.y = 0;
    target_steps.x = 0;
    target_steps.y = 0;
    Set = 1;
}
bool Taonoican_Step_x(long current, long target, int8_t direction_x)
{
    if (target == current)
        return false;
    //stop us if we're at home and still going
    else if (!X_MIN.read() && !direction_x)
        return false;
    //stop us if we're at max and still going
    else if (!X_MAX.read() && direction_x)
        return false;

    //default to being able to step
    return true;
}

bool Taonoican_Step_y(long current, long target, int8_t direction_y)
{
    if (target == current)
        return false;
    //stop us if we're at home and still going
    else if (!Y_MIN.read() && !direction_y)
        return false;
    //stop us if we're at max and still going
    else if (!Y_MAX.read() && direction_y)
        return false;

    //default to being able to step
    return true;
}

void Taonoi_Move()
{
    //figure out our deltas
    max_delta = delta_steps.x > delta_steps.y ? delta_steps.x : delta_steps.y;
    //init stuff.
    long x_counter = -max_delta / 2;
    long y_counter = -max_delta / 2;

    //our step flags
    bool x_can_step = 0;
    bool y_can_step = 0;
    
    int A = 0;
    int B = 0;

    do {
        x_can_step = Taonoican_Step_x(current_steps.x, target_steps.x, x_direction);
        y_can_step = Taonoican_Step_y(current_steps.y, target_steps.y, y_direction);
        if (x_can_step) {
            x_counter += delta_steps.x;

            if (x_counter > 0) {
                x.step(1,x_direction,SPEED_X,ACCEL_OFF);
                x_counter -= max_delta;

                if (x_direction)
                    current_steps.x++;
                else
                    current_steps.x--;
            }
            else
            A++;
        }

        if (y_can_step) {
            y_counter += delta_steps.y;

            if (y_counter > 0) {
                y.step(1,y_direction,SPEED_Y,ACCEL_OFF);
                y_counter -= max_delta;

                if (y_direction)
                    current_steps.y++;
                else
                    current_steps.y--;
            }
            else
            B++;
        }
        if(A > 10000 || B>10000)
        break;

        //wait for next step.
    } while (x_can_step || y_can_step);
    //set our points to be the same
    current_steps.x = target_steps.x;
    current_steps.y = target_steps.y;
    current_mm.x = target_mm.x;
    current_mm.y = target_mm.y;
    calculate_deltas();
}
void calculate_deltas()
{
    delta_mm.x = abs(target_mm.x - current_mm.x);
    delta_mm.y = abs(target_mm.y - current_mm.y);

    target_steps.x = X_STEPS_PER_MM * target_mm.x;
    target_steps.y = Y_STEPS_PER_MM * target_mm.y;
    if(Set == 1) {
        if(target_mm.x > MAX.x)
            MAX.x = target_mm.x;
        if(target_mm.x < MIN.x)
            MIN.x = target_mm.x;
        if(target_mm.y > MAX.y)
            MAX.y = target_mm.y;
        if(target_mm.y < MIN.y)
            MIN.y = target_mm.y;
    }


    delta_steps.x = abs(target_steps.x - current_steps.x);
    delta_steps.y = abs(target_steps.y - current_steps.y);

    x_direction = (target_steps.x >= current_steps.x);
    y_direction = (target_steps.y >= current_steps.y);
}

void Curve (float i, float j)
{
    FloatPoint cent;
    cent.x = i+current_mm.x;
    cent.y = j+current_mm.y;

    // Centre coordinates are always relative
    float angleA, angleB, angle, radius, length, aX, aY, bX, bY;

    aX = (current_mm.x - cent.x);
    aY = (current_mm.y - cent.y);
    bX = (fp.x - cent.x);
    bY = (fp.y - cent.y);

    if (Mode_G == 2) { // Clockwise
        angleA = atan2(bY, bX);
        angleB = atan2(aY, aX);
    } else { // Counterclockwise
        angleA = atan2(aY, aX);
        angleB = atan2(bY, bX);
    }

    // Make sure angleB is always greater than angleA
    // and if not add 2PI so that it is (this also takes
    // care of the special case of angleA == angleB,
    // ie we want a complete circle)
    if (angleB <= angleA) angleB += 2 * PI;
    angle = angleB - angleA;

    radius = sqrt(aX * aX + aY * aY);
    length = radius * angle;
    int steps, s, step;
    steps = (int) ceil(length / curve_section);

    //FloatPoint newPoint;
    for (s = 1; s <= steps; s++) {
        step = (Mode_G == 3) ? s : steps - s; // Work backwards for CW
        target_mm.x = cent.x + radius * cos(angleA + angle * ((float) step / steps));
        target_mm.y = cent.y + radius * sin(angleA + angle * ((float) step / steps));
        calculate_deltas();
        Taonoi_Move();
    }
}

void Curve_Ex (float i, float j)
{
    FloatPoint cent;
    cent.x = i+current_mm.x;
    cent.y = j+current_mm.y;

    // Centre coordinates are always relative
    float angleA, angleB, angle, radius, length, aX, aY, bX, bY;

    aX = (current_mm.x - cent.x);
    aY = (current_mm.y - cent.y);
    bX = (fp.x - cent.x);
    bY = (fp.y - cent.y);

    if (Mode_G == 2) { // Clockwise
        angleA = atan2(bY, bX);
        angleB = atan2(aY, aX);
    } else { // Counterclockwise
        angleA = atan2(aY, aX);
        angleB = atan2(bY, bX);
    }

    // Make sure angleB is always greater than angleA
    // and if not add 2PI so that it is (this also takes
    // care of the special case of angleA == angleB,
    // ie we want a complete circle)
    if (angleB <= angleA) angleB += 2 * PI;
    angle = angleB - angleA;

    radius = sqrt(aX * aX + aY * aY);
    length = radius * angle;
    int steps, s, step;
    steps = (int) ceil(length / curve_section);

    //FloatPoint newPoint;
    for (s = 1; s <= steps; s++) {
        step = (Mode_G == 3) ? s : steps - s; // Work backwards for CW
        target_mm.x = cent.x + 2*radius * cos(angleA + angle * ((float) step / steps));
        target_mm.y = cent.y + radius * sin(angleA + angle * ((float) step / steps));
        calculate_deltas();
        Taonoi_Move();
    }
}

void Curve_Ey (float i, float j)
{
    FloatPoint cent;
    cent.x = i+current_mm.x;
    cent.y = j+current_mm.y;

    // Centre coordinates are always relative
    float angleA, angleB, angle, radius, length, aX, aY, bX, bY;

    aX = (current_mm.x - cent.x);
    aY = (current_mm.y - cent.y);
    bX = (fp.x - cent.x);
    bY = (fp.y - cent.y);

    if (Mode_G == 2) { // Clockwise
        angleA = atan2(bY, bX);
        angleB = atan2(aY, aX);
    } else { // Counterclockwise
        angleA = atan2(aY, aX);
        angleB = atan2(bY, bX);
    }

    // Make sure angleB is always greater than angleA
    // and if not add 2PI so that it is (this also takes
    // care of the special case of angleA == angleB,
    // ie we want a complete circle)
    if (angleB <= angleA) angleB += 2 * PI;
    angle = angleB - angleA;

    radius = sqrt(aX * aX + aY * aY);
    length = radius * angle;
    int steps, s, step;
    steps = (int) ceil(length / curve_section);

    //FloatPoint newPoint;
    for (s = 1; s <= steps; s++) {
        step = (Mode_G == 3) ? s : steps - s; // Work backwards for CW
        target_mm.x = cent.x + radius * cos(angleA + angle * ((float) step / steps));
        target_mm.y = cent.y + 2*radius * sin(angleA + angle * ((float) step / steps));
        calculate_deltas();
        Taonoi_Move();
    }
}

void Taonoi_Square(float X_MAX, float X_MIN, float Y_MAX, float Y_MIN)
{
    float Delta_X = abs(X_MAX - X_MIN);
    float Delta_Y = abs(Y_MAX - Y_MIN);
    int Max = (Delta_X > Delta_Y) ? ceil(Delta_X) : ceil(Delta_Y);
    float Cen_X = (X_MAX + X_MIN) / 2;
    float Cen_Y = (Y_MAX + Y_MIN) / 2;
    Coundown = ceil((Max/5)*(Max/5+1)*(0.115));
    target_mm.x = Cen_X;
    target_mm.y = Cen_Y;
    calculate_deltas();
    Taonoi_Move();
    Pen_1 = 0;
    //percent_coundown(Coundown);
    //timer.start();
    for(int i = 0; i < Max + 4 + SIZE; i += 5) {
        if(i % 2 != 0) {
            target_mm.x = current_mm.x + i;
            calculate_deltas();
            Taonoi_Move();
            target_mm.y = current_mm.y + i;
            calculate_deltas();
            Taonoi_Move();
        } else {
            target_mm.x = current_mm.x - i;
            calculate_deltas();
            Taonoi_Move();
            target_mm.y = current_mm.y - i;
            calculate_deltas();
            Taonoi_Move();
        }

    }
    Pen_1 = 1;
    for(int k=0; k<5; k++) {
        buzz=1;
        wait(0.5);
        buzz=0;
    }
    
    MAX.x = MAX.y = 0;
    MAX.x = MAX.y = 200;
}

void Taonoi_Around(float X_MAX, float X_MIN, float Y_MAX, float Y_MIN)
{
    float Delta_X = abs(X_MAX - X_MIN);
    float Delta_Y = abs(Y_MAX - Y_MIN);
    int   Max = (Delta_X > Delta_Y) ? ceil(Delta_X) : ceil(Delta_Y);
    float Cen_X = (X_MAX + X_MIN) / 2;
    float Cen_Y = (Y_MAX + Y_MIN) / 2;
    int   R_max = Max/2+Curve_feq_MM;
    int   Num_c = R_max/Curve_feq_MM+1;
    Coundown = ceil((12.5*Num_c+5*(Num_c-1)*(Num_c))*0.094);
    float R1 = Curve_feq_MM;
    float R2 = R1/2;
    //int Coundown = ceil((Max/5)*(Max/5+1)*(0.115));
    target_mm.x = Cen_X;
    target_mm.y = Cen_Y;
    calculate_deltas();
    Taonoi_Move();
    Pen_1 = 0;
//   percent_coundown(Coundown_c);
    target_mm.x = Cen_X + R1;
    target_mm.y = Cen_Y;
    calculate_deltas();
    Taonoi_Move();
    do {
        Mode_G = 3;
        fp.x = Cen_X-R1;
        fp.y = Cen_Y;
        Curve(-R1,0);

        fp.x = Cen_X+R1+Curve_feq_MM;
        fp.y = Cen_Y;
        Curve(R1 + R2,0);


        R1 = R1 + Curve_feq_MM;
    } while(R1 <= R_max + SIZE);
    Pen_1 = 1;
    for(int k=0; k<5; k++) {
        buzz=1;
        wait(0.5);
        buzz=0;
    }
    
    Mode_G = 0;
    MAX.x = MAX.y =0;
    MIN.x = MIN.y =200;
}

void Taonoi_Ellipse(float X_MAX, float X_MIN, float Y_MAX, float Y_MIN)
{
    float Delta_X = abs(X_MAX - X_MIN);
    float Delta_Y = abs(Y_MAX - Y_MIN);
    int   Max = (Delta_X > Delta_Y) ? ceil(Delta_X) : ceil(Delta_Y);
    float Cen_X = (X_MAX + X_MIN) / 2;
    float Cen_Y = (Y_MAX + Y_MIN) / 2;
    int   R_max = Max/2+Curve_feq_MM;
    int   Num_c = R_max/Curve_feq_MM+1;
    Coundown = ceil((12.5*Num_c+5*(Num_c-1)*(Num_c))*0.094);
    float R1 = Curve_feq_MM;
    float R2 = R1/2;
    target_mm.x = Cen_X;
    target_mm.y = Cen_Y;
    calculate_deltas();
    Taonoi_Move();
    Pen_1 = 0;
//percent_coundown(Coundown_c);
    target_mm.x = Cen_X + R1;
    target_mm.y = Cen_Y;
    calculate_deltas();
    Taonoi_Move();
    do {
        Mode_G = 3;
        fp.x = Cen_X-R1;
        fp.y = Cen_Y;
        Curve_Ey(-R1,0);

        fp.x = Cen_X+R1+Curve_feq_MM;
        fp.y = Cen_Y;
        Curve_Ey(R1 + R2,0);


        R1 = R1 + Curve_feq_MM;
    } while(R1<=R_max + SIZE);
    Pen_1 = 1;
    for(int k=0; k<5; k++) {
        buzz=1;
        wait(0.5);
        buzz=0;
    }
    //timer.stop();
    /*} else {
        do {
            Mode_G = 3;
            fp.x = Cen_X-2*R1;
            fp.y = Cen_Y;
            Curve_Ex(-R1,0);

            fp.x = Cen_X+2*R1+2*Curve_feq_MM;
            fp.y = Cen_Y;
            Curve_Ex(R1 + R2,0);


            R1 = R1 + Curve_feq_MM;
        } while(R1<=R_max + SIZE);
        Pen_1 = 1;
        Mode_G = 0;
    }*/
    
    Mode_G = 0;
    MAX.x = MAX.y =0;
    MIN.x = MIN.y =200;
}

void beatup()
{
    static uint8_t colon = 0;

    display.setColon(colon);
    if (colon) {
        second++;
        if (second >= 60) {
            second = 0;
            minute++;
            if (minute >= 60) {
                minute = 0;

                hour++;
                if (hour >= 24) {
                    hour = 0;
                }
                display.write(0, hour / 10);
                display.write(1, hour % 10);
            }
            display.write(2, minute / 10);
            display.write(3, minute % 10);
        }
    }
    colon = 1 - colon;
}

/*void percent_coundown()
static uint8_t colon = 0;
int current_time = 0 ;
int current_percent =0;
    display.setColon(colon);
    if (colon) {

    }
    colon = 1 - colon;
}

{
    int current_time = 0 ;
    int current_percent =0;
    display.write(0, 0);

    do {
        current_time=timer.read();
        current_percent = current_time*100/time;
        display.write(1, current_percent/100);
        display.write(2, (current_percent%100)/10);
        display.write(3, current_percent%10);
    } while(time<=current_time);
    display.write(1, 1);
    display.write(2, 0);
    display.write(3, 0);
    for(int k=0; k<5; k++) {
        buzz=1;
        wait(5);
        buzz=0;
    }
    display.setColon(true);
    tick.attach(&beatup, 0.5);
    current_time=0;


}*/