Yuliya Smirnova / Mbed 2 deprecated firstDelta

4653

Dependencies:   mbed-STM32F103C8T6 mbed USBDevice_STM32F103

main.cpp@2:d4dad64faadb, 2018-02-03 (annotated)

Committer:
yuliyasm
Date:
Sat Feb 03 19:25:25 2018 +0000
Revision:
2:d4dad64faadb
Parent:
1:f9c784a35e9c
Child:
3:086f8c1079ff
preAlfa Stepper Motor Control

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 2:d4dad64faadb 5 #include "AccelStepper.h"
yuliyasm 2:d4dad64faadb 6 //X20Y20Z-300\r - пример команды
yuliyasm 2:d4dad64faadb 7
yuliyasm 1:f9c784a35e9c 8
yuliyasm 1:f9c784a35e9c 9 const float e = 24; // end effector
yuliyasm 1:f9c784a35e9c 10 const float f = 75; // base
yuliyasm 1:f9c784a35e9c 11 const float re = 300;
yuliyasm 1:f9c784a35e9c 12 const float rf = 100;
yuliyasm 1:f9c784a35e9c 13
yuliyasm 1:f9c784a35e9c 14 const float pi = 3.14;
yuliyasm 1:f9c784a35e9c 15 const float cos120 = -0.5;
yuliyasm 1:f9c784a35e9c 16 const float sin120 = sqrt(3.0) / 2;
yuliyasm 1:f9c784a35e9c 17 const float tg30 = 1 / sqrt(3.0);
yuliyasm 2:d4dad64faadb 18 const float q = 1; //микрошаг
yuliyasm 1:f9c784a35e9c 19
yuliyasm 1:f9c784a35e9c 20 bool parseXYZ(char* m, float* A);
yuliyasm 1:f9c784a35e9c 21 int delta_calcInverse(float* A, float* B);
yuliyasm 1:f9c784a35e9c 22
yuliyasm 2:d4dad64faadb 23 PinName stp[3] = {PB_15,PA_8,PA_9};
yuliyasm 2:d4dad64faadb 24 PinName dir[3] = {PB_12,PB_13,PB_14};
yuliyasm 2:d4dad64faadb 25
yuliyasm 2:d4dad64faadb 26 AccelStepper Xaxis(1, stp[0], dir[0]);
yuliyasm 2:d4dad64faadb 27 AccelStepper Yaxis(1, stp[1], dir[1]);
yuliyasm 2:d4dad64faadb 28 AccelStepper Zaxis(1, stp[2], dir[2]);
yuliyasm 2:d4dad64faadb 29 AccelStepper Motors[3] = {Xaxis, Yaxis, Zaxis};
yuliyasm 2:d4dad64faadb 30 Timer t;
yuliyasm 1:f9c784a35e9c 31
yuliyasm 1:f9c784a35e9c 32 int main()
yuliyasm 1:f9c784a35e9c 33 {
yuliyasm 0:3cf5622d5b76 34 confSysClock(); //Configure system clock (72MHz HSE clock, 48MHz USB clock)
yuliyasm 1:f9c784a35e9c 35 USBSerial usbSerial(0x1f00, 0x2012, 0x0001, false);
yuliyasm 2:d4dad64faadb 36 //DigitalOut myled(LED1);
yuliyasm 2:d4dad64faadb 37 DigitalOut enb(PA_10);
yuliyasm 2:d4dad64faadb 38 t.start();
yuliyasm 2:d4dad64faadb 39
yuliyasm 2:d4dad64faadb 40 for (int i = 0; i < 3; i++) {
yuliyasm 2:d4dad64faadb 41 Motors[i].setMaxSpeed(1000);
yuliyasm 2:d4dad64faadb 42 Motors[i].setAcceleration(100);
yuliyasm 2:d4dad64faadb 43 Motors[i].setMinPulseWidth(10000);
yuliyasm 2:d4dad64faadb 44 //Motors[i].moveTo(500);
yuliyasm 2:d4dad64faadb 45 }
yuliyasm 2:d4dad64faadb 46
yuliyasm 2:d4dad64faadb 47 usbSerial.printf("Ready\r\n");
yuliyasm 1:f9c784a35e9c 48
yuliyasm 0:3cf5622d5b76 49 while(1) {
yuliyasm 2:d4dad64faadb 50
yuliyasm 2:d4dad64faadb 51 enb = 1;
yuliyasm 2:d4dad64faadb 52 /*myled = 0; // turn the LED on
yuliyasm 0:3cf5622d5b76 53 wait_ms(200); // 200 millisecond
yuliyasm 0:3cf5622d5b76 54 myled = 1; // turn the LED off
yuliyasm 0:3cf5622d5b76 55 wait_ms(1000); // 1000 millisecond
yuliyasm 2:d4dad64faadb 56 usbSerial.printf("Blink\r\n");*/
yuliyasm 1:f9c784a35e9c 57
yuliyasm 1:f9c784a35e9c 58 if (usbSerial.available()) {
yuliyasm 2:d4dad64faadb 59 char message[32];
yuliyasm 1:f9c784a35e9c 60 wait_ms(2);
yuliyasm 1:f9c784a35e9c 61 usbSerial.scanf("%s", message);
yuliyasm 1:f9c784a35e9c 62 strcat(message, "\r");
yuliyasm 1:f9c784a35e9c 63 usbSerial.printf("%s", message);
yuliyasm 1:f9c784a35e9c 64
yuliyasm 2:d4dad64faadb 65 float point[3];
yuliyasm 2:d4dad64faadb 66 bool p = parseXYZ(message, point);
yuliyasm 1:f9c784a35e9c 67 if(p == false)
yuliyasm 1:f9c784a35e9c 68 return;
yuliyasm 2:d4dad64faadb 69 usbSerial.printf("%.2f \r", point[0]);
yuliyasm 2:d4dad64faadb 70 usbSerial.printf("%.2f \r", point[1]);
yuliyasm 2:d4dad64faadb 71 usbSerial.printf("%.2f \r", point[2]);
yuliyasm 1:f9c784a35e9c 72
yuliyasm 2:d4dad64faadb 73 float angles[3];
yuliyasm 2:d4dad64faadb 74 if (!delta_calcInverse(point, angles)) {
yuliyasm 1:f9c784a35e9c 75 for (int i = 0; i < 3; i++) {
yuliyasm 2:d4dad64faadb 76 usbSerial.printf("%.2f \r", angles[i]);
yuliyasm 2:d4dad64faadb 77 Motors[i].moveTo(angles[i]*q/1.8);
yuliyasm 1:f9c784a35e9c 78 }
yuliyasm 1:f9c784a35e9c 79 }
yuliyasm 1:f9c784a35e9c 80 }
yuliyasm 2:d4dad64faadb 81 for (int i = 0; i < 3; i++)
yuliyasm 2:d4dad64faadb 82 Motors[i].runToPosition();
yuliyasm 0:3cf5622d5b76 83 }
yuliyasm 0:3cf5622d5b76 84 }
yuliyasm 1:f9c784a35e9c 85
yuliyasm 1:f9c784a35e9c 86 bool parseXYZ(char* m, float* A)
yuliyasm 1:f9c784a35e9c 87 {
yuliyasm 1:f9c784a35e9c 88 char buff[32] = "";
yuliyasm 1:f9c784a35e9c 89 int i = 0;
yuliyasm 1:f9c784a35e9c 90 if (m[i] != 'X')
yuliyasm 1:f9c784a35e9c 91 return false;
yuliyasm 1:f9c784a35e9c 92 i++;
yuliyasm 1:f9c784a35e9c 93 while (m[i] != 'Y' && i < 32) {
yuliyasm 1:f9c784a35e9c 94 strcat(buff, &m[i]);
yuliyasm 1:f9c784a35e9c 95 i++;
yuliyasm 1:f9c784a35e9c 96 }
yuliyasm 1:f9c784a35e9c 97 A[0] = atof(buff);
yuliyasm 1:f9c784a35e9c 98
yuliyasm 1:f9c784a35e9c 99 strcpy(buff,"");
yuliyasm 1:f9c784a35e9c 100 i++;
yuliyasm 1:f9c784a35e9c 101 while (m[i] != 'Z' && i < 32) {
yuliyasm 1:f9c784a35e9c 102 strcat(buff, &m[i]);
yuliyasm 1:f9c784a35e9c 103 i++;
yuliyasm 1:f9c784a35e9c 104 }
yuliyasm 1:f9c784a35e9c 105 A[1] = atof(buff);
yuliyasm 1:f9c784a35e9c 106
yuliyasm 1:f9c784a35e9c 107 strcpy(buff,"");
yuliyasm 1:f9c784a35e9c 108 i++;
yuliyasm 1:f9c784a35e9c 109 while (m[i] != '\r' && i < 32) {
yuliyasm 1:f9c784a35e9c 110 strcat(buff, &m[i]);
yuliyasm 1:f9c784a35e9c 111 i++;
yuliyasm 1:f9c784a35e9c 112 }
yuliyasm 1:f9c784a35e9c 113 A[2] = atof(buff);
yuliyasm 1:f9c784a35e9c 114
yuliyasm 1:f9c784a35e9c 115 return true;
yuliyasm 1:f9c784a35e9c 116 }
yuliyasm 1:f9c784a35e9c 117
yuliyasm 1:f9c784a35e9c 118 int delta_calcAngleYZ(float x0, float y0, float z0, float &theta)
yuliyasm 1:f9c784a35e9c 119 {
yuliyasm 1:f9c784a35e9c 120 float y1 = -0.5 * tg30 * f; // f/2 * tg 30
yuliyasm 1:f9c784a35e9c 121 y0 -= 0.5 * tg30 * e; // shift center to edge
yuliyasm 1:f9c784a35e9c 122 // z = a + b*y
yuliyasm 1:f9c784a35e9c 123 float a = (x0 * x0 + y0 * y0 + z0 * z0 + rf * rf - re * re - y1 * y1) / (2 * z0);
yuliyasm 1:f9c784a35e9c 124 float b = (y1 - y0) / z0;
yuliyasm 1:f9c784a35e9c 125 // discriminant
yuliyasm 1:f9c784a35e9c 126 float d = -(a + b * y1) * (a + b * y1) + rf * (b * b * rf + rf);
yuliyasm 1:f9c784a35e9c 127 if (d < 0) return -1; // non-existing point
yuliyasm 1:f9c784a35e9c 128 float yj = (y1 - a * b - sqrt(d)) / (b * b + 1); // choosing outer point
yuliyasm 1:f9c784a35e9c 129 float zj = a + b * yj;
yuliyasm 1:f9c784a35e9c 130 theta = 180.0 * atan(-zj / (y1 - yj)) / pi + ((yj > y1) ? 180.0 : 0.0);
yuliyasm 1:f9c784a35e9c 131 return 0;
yuliyasm 1:f9c784a35e9c 132 }
yuliyasm 1:f9c784a35e9c 133
yuliyasm 1:f9c784a35e9c 134 // inverse kinematics: (x0, y0, z0) -> (theta1, theta2, theta3)
yuliyasm 1:f9c784a35e9c 135 // returned status: 0=OK, -1=non-existing position
yuliyasm 1:f9c784a35e9c 136 int delta_calcInverse(float* A, float* B)
yuliyasm 1:f9c784a35e9c 137 {
yuliyasm 1:f9c784a35e9c 138 float x0 = A[0];
yuliyasm 1:f9c784a35e9c 139 float y0 = A[1];
yuliyasm 1:f9c784a35e9c 140 float z0 = A[2];
yuliyasm 1:f9c784a35e9c 141 int status = delta_calcAngleYZ(x0, y0, z0, B[0]);
yuliyasm 1:f9c784a35e9c 142 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 143 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 144 return status;
yuliyasm 1:f9c784a35e9c 145 }