4653
Dependencies: mbed-STM32F103C8T6 mbed USBDevice_STM32F103
Diff: main.cpp
- Revision:
- 1:f9c784a35e9c
- Parent:
- 0:3cf5622d5b76
- Child:
- 2:d4dad64faadb
--- a/main.cpp Mon Jan 29 21:59:00 2018 +0000 +++ b/main.cpp Tue Jan 30 18:33:46 2018 +0000 @@ -1,20 +1,119 @@ #include "stm32f103c8t6.h" #include "mbed.h" - -int main() { +#include "USBSerial.h" +#include <string> + +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); + + +int main() +{ confSysClock(); //Configure system clock (72MHz HSE clock, 48MHz USB clock) - - Serial pc(PA_2, PA_3); + USBSerial usbSerial(0x1f00, 0x2012, 0x0001, false); DigitalOut myled(LED1); - + while(1) { - // The on-board LED is connected, via a resistor, to +3.3V (not to GND). - // So to turn the LED on or off we have to set it to 0 or 1 respectively myled = 0; // turn the LED on wait_ms(200); // 200 millisecond myled = 1; // turn the LED off wait_ms(1000); // 1000 millisecond - pc.printf("Blink\r\n"); + usbSerial.printf("Blink\r\n"); + + char message[32]; + if (usbSerial.available()) { + wait_ms(2); + usbSerial.scanf("%s", message); + strcat(message, "\r"); + usbSerial.printf("%s", message); + + float A[32]; + bool p = parseXYZ(message, A); + if(p == false) + return; + usbSerial.printf("%.2f \r", A[0]); + usbSerial.printf("%.2f \r", A[1]); + usbSerial.printf("%.2f \r", A[2]); + + float B[3]; + if (!delta_calcInverse(A, B)) { + for (int i = 0; i < 3; i++) { + usbSerial.printf("%.2f \r", B[i]); + //Motors[i].moveTo(B[i]*q/1.8); + } + } + } } } - \ No newline at end of file + +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; +}