main.cpp

00001 //X-300Y22.5Z-300\r\n - пример команды управления
00002 #include "stm32f103c8t6.h"
00003 #include "mbed.h"
00004 #include "AccelStepper.h"
00005 #include <ctype.h>
00006 
00007 const float r = 25;     // end effector
00008 const float R = 80;     // base
00009 const float l = 225.5;
00010 const float L = 100;
00011 const int q = 8;        // микрошаг
00012 
00013 #define pi      3.14159
00014 #define cos120  -0.5
00015 #define sin120  0.866
00016 #define tg30    0.57735
00017 #define MAXP    500     // предельные координаты
00018 
00019 bool parseXYZ(char* m, float* A);
00020 float readNum(char* m, int i);
00021 bool delta_calcInverse(float* A, float* B);
00022 void MotorsInit();
00023 void Rx_interrupt();
00024 
00025 PinName         stp[3] = {PB_15,PA_8,PA_9};
00026 PinName         dir[3] = {PB_12,PB_13,PB_14};
00027 AccelStepper    axis1(1, stp[0], dir[0]);
00028 AccelStepper    axis2(1, stp[1], dir[1]);
00029 AccelStepper    axis3(1, stp[2], dir[2]);
00030 AccelStepper    Motors[3] = {axis1, axis2, axis3};
00031 DigitalIn       zero1(PA_15);
00032 DigitalIn       zero2(PA_12);
00033 DigitalIn       zero3(PA_11);
00034 DigitalIn       zero[3] = {zero1, zero2, zero3};
00035 DigitalOut      myled(LED1);
00036 DigitalOut      enb(PA_10);
00037 Timer           t;
00038 Serial          pc(PB_10, PB_11);
00039 
00040 ////////////////////////////////////////////////////////////////////////////////////
00041 int main()
00042 {
00043     confSysClock();     //Configure system clock (72MHz HSE clock, 48MHz USB clock)
00044     t.start();
00045     pc.baud(9600);
00046     pc.attach(&Rx_interrupt, Serial::RxIrq);
00047     pc.printf("Hello\r\n");
00048     MotorsInit();
00049 
00050     while(1) {
00051         for (int i = 0; i < 3; i++)
00052             Motors[i].run();
00053         if (Motors[0].distanceToGo() == 0 && Motors[1].distanceToGo() == 0 && Motors[2].distanceToGo() == 0)
00054             myled = 1;
00055     }
00056 }
00057 ///////////////////////////////////////////////////////////////////////////////////
00058 
00059 void MotorsInit()
00060 {
00061     enb = 0;
00062     myled = 0;
00063 
00064     for (int i = 0; i < 3; i++) {
00065         Motors[i].setMinPulseWidth (100);
00066         Motors[i].setMaxSpeed (500);
00067         Motors[i].setAcceleration (100);
00068         Motors[i].moveTo(-90*q/1.8);
00069     }
00070     while(Motors[0].distanceToGo() || Motors[1].distanceToGo() || Motors[2].distanceToGo()) {
00071         for (int i = 0; i < 3; i++) {
00072             if(zero[i] != 0)
00073                 Motors[i].run();
00074             else
00075                 Motors[i].moveTo(Motors[i].currentPosition());
00076         }
00077     }
00078     for (int i = 0; i < 3; i++) {
00079         Motors[i].stop();
00080         Motors[i].setCurrentPosition(0);
00081         Motors[i].setMaxSpeed (2000);
00082         Motors[i].setAcceleration (500);
00083     }
00084     myled = 1;
00085     pc.printf("Ready\r\n\r\n");
00086 }
00087 
00088 
00089 bool parseXYZ(char* m, float* A)
00090 {
00091     for(int i = 0; i < 32; i++) {
00092         if (m[i] == '\n' || m[i] == '\r')
00093             break;
00094         /////////////////////////////////
00095         if (m[i] == 'X') {
00096             i++;
00097             A[0] = readNum(m, i);
00098         }
00099         ////////////////////////////////
00100         if (m[i] == 'Y') {
00101             i++;
00102             A[1] = readNum(m, i);
00103         }
00104         //////////////////////////////////
00105         if (m[i] == 'Z') {
00106             i++;
00107             A[2] = readNum(m, i);
00108         }
00109     }
00110     for(int i = 0; i < 3; i++) { // проверка на адекватность
00111         if ((A[i] <= -MAXP) || (A[i] >= MAXP))
00112             return false;
00113     }
00114     return true;
00115 }
00116 
00117 float readNum(char* m, int i)
00118 {
00119     char buff[10] = "";
00120     int n = 0;
00121     while ((isdigit(m[i]) || m[i] == '.' || m[i] == '-') && n < 9) {
00122         buff[n] = m[i];
00123         i++;
00124         n++;
00125     }
00126     buff[n] = '\0';
00127     if (n == 0)
00128         return (MAXP);
00129     return(atof(buff));
00130 }
00131 
00132 
00133 bool delta_calcAngleYZ(float x0, float y0, float z0, float &theta)
00134 {
00135     float y1 = -R; // f/2 * tg 30
00136     y0 = y0-r;    // shift center to edge
00137     // z = a + b*y
00138     float a = (x0*x0 + y0*y0 + z0*z0 + L*L - l*l - y1*y1) / (2 * z0);
00139     float b = (y1 - y0) / z0;
00140     // discriminant
00141     float D = -(a + b * y1)*(a + b * y1) + L*L * (b*b + 1);
00142     if (D < 0) return false; // non-existing point
00143     float yj = (y1 - a * b - sqrt(D)) / (b*b + 1); // choosing outer point
00144     float zj = a + b * yj;
00145     theta = atan(-zj / (y1 - yj)) + ((yj > y1) ? pi : 0);
00146     theta = 180*theta/pi;
00147     return true;
00148 }
00149 
00150 bool delta_calcInverse(float* A, float* B)
00151 {
00152     if (A[2] > 0)
00153         return false;
00154     float x0 = A[0];
00155     float y0 = A[1];
00156     float z0 = A[2];
00157     int status = delta_calcAngleYZ(x0, y0, z0, B[0]);
00158     if (status) {
00159         x0 = A[0]*cos120 + A[1]*sin120;
00160         y0 = A[1]*cos120 - A[0]*sin120;
00161         z0 = A[2];
00162         status = delta_calcAngleYZ(x0, y0, z0, B[1]);
00163     } // rotate coords to +120 deg
00164     if (status) {
00165         x0 = A[0]*cos120 - A[1]*sin120;
00166         y0 = A[1]*cos120 + A[0]*sin120;
00167         z0 = A[2];
00168         status = delta_calcAngleYZ(x0, y0, z0, B[2]);
00169     } // rotate coords to -120 deg
00170     return status;
00171 }
00172 
00173 void Rx_interrupt()
00174 {
00175     char message[32] = "";
00176     pc.scanf("%[^\n]s", message);
00177     strcat(message, "\n");
00178     pc.printf("%s", message);
00179 
00180     float point[3] = {MAXP, MAXP, MAXP};
00181     float angles[3] = {0, 0, 0};
00182 
00183     bool p = parseXYZ(message, point);
00184     if(p == false) {
00185         pc.printf("Parse fail\r\n");
00186         return;
00187     }
00188     pc.printf("Point: %.2f ", point[0]);
00189     pc.printf("%.2f ", point[1]);
00190     pc.printf("%.2f\r\n", point[2]);
00191     p = delta_calcInverse(point, angles);
00192     if(p == false) {
00193         pc.printf("Kinematic fail\r\n");
00194         return;
00195     }
00196     pc.printf("Angle: ");
00197     for (int i = 0; i < 3; i++) {
00198         pc.printf("%.2f ", angles[i]);
00199         Motors[i].moveTo(int(angles[i]*q/1.8));
00200     }
00201     myled = 0;
00202     pc.printf("\r\nSuccess!\r\n\r\n");
00203     return;
00204 }