Diff: main.cpp

Revision:

0:e7aedcbababd

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Dec 12 07:40:53 2015 +0000
@@ -0,0 +1,710 @@
+#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;
+
+
+}*/
+ 
+