4653
Dependencies: mbed-STM32F103C8T6 mbed USBDevice_STM32F103
Diff: main.cpp
- Revision:
- 3:086f8c1079ff
- Parent:
- 2:d4dad64faadb
diff -r d4dad64faadb -r 086f8c1079ff main.cpp --- a/main.cpp Sat Feb 03 19:25:25 2018 +0000 +++ b/main.cpp Mon May 07 11:41:10 2018 +0000 @@ -1,88 +1,118 @@ #include "stm32f103c8t6.h" #include "mbed.h" -#include "USBSerial.h" #include <string> #include "AccelStepper.h" //X20Y20Z-300\r - пример команды - -const float e = 24; // end effector -const float f = 75; // base -const float re = 300; +const float e = 20; // end effector +const float f = 80; // base +const float re = 225; const float rf = 100; const float pi = 3.14; const float cos120 = -0.5; const float sin120 = sqrt(3.0) / 2; const float tg30 = 1 / sqrt(3.0); -const float q = 1; //микрошаг +const float q = 8; //микрошаг bool parseXYZ(char* m, float* A); int delta_calcInverse(float* A, float* B); +void MotorsInit(); PinName stp[3] = {PB_15,PA_8,PA_9}; PinName dir[3] = {PB_12,PB_13,PB_14}; +AccelStepper axis1(1, stp[0], dir[0]); +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}; +Serial pc(PA_2, PA_3); +DigitalOut myled(LED1); +DigitalOut enb(PA_10); -AccelStepper Xaxis(1, stp[0], dir[0]); -AccelStepper Yaxis(1, stp[1], dir[1]); -AccelStepper Zaxis(1, stp[2], dir[2]); -AccelStepper Motors[3] = {Xaxis, Yaxis, Zaxis}; Timer t; int main() { confSysClock(); //Configure system clock (72MHz HSE clock, 48MHz USB clock) - USBSerial usbSerial(0x1f00, 0x2012, 0x0001, false); - //DigitalOut myled(LED1); - DigitalOut enb(PA_10); + pc.baud(9600); t.start(); - for (int i = 0; i < 3; i++) { - Motors[i].setMaxSpeed(1000); - Motors[i].setAcceleration(100); - Motors[i].setMinPulseWidth(10000); - //Motors[i].moveTo(500); - } - - usbSerial.printf("Ready\r\n"); - + MotorsInit(); + float angles[3] = {0, 0, 0}; + float point[3] = {0, 0, 0}; + char message[32] = ""; + char c = '\0'; while(1) { - enb = 1; - /*myled = 0; // turn the LED on - wait_ms(200); // 200 millisecond - myled = 1; // turn the LED off - wait_ms(1000); // 1000 millisecond - usbSerial.printf("Blink\r\n");*/ + for (int i = 0; i < 3; i++) + Motors[i].run(); + if (Motors[0].distanceToGo() == 0) + myled = 1; - if (usbSerial.available()) { - char message[32]; - wait_ms(2); - usbSerial.scanf("%s", message); - strcat(message, "\r"); - usbSerial.printf("%s", message); + if (pc.readable()) { + wait_ms(300); + strcpy(message, ""); + pc.scanf("%s\r\n", message); + strcat(message, "\n"); + pc.printf("%s", message); - float point[3]; bool p = parseXYZ(message, point); - if(p == false) - return; - usbSerial.printf("%.2f \r", point[0]); - usbSerial.printf("%.2f \r", point[1]); - usbSerial.printf("%.2f \r", point[2]); + if(p == false) { + pc.printf("Parse fail"); + continue; + } + pc.printf("%.2f ", point[0]); + pc.printf("%.2f ", point[1]); + pc.printf("%.2f \n", point[2]); + delta_calcInverse(point, angles); - float angles[3]; - if (!delta_calcInverse(point, angles)) { - for (int i = 0; i < 3; i++) { - usbSerial.printf("%.2f \r", angles[i]); - Motors[i].moveTo(angles[i]*q/1.8); - } + for (int i = 0; i < 3; i++) { + pc.printf("%.2f ", angles[i]); + Motors[i].moveTo(int(angles[i]*q/1.8)); + myled = 0; + //Motors[i].runToPosition(); + //myled = 1; } } - for (int i = 0; i < 3; i++) - Motors[i].runToPosition(); } } +void MotorsInit() +{ + enb = 0; + myled = 0; + pc.printf("Hello\r\n"); + + 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(zero[0] || zero[1] || zero[2]) { + for (int i = 0; i < 3; i++) { + if(zero[i] != 0) + Motors[i].runSpeedToPosition(); + if (!Motors[i].distanceToGo()) + goto setup; + } + } +setup: + for (int i = 0; i < 3; i++) { + Motors[i].stop(); + Motors[i].setCurrentPosition(0); + } + myled = 1; + pc.printf("Ready\r\n"); +} + + bool parseXYZ(char* m, float* A) { char buff[32] = ""; @@ -90,7 +120,9 @@ if (m[i] != 'X') return false; i++; - while (m[i] != 'Y' && i < 32) { + while (m[i] != 'Y') { + if (m[i] == '\n') + return false; strcat(buff, &m[i]); i++; } @@ -98,7 +130,9 @@ strcpy(buff,""); i++; - while (m[i] != 'Z' && i < 32) { + while (m[i] != 'Z') { + if (m[i] == '\n') + return false; strcat(buff, &m[i]); i++; } @@ -106,12 +140,13 @@ strcpy(buff,""); i++; - while (m[i] != '\r' && i < 32) { + while (m[i] != '\n') { + if (i > 32) + return false; strcat(buff, &m[i]); i++; } A[2] = atof(buff); - return true; } @@ -131,8 +166,6 @@ return 0; } -// inverse kinematics: (x0, y0, z0) -> (theta1, theta2, theta3) -// returned status: 0=OK, -1=non-existing position int delta_calcInverse(float* A, float* B) { float x0 = A[0]; @@ -142,4 +175,4 @@ if (status == 0) status = delta_calcAngleYZ(x0 * cos120 + y0 * sin120, y0 * cos120 - x0 * sin120, z0, B[1]); // rotate coords to +120 deg if (status == 0) status = delta_calcAngleYZ(x0 * cos120 - y0 * sin120, y0 * cos120 + x0 * sin120, z0, B[2]); // rotate coords to -120 deg return status; -} +} \ No newline at end of file