![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Одновременное управление 3 шаговыми двигателями
Dependencies: AccelStepper mbed-STM32F103C8T6 mbed
Fork of Delta_Robot_2018 by
Revision 1:2b8f94cdaaea, committed 2018-07-08
- Comitter:
- yuliyasm
- Date:
- Sun Jul 08 20:46:00 2018 +0000
- Parent:
- 0:1825dad3704f
- Commit message:
- 1
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Mon May 21 08:05:59 2018 +0000 +++ b/main.cpp Sun Jul 08 20:46:00 2018 +0000 @@ -1,24 +1,14 @@ -//X-300Y22.5Z-300\r\n - пример команды управления +//X300Y500Z1000\r\n - пример команды управления (3 угла в градусах) #include "stm32f103c8t6.h" #include "mbed.h" #include "AccelStepper.h" #include <ctype.h> -const float r = 25; // end effector -const float R = 80; // base -const float l = 225.5; -const float L = 100; const int q = 8; // микрошаг - -#define pi 3.14159 -#define cos120 -0.5 -#define sin120 0.866 -#define tg30 0.57735 -#define MAXP 500 // предельные координаты +#define MSpeed 2000 // максимальная скорость bool parseXYZ(char* m, float* A); float readNum(char* m, int i); -bool delta_calcInverse(float* A, float* B); void MotorsInit(); void Rx_interrupt(); @@ -28,28 +18,32 @@ AccelStepper axis2(1, stp[1], dir[1]); AccelStepper axis3(1, stp[2], dir[2]); AccelStepper Motors[3] = {axis1, axis2, axis3}; -DigitalIn zero1(PB_10); -DigitalIn zero2(PB_1); -DigitalIn zero3(PB_0); -DigitalIn zero[3] = {zero1, zero2, zero3}; + DigitalOut myled(LED1); DigitalOut enb(PA_10); Timer t; +Timer t_my; Serial pc(PB_10, PB_11); +bool flags[3] = {true, true, true}; //////////////////////////////////////////////////////////////////////////////////// int main() { confSysClock(); //Configure system clock (72MHz HSE clock, 48MHz USB clock) t.start(); + t_my.start(); pc.baud(9600); pc.attach(&Rx_interrupt, Serial::RxIrq); pc.printf("Hello\r\n"); MotorsInit(); - while(1) { - for (int i = 0; i < 3; i++) - Motors[i].run(); + for (int i = 0; i < 3; i++) { + Motors[i].runSpeedToPosition(); + if (Motors[i].distanceToGo() == 0 && flags[i] == false) { + flags[i] = true; + pc.printf("%d - finish in %d msec\r\n", i, t_my.read_ms()); + } + } if (Motors[0].distanceToGo() == 0 && Motors[1].distanceToGo() == 0 && Motors[2].distanceToGo() == 0) myled = 1; } @@ -60,24 +54,9 @@ { enb = 0; myled = 0; - for (int i = 0; i < 3; i++) { - Motors[i].setMinPulseWidth (100); - Motors[i].setMaxSpeed (500); - Motors[i].setAcceleration (100); - Motors[i].moveTo(-90*q/1.8); - Motors[i].setSpeed (50); - } - while(Motors[0].distanceToGo() && Motors[1].distanceToGo() && Motors[2].distanceToGo()) { - for (int i = 0; i < 3; i++) { - if(zero[i] != 0) - Motors[i].runSpeedToPosition(); - else - Motors[i].moveTo(Motors[i].currentPosition()); - } - } - for (int i = 0; i < 3; i++) { - Motors[i].stop(); + Motors[i].setMinPulseWidth (20); + Motors[i].setMaxSpeed (MSpeed*2); Motors[i].setCurrentPosition(0); } myled = 1; @@ -106,10 +85,6 @@ A[2] = readNum(m, i); } } - for(int i = 0; i < 3; i++) { // проверка на адекватность - if ((A[i] <= -MAXP) || (A[i] >= MAXP)) - return false; - } return true; } @@ -122,51 +97,27 @@ i++; n++; } + if (n == 0) { + buff[n] = '0'; + n++; + } buff[n] = '\0'; - if (n == 0) - return (MAXP); return(atof(buff)); } -bool delta_calcAngleYZ(float x0, float y0, float z0, float &theta) -{ - float y1 = -R; // f/2 * tg 30 - y0 = y0-r; // shift center to edge - // z = a + b*y - float a = (x0*x0 + y0*y0 + z0*z0 + L*L - l*l - y1*y1) / (2 * z0); - float b = (y1 - y0) / z0; - // discriminant - float D = -(a + b * y1)*(a + b * y1) + L*L * (b*b + 1); - if (D < 0) return false; // non-existing point - float yj = (y1 - a * b - sqrt(D)) / (b*b + 1); // choosing outer point - float zj = a + b * yj; - theta = atan(-zj / (y1 - yj)) + ((yj > y1) ? pi : 0); - theta = 180*theta/pi; - return true; -} - -bool delta_calcInverse(float* A, float* B) +void constTime() { - if (A[2] > 0) - return false; - float x0 = A[0]; - float y0 = A[1]; - float z0 = A[2]; - int status = delta_calcAngleYZ(x0, y0, z0, B[0]); - if (status) { - x0 = A[0]*cos120 + A[1]*sin120; - y0 = A[1]*cos120 - A[0]*sin120; - z0 = A[2]; - status = delta_calcAngleYZ(x0, y0, z0, B[1]); - } // rotate coords to +120 deg - if (status) { - x0 = A[0]*cos120 - A[1]*sin120; - y0 = A[1]*cos120 + A[0]*sin120; - z0 = A[2]; - status = delta_calcAngleYZ(x0, y0, z0, B[2]); - } // rotate coords to -120 deg - return status; + float DeltaMax = 0; + for (int i = 0; i < 3; i++) { + if(abs(Motors[i].distanceToGo()) > DeltaMax) + DeltaMax = abs(Motors[i].distanceToGo()); + } + float k = MSpeed/DeltaMax; + for (int i = 0; i < 3; i++) { + Motors[i].setSpeed(k*abs(Motors[i].distanceToGo())); + pc.printf("\r\nSpeed %d", int(k*abs(Motors[i].distanceToGo()))); + } } void Rx_interrupt() @@ -174,30 +125,21 @@ char message[32] = ""; pc.scanf("%[^\n]s", message); strcat(message, "\n"); - pc.printf("%s", message); - - float point[3] = {MAXP, MAXP, MAXP}; - float angles[3] = {0, 0, 0}; + pc.printf("\r\n%s", message); - bool p = parseXYZ(message, point); - if(p == false) { - pc.printf("Parse fail\r\n"); - return; - } - pc.printf("Point: %.2f ", point[0]); - pc.printf("%.2f ", point[1]); - pc.printf("%.2f\r\n", point[2]); - p = delta_calcInverse(point, angles); - if(p == false) { - pc.printf("Kinematic fail\r\n"); - return; - } + float angles[3] = {0, 0, 0}; + parseXYZ(message, angles); pc.printf("Angle: "); for (int i = 0; i < 3; i++) { pc.printf("%.2f ", angles[i]); - Motors[i].moveTo(int(angles[i]*q/1.8)); + Motors[i].move(int(angles[i]*q/1.8)); + flags[i] = false; } + constTime(); myled = 0; - pc.printf("\r\nSuccess!\r\n\r\n"); + t_my.stop(); + t_my.reset(); + t_my.start(); + pc.printf("\r\nWait!\r\n"); return; } \ No newline at end of file