main.cpp

00001 //X300Y500Z1000\r\n - пример команды управления (3 угла в градусах)
00002 #include "stm32f103c8t6.h"
00003 #include "mbed.h"
00004 #include "AccelStepper.h"
00005 #include <ctype.h>
00006 
00007 const int q = 8;        // микрошаг
00008 #define MSpeed  2000    // максимальная скорость
00009 
00010 bool parseXYZ(char* m, float* A);
00011 float readNum(char* m, int i);
00012 void MotorsInit();
00013 void Rx_interrupt();
00014 
00015 PinName         stp[3] = {PB_15,PA_8,PA_9};
00016 PinName         dir[3] = {PB_12,PB_13,PB_14};
00017 AccelStepper    axis1(1, stp[0], dir[0]);
00018 AccelStepper    axis2(1, stp[1], dir[1]);
00019 AccelStepper    axis3(1, stp[2], dir[2]);
00020 AccelStepper    Motors[3] = {axis1, axis2, axis3};
00021 
00022 DigitalOut      myled(LED1);
00023 DigitalOut      enb(PA_10);
00024 Timer           t;
00025 Timer           t_my;
00026 Serial          pc(PB_10, PB_11);
00027 
00028 bool flags[3] = {true, true, true};
00029 ////////////////////////////////////////////////////////////////////////////////////
00030 int main()
00031 {
00032     confSysClock();     //Configure system clock (72MHz HSE clock, 48MHz USB clock)
00033     t.start();
00034     t_my.start();
00035     pc.baud(9600);
00036     pc.attach(&Rx_interrupt, Serial::RxIrq);
00037     pc.printf("Hello\r\n");
00038     MotorsInit();
00039     while(1) {
00040         for (int i = 0; i < 3; i++) {
00041             Motors[i].runSpeedToPosition();
00042             if (Motors[i].distanceToGo() == 0 && flags[i] == false) {
00043                 flags[i] = true;
00044                 pc.printf("%d - finish in %d msec\r\n", i, t_my.read_ms());
00045             }
00046         }
00047         if (Motors[0].distanceToGo() == 0 && Motors[1].distanceToGo() == 0 && Motors[2].distanceToGo() == 0)
00048             myled = 1;
00049     }
00050 }
00051 ///////////////////////////////////////////////////////////////////////////////////
00052 
00053 void MotorsInit()
00054 {
00055     enb = 0;
00056     myled = 0;
00057     for (int i = 0; i < 3; i++) {
00058         Motors[i].setMinPulseWidth (20);
00059         Motors[i].setMaxSpeed (MSpeed*2);
00060         Motors[i].setCurrentPosition(0);
00061     }
00062     myled = 1;
00063     pc.printf("Ready\r\n\r\n");
00064 }
00065 
00066 
00067 bool parseXYZ(char* m, float* A)
00068 {
00069     for(int i = 0; i < 32; i++) {
00070         if (m[i] == '\n' || m[i] == '\r')
00071             break;
00072         /////////////////////////////////
00073         if (m[i] == 'X') {
00074             i++;
00075             A[0] = readNum(m, i);
00076         }
00077         ////////////////////////////////
00078         if (m[i] == 'Y') {
00079             i++;
00080             A[1] = readNum(m, i);
00081         }
00082         //////////////////////////////////
00083         if (m[i] == 'Z') {
00084             i++;
00085             A[2] = readNum(m, i);
00086         }
00087     }
00088     return true;
00089 }
00090 
00091 float readNum(char* m, int i)
00092 {
00093     char buff[10] = "";
00094     int n = 0;
00095     while ((isdigit(m[i]) || m[i] == '.' || m[i] == '-') && n < 9) {
00096         buff[n] = m[i];
00097         i++;
00098         n++;
00099     }
00100     if (n == 0) {
00101         buff[n] = '0';
00102         n++;
00103     }
00104     buff[n] = '\0';
00105     return(atof(buff));
00106 }
00107 
00108 
00109 void constTime()
00110 {
00111     float DeltaMax = 0;
00112     for (int i = 0; i < 3; i++) {
00113         if(abs(Motors[i].distanceToGo()) > DeltaMax)
00114             DeltaMax = abs(Motors[i].distanceToGo());
00115     }
00116     float k = MSpeed/DeltaMax;
00117     for (int i = 0; i < 3; i++) {
00118         Motors[i].setSpeed(k*abs(Motors[i].distanceToGo()));
00119         pc.printf("\r\nSpeed %d", int(k*abs(Motors[i].distanceToGo())));
00120     }
00121 }
00122 
00123 void Rx_interrupt()
00124 {
00125     char message[32] = "";
00126     pc.scanf("%[^\n]s", message);
00127     strcat(message, "\n");
00128     pc.printf("\r\n%s", message);
00129 
00130     float angles[3] = {0, 0, 0};
00131     parseXYZ(message, angles);
00132     pc.printf("Angle: ");
00133     for (int i = 0; i < 3; i++) {
00134         pc.printf("%.2f ", angles[i]);
00135         Motors[i].move(int(angles[i]*q/1.8));
00136         flags[i] = false;
00137     }
00138     constTime();
00139     myled = 0;
00140     t_my.stop();
00141     t_my.reset();
00142     t_my.start();
00143     pc.printf("\r\nWait!\r\n");
00144     return;
00145 }