Yuliya Smirnova / Mbed 2 deprecated firstDelta

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?

UserRevisionLine numberNew 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 }