main.cpp

00001 #include "stm32f103c8t6.h"
00002 #include "mbed.h"
00003 #include <string>
00004 #include "AccelStepper.h"
00005 //X20Y20Z-300\r - пример команды
00006 
00007 const float e = 20;     // end effector
00008 const float f = 80;     // base
00009 const float re = 225;
00010 const float rf = 100;
00011 
00012 const float pi = 3.14;
00013 const float cos120 = -0.5;
00014 const float sin120 = sqrt(3.0) / 2;
00015 const float tg30 = 1 / sqrt(3.0);
00016 const float q = 8; //микрошаг
00017 
00018 bool parseXYZ(char* m, float* A);
00019 int delta_calcInverse(float* A, float* B);
00020 void MotorsInit();
00021 
00022 PinName stp[3] = {PB_15,PA_8,PA_9};
00023 PinName dir[3] = {PB_12,PB_13,PB_14};
00024 AccelStepper axis1(1, stp[0], dir[0]);
00025 AccelStepper axis2(1, stp[1], dir[1]);
00026 AccelStepper axis3(1, stp[2], dir[2]);
00027 AccelStepper Motors[3] = {axis1, axis2, axis3};
00028 DigitalIn  zero1(PB_10);
00029 DigitalIn  zero2(PB_1);
00030 DigitalIn  zero3(PB_0);
00031 DigitalIn zero[3] = {zero1, zero2, zero3};
00032 Serial      pc(PA_2, PA_3);
00033 DigitalOut  myled(LED1);
00034 DigitalOut  enb(PA_10);
00035 
00036 Timer t;
00037 
00038 int main()
00039 {
00040     confSysClock();     //Configure system clock (72MHz HSE clock, 48MHz USB clock)
00041     pc.baud(9600);
00042     t.start();
00043 
00044     MotorsInit();
00045     float angles[3] = {0, 0, 0};
00046     float point[3] = {0, 0, 0};
00047     char message[32] = "";
00048     char c = '\0';
00049     while(1) {
00050 
00051         for (int i = 0; i < 3; i++)
00052             Motors[i].run();
00053         if (Motors[0].distanceToGo() == 0)
00054             myled = 1;
00055 
00056         if (pc.readable()) {
00057             wait_ms(300);
00058             strcpy(message, "");
00059             pc.scanf("%s\r\n", message);
00060             strcat(message, "\n");
00061             pc.printf("%s", message);
00062 
00063             bool p = parseXYZ(message, point);
00064             if(p == false) {
00065                 pc.printf("Parse fail");
00066                 continue;
00067             }
00068             pc.printf("%.2f ", point[0]);
00069             pc.printf("%.2f ", point[1]);
00070             pc.printf("%.2f \n", point[2]);
00071             delta_calcInverse(point, angles);
00072 
00073             for (int i = 0; i < 3; i++) {
00074                 pc.printf("%.2f ", angles[i]);
00075                 Motors[i].moveTo(int(angles[i]*q/1.8));
00076                 myled = 0;
00077                 //Motors[i].runToPosition();
00078                 //myled = 1;
00079             }
00080         }
00081     }
00082 }
00083 
00084 void MotorsInit()
00085 {
00086     enb = 0;
00087     myled = 0;
00088     pc.printf("Hello\r\n");
00089 
00090     for (int i = 0; i < 3; i++) {
00091         Motors[i].setMinPulseWidth (100);
00092         Motors[i].setMaxSpeed (500);
00093         Motors[i].setAcceleration (100);
00094         Motors[i].moveTo(-90*q/1.8);
00095         Motors[i].setSpeed (50);
00096     }
00097 
00098     while(zero[0] || zero[1] || zero[2]) {
00099         for (int i = 0; i < 3; i++) {
00100             if(zero[i] != 0)
00101                 Motors[i].runSpeedToPosition();
00102             if (!Motors[i].distanceToGo())
00103                 goto setup;
00104         }
00105     }
00106 setup:
00107     for (int i = 0; i < 3; i++) {
00108         Motors[i].stop();
00109         Motors[i].setCurrentPosition(0);
00110     }
00111     myled = 1;
00112     pc.printf("Ready\r\n");
00113 }
00114 
00115 
00116 bool parseXYZ(char* m, float* A)
00117 {
00118     char buff[32] = "";
00119     int i = 0;
00120     if (m[i] != 'X')
00121         return false;
00122     i++;
00123     while (m[i] != 'Y') {
00124         if (m[i] == '\n')
00125             return false;
00126         strcat(buff, &m[i]);
00127         i++;
00128     }
00129     A[0] = atof(buff);
00130 
00131     strcpy(buff,"");
00132     i++;
00133     while (m[i] != 'Z') {
00134         if (m[i] == '\n')
00135             return false;
00136         strcat(buff, &m[i]);
00137         i++;
00138     }
00139     A[1] = atof(buff);
00140 
00141     strcpy(buff,"");
00142     i++;
00143     while (m[i] != '\n') {
00144         if (i > 32)
00145             return false;
00146         strcat(buff, &m[i]);
00147         i++;
00148     }
00149     A[2] = atof(buff);
00150     return true;
00151 }
00152 
00153 int delta_calcAngleYZ(float x0, float y0, float z0, float &theta)
00154 {
00155     float y1 = -0.5 * tg30 * f; // f/2 * tg 30
00156     y0 -= 0.5 * tg30    * e;    // shift center to edge
00157     // z = a + b*y
00158     float a = (x0 * x0 + y0 * y0 + z0 * z0 + rf * rf - re * re - y1 * y1) / (2 * z0);
00159     float b = (y1 - y0) / z0;
00160     // discriminant
00161     float d = -(a + b * y1) * (a + b * y1) + rf * (b * b * rf + rf);
00162     if (d < 0) return -1; // non-existing point
00163     float yj = (y1 - a * b - sqrt(d)) / (b * b + 1); // choosing outer point
00164     float zj = a + b * yj;
00165     theta = 180.0 * atan(-zj / (y1 - yj)) / pi + ((yj > y1) ? 180.0 : 0.0);
00166     return 0;
00167 }
00168 
00169 int delta_calcInverse(float* A, float* B)
00170 {
00171     float x0 = A[0];
00172     float y0 = A[1];
00173     float z0 = A[2];
00174     int status = delta_calcAngleYZ(x0, y0, z0, B[0]);
00175     if (status == 0) status = delta_calcAngleYZ(x0 * cos120 + y0 * sin120, y0 * cos120 - x0 * sin120, z0, B[1]); // rotate coords to +120 deg
00176     if (status == 0) status = delta_calcAngleYZ(x0 * cos120 - y0 * sin120, y0 * cos120 + x0 * sin120, z0, B[2]); // rotate coords to -120 deg
00177     return status;
00178 }