4653
Dependencies: mbed-STM32F103C8T6 mbed USBDevice_STM32F103
main.cpp
- Committer:
- yuliyasm
- Date:
- 2018-02-03
- Revision:
- 2:d4dad64faadb
- Parent:
- 1:f9c784a35e9c
- Child:
- 3:086f8c1079ff
File content as of revision 2:d4dad64faadb:
#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 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; //микрошаг bool parseXYZ(char* m, float* A); int delta_calcInverse(float* A, float* B); PinName stp[3] = {PB_15,PA_8,PA_9}; PinName dir[3] = {PB_12,PB_13,PB_14}; 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); 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"); 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");*/ if (usbSerial.available()) { char message[32]; wait_ms(2); usbSerial.scanf("%s", message); strcat(message, "\r"); usbSerial.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]); 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++) Motors[i].runToPosition(); } } bool parseXYZ(char* m, float* A) { char buff[32] = ""; int i = 0; if (m[i] != 'X') return false; i++; while (m[i] != 'Y' && i < 32) { strcat(buff, &m[i]); i++; } A[0] = atof(buff); strcpy(buff,""); i++; while (m[i] != 'Z' && i < 32) { strcat(buff, &m[i]); i++; } A[1] = atof(buff); strcpy(buff,""); i++; while (m[i] != '\r' && i < 32) { strcat(buff, &m[i]); i++; } A[2] = atof(buff); return true; } int delta_calcAngleYZ(float x0, float y0, float z0, float &theta) { float y1 = -0.5 * tg30 * f; // f/2 * tg 30 y0 -= 0.5 * tg30 * e; // shift center to edge // z = a + b*y float a = (x0 * x0 + y0 * y0 + z0 * z0 + rf * rf - re * re - y1 * y1) / (2 * z0); float b = (y1 - y0) / z0; // discriminant float d = -(a + b * y1) * (a + b * y1) + rf * (b * b * rf + rf); if (d < 0) return -1; // non-existing point float yj = (y1 - a * b - sqrt(d)) / (b * b + 1); // choosing outer point float zj = a + b * yj; theta = 180.0 * atan(-zj / (y1 - yj)) / pi + ((yj > y1) ? 180.0 : 0.0); 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]; float y0 = A[1]; float z0 = A[2]; int status = delta_calcAngleYZ(x0, y0, z0, B[0]); 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; }