4653
Dependencies: mbed-STM32F103C8T6 mbed USBDevice_STM32F103
main.cpp@1:f9c784a35e9c, 2018-01-30 (annotated)
- Committer:
- yuliyasm
- Date:
- Tue Jan 30 18:33:46 2018 +0000
- Revision:
- 1:f9c784a35e9c
- Parent:
- 0:3cf5622d5b76
- Child:
- 2:d4dad64faadb
it works
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yuliyasm | 0:3cf5622d5b76 | 1 | #include "stm32f103c8t6.h" |
yuliyasm | 0:3cf5622d5b76 | 2 | #include "mbed.h" |
yuliyasm | 1:f9c784a35e9c | 3 | #include "USBSerial.h" |
yuliyasm | 1:f9c784a35e9c | 4 | #include <string> |
yuliyasm | 1:f9c784a35e9c | 5 | |
yuliyasm | 1:f9c784a35e9c | 6 | const float e = 24; // end effector |
yuliyasm | 1:f9c784a35e9c | 7 | const float f = 75; // base |
yuliyasm | 1:f9c784a35e9c | 8 | const float re = 300; |
yuliyasm | 1:f9c784a35e9c | 9 | const float rf = 100; |
yuliyasm | 1:f9c784a35e9c | 10 | |
yuliyasm | 1:f9c784a35e9c | 11 | const float pi = 3.14; |
yuliyasm | 1:f9c784a35e9c | 12 | const float cos120 = -0.5; |
yuliyasm | 1:f9c784a35e9c | 13 | const float sin120 = sqrt(3.0) / 2; |
yuliyasm | 1:f9c784a35e9c | 14 | const float tg30 = 1 / sqrt(3.0); |
yuliyasm | 1:f9c784a35e9c | 15 | const float q = 1; |
yuliyasm | 1:f9c784a35e9c | 16 | |
yuliyasm | 1:f9c784a35e9c | 17 | bool parseXYZ(char* m, float* A); |
yuliyasm | 1:f9c784a35e9c | 18 | int delta_calcInverse(float* A, float* B); |
yuliyasm | 1:f9c784a35e9c | 19 | |
yuliyasm | 1:f9c784a35e9c | 20 | |
yuliyasm | 1:f9c784a35e9c | 21 | int main() |
yuliyasm | 1:f9c784a35e9c | 22 | { |
yuliyasm | 0:3cf5622d5b76 | 23 | confSysClock(); //Configure system clock (72MHz HSE clock, 48MHz USB clock) |
yuliyasm | 1:f9c784a35e9c | 24 | USBSerial usbSerial(0x1f00, 0x2012, 0x0001, false); |
yuliyasm | 0:3cf5622d5b76 | 25 | DigitalOut myled(LED1); |
yuliyasm | 1:f9c784a35e9c | 26 | |
yuliyasm | 0:3cf5622d5b76 | 27 | while(1) { |
yuliyasm | 0:3cf5622d5b76 | 28 | myled = 0; // turn the LED on |
yuliyasm | 0:3cf5622d5b76 | 29 | wait_ms(200); // 200 millisecond |
yuliyasm | 0:3cf5622d5b76 | 30 | myled = 1; // turn the LED off |
yuliyasm | 0:3cf5622d5b76 | 31 | wait_ms(1000); // 1000 millisecond |
yuliyasm | 1:f9c784a35e9c | 32 | usbSerial.printf("Blink\r\n"); |
yuliyasm | 1:f9c784a35e9c | 33 | |
yuliyasm | 1:f9c784a35e9c | 34 | char message[32]; |
yuliyasm | 1:f9c784a35e9c | 35 | if (usbSerial.available()) { |
yuliyasm | 1:f9c784a35e9c | 36 | wait_ms(2); |
yuliyasm | 1:f9c784a35e9c | 37 | usbSerial.scanf("%s", message); |
yuliyasm | 1:f9c784a35e9c | 38 | strcat(message, "\r"); |
yuliyasm | 1:f9c784a35e9c | 39 | usbSerial.printf("%s", message); |
yuliyasm | 1:f9c784a35e9c | 40 | |
yuliyasm | 1:f9c784a35e9c | 41 | float A[32]; |
yuliyasm | 1:f9c784a35e9c | 42 | bool p = parseXYZ(message, A); |
yuliyasm | 1:f9c784a35e9c | 43 | if(p == false) |
yuliyasm | 1:f9c784a35e9c | 44 | return; |
yuliyasm | 1:f9c784a35e9c | 45 | usbSerial.printf("%.2f \r", A[0]); |
yuliyasm | 1:f9c784a35e9c | 46 | usbSerial.printf("%.2f \r", A[1]); |
yuliyasm | 1:f9c784a35e9c | 47 | usbSerial.printf("%.2f \r", A[2]); |
yuliyasm | 1:f9c784a35e9c | 48 | |
yuliyasm | 1:f9c784a35e9c | 49 | float B[3]; |
yuliyasm | 1:f9c784a35e9c | 50 | if (!delta_calcInverse(A, B)) { |
yuliyasm | 1:f9c784a35e9c | 51 | for (int i = 0; i < 3; i++) { |
yuliyasm | 1:f9c784a35e9c | 52 | usbSerial.printf("%.2f \r", B[i]); |
yuliyasm | 1:f9c784a35e9c | 53 | //Motors[i].moveTo(B[i]*q/1.8); |
yuliyasm | 1:f9c784a35e9c | 54 | } |
yuliyasm | 1:f9c784a35e9c | 55 | } |
yuliyasm | 1:f9c784a35e9c | 56 | } |
yuliyasm | 0:3cf5622d5b76 | 57 | } |
yuliyasm | 0:3cf5622d5b76 | 58 | } |
yuliyasm | 1:f9c784a35e9c | 59 | |
yuliyasm | 1:f9c784a35e9c | 60 | bool parseXYZ(char* m, float* A) |
yuliyasm | 1:f9c784a35e9c | 61 | { |
yuliyasm | 1:f9c784a35e9c | 62 | char buff[32] = ""; |
yuliyasm | 1:f9c784a35e9c | 63 | int i = 0; |
yuliyasm | 1:f9c784a35e9c | 64 | if (m[i] != 'X') |
yuliyasm | 1:f9c784a35e9c | 65 | return false; |
yuliyasm | 1:f9c784a35e9c | 66 | i++; |
yuliyasm | 1:f9c784a35e9c | 67 | while (m[i] != 'Y' && i < 32) { |
yuliyasm | 1:f9c784a35e9c | 68 | strcat(buff, &m[i]); |
yuliyasm | 1:f9c784a35e9c | 69 | i++; |
yuliyasm | 1:f9c784a35e9c | 70 | } |
yuliyasm | 1:f9c784a35e9c | 71 | A[0] = atof(buff); |
yuliyasm | 1:f9c784a35e9c | 72 | |
yuliyasm | 1:f9c784a35e9c | 73 | strcpy(buff,""); |
yuliyasm | 1:f9c784a35e9c | 74 | i++; |
yuliyasm | 1:f9c784a35e9c | 75 | while (m[i] != 'Z' && i < 32) { |
yuliyasm | 1:f9c784a35e9c | 76 | strcat(buff, &m[i]); |
yuliyasm | 1:f9c784a35e9c | 77 | i++; |
yuliyasm | 1:f9c784a35e9c | 78 | } |
yuliyasm | 1:f9c784a35e9c | 79 | A[1] = atof(buff); |
yuliyasm | 1:f9c784a35e9c | 80 | |
yuliyasm | 1:f9c784a35e9c | 81 | strcpy(buff,""); |
yuliyasm | 1:f9c784a35e9c | 82 | i++; |
yuliyasm | 1:f9c784a35e9c | 83 | while (m[i] != '\r' && i < 32) { |
yuliyasm | 1:f9c784a35e9c | 84 | strcat(buff, &m[i]); |
yuliyasm | 1:f9c784a35e9c | 85 | i++; |
yuliyasm | 1:f9c784a35e9c | 86 | } |
yuliyasm | 1:f9c784a35e9c | 87 | A[2] = atof(buff); |
yuliyasm | 1:f9c784a35e9c | 88 | |
yuliyasm | 1:f9c784a35e9c | 89 | return true; |
yuliyasm | 1:f9c784a35e9c | 90 | } |
yuliyasm | 1:f9c784a35e9c | 91 | |
yuliyasm | 1:f9c784a35e9c | 92 | int delta_calcAngleYZ(float x0, float y0, float z0, float &theta) |
yuliyasm | 1:f9c784a35e9c | 93 | { |
yuliyasm | 1:f9c784a35e9c | 94 | float y1 = -0.5 * tg30 * f; // f/2 * tg 30 |
yuliyasm | 1:f9c784a35e9c | 95 | y0 -= 0.5 * tg30 * e; // shift center to edge |
yuliyasm | 1:f9c784a35e9c | 96 | // z = a + b*y |
yuliyasm | 1:f9c784a35e9c | 97 | float a = (x0 * x0 + y0 * y0 + z0 * z0 + rf * rf - re * re - y1 * y1) / (2 * z0); |
yuliyasm | 1:f9c784a35e9c | 98 | float b = (y1 - y0) / z0; |
yuliyasm | 1:f9c784a35e9c | 99 | // discriminant |
yuliyasm | 1:f9c784a35e9c | 100 | float d = -(a + b * y1) * (a + b * y1) + rf * (b * b * rf + rf); |
yuliyasm | 1:f9c784a35e9c | 101 | if (d < 0) return -1; // non-existing point |
yuliyasm | 1:f9c784a35e9c | 102 | float yj = (y1 - a * b - sqrt(d)) / (b * b + 1); // choosing outer point |
yuliyasm | 1:f9c784a35e9c | 103 | float zj = a + b * yj; |
yuliyasm | 1:f9c784a35e9c | 104 | theta = 180.0 * atan(-zj / (y1 - yj)) / pi + ((yj > y1) ? 180.0 : 0.0); |
yuliyasm | 1:f9c784a35e9c | 105 | return 0; |
yuliyasm | 1:f9c784a35e9c | 106 | } |
yuliyasm | 1:f9c784a35e9c | 107 | |
yuliyasm | 1:f9c784a35e9c | 108 | // inverse kinematics: (x0, y0, z0) -> (theta1, theta2, theta3) |
yuliyasm | 1:f9c784a35e9c | 109 | // returned status: 0=OK, -1=non-existing position |
yuliyasm | 1:f9c784a35e9c | 110 | int delta_calcInverse(float* A, float* B) |
yuliyasm | 1:f9c784a35e9c | 111 | { |
yuliyasm | 1:f9c784a35e9c | 112 | float x0 = A[0]; |
yuliyasm | 1:f9c784a35e9c | 113 | float y0 = A[1]; |
yuliyasm | 1:f9c784a35e9c | 114 | float z0 = A[2]; |
yuliyasm | 1:f9c784a35e9c | 115 | int status = delta_calcAngleYZ(x0, y0, z0, B[0]); |
yuliyasm | 1:f9c784a35e9c | 116 | if (status == 0) status = delta_calcAngleYZ(x0 * cos120 + y0 * sin120, y0 * cos120 - x0 * sin120, z0, B[1]); // rotate coords to +120 deg |
yuliyasm | 1:f9c784a35e9c | 117 | if (status == 0) status = delta_calcAngleYZ(x0 * cos120 - y0 * sin120, y0 * cos120 + x0 * sin120, z0, B[2]); // rotate coords to -120 deg |
yuliyasm | 1:f9c784a35e9c | 118 | return status; |
yuliyasm | 1:f9c784a35e9c | 119 | } |