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;
+}