Yuliya Smirnova / Mbed 2 deprecated firstDelta

4653

Dependencies:   mbed-STM32F103C8T6 mbed USBDevice_STM32F103

main.cpp@3:086f8c1079ff, 2018-05-07 (annotated)

Committer:
yuliyasm
Date:
Mon May 07 11:41:10 2018 +0000
Revision:
3:086f8c1079ff
Parent:
2:d4dad64faadb 
4

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 <string>
yuliyasm 2:d4dad64faadb 4 #include "AccelStepper.h"
yuliyasm 2:d4dad64faadb 5 //X20Y20Z-300\r - пример команды
yuliyasm 2:d4dad64faadb 6
yuliyasm 3:086f8c1079ff 7 const float e = 20; // end effector
yuliyasm 3:086f8c1079ff 8 const float f = 80; // base
yuliyasm 3:086f8c1079ff 9 const float re = 225;
yuliyasm 1:f9c784a35e9c 10 const float rf = 100;
yuliyasm 1:f9c784a35e9c 11
yuliyasm 1:f9c784a35e9c 12 const float pi = 3.14;
yuliyasm 1:f9c784a35e9c 13 const float cos120 = -0.5;
yuliyasm 1:f9c784a35e9c 14 const float sin120 = sqrt(3.0) / 2;
yuliyasm 1:f9c784a35e9c 15 const float tg30 = 1 / sqrt(3.0);
yuliyasm 3:086f8c1079ff 16 const float q = 8; //микрошаг
yuliyasm 1:f9c784a35e9c 17
yuliyasm 1:f9c784a35e9c 18 bool parseXYZ(char* m, float* A);
yuliyasm 1:f9c784a35e9c 19 int delta_calcInverse(float* A, float* B);
yuliyasm 3:086f8c1079ff 20 void MotorsInit();
yuliyasm 1:f9c784a35e9c 21
yuliyasm 2:d4dad64faadb 22 PinName stp[3] = {PB_15,PA_8,PA_9};
yuliyasm 2:d4dad64faadb 23 PinName dir[3] = {PB_12,PB_13,PB_14};
yuliyasm 3:086f8c1079ff 24 AccelStepper axis1(1, stp[0], dir[0]);
yuliyasm 3:086f8c1079ff 25 AccelStepper axis2(1, stp[1], dir[1]);
yuliyasm 3:086f8c1079ff 26 AccelStepper axis3(1, stp[2], dir[2]);
yuliyasm 3:086f8c1079ff 27 AccelStepper Motors[3] = {axis1, axis2, axis3};
yuliyasm 3:086f8c1079ff 28 DigitalIn zero1(PB_10);
yuliyasm 3:086f8c1079ff 29 DigitalIn zero2(PB_1);
yuliyasm 3:086f8c1079ff 30 DigitalIn zero3(PB_0);
yuliyasm 3:086f8c1079ff 31 DigitalIn zero[3] = {zero1, zero2, zero3};
yuliyasm 3:086f8c1079ff 32 Serial pc(PA_2, PA_3);
yuliyasm 3:086f8c1079ff 33 DigitalOut myled(LED1);
yuliyasm 3:086f8c1079ff 34 DigitalOut enb(PA_10);
yuliyasm 2:d4dad64faadb 35
yuliyasm 2:d4dad64faadb 36 Timer t;
yuliyasm 1:f9c784a35e9c 37
yuliyasm 1:f9c784a35e9c 38 int main()
yuliyasm 1:f9c784a35e9c 39 {
yuliyasm 0:3cf5622d5b76 40 confSysClock(); //Configure system clock (72MHz HSE clock, 48MHz USB clock)
yuliyasm 3:086f8c1079ff 41 pc.baud(9600);
yuliyasm 2:d4dad64faadb 42 t.start();
yuliyasm 2:d4dad64faadb 43
yuliyasm 3:086f8c1079ff 44 MotorsInit();
yuliyasm 3:086f8c1079ff 45 float angles[3] = {0, 0, 0};
yuliyasm 3:086f8c1079ff 46 float point[3] = {0, 0, 0};
yuliyasm 3:086f8c1079ff 47 char message[32] = "";
yuliyasm 3:086f8c1079ff 48 char c = '\0';
yuliyasm 0:3cf5622d5b76 49 while(1) {
yuliyasm 2:d4dad64faadb 50
yuliyasm 3:086f8c1079ff 51 for (int i = 0; i < 3; i++)
yuliyasm 3:086f8c1079ff 52 Motors[i].run();
yuliyasm 3:086f8c1079ff 53 if (Motors[0].distanceToGo() == 0)
yuliyasm 3:086f8c1079ff 54 myled = 1;
yuliyasm 1:f9c784a35e9c 55
yuliyasm 3:086f8c1079ff 56 if (pc.readable()) {
yuliyasm 3:086f8c1079ff 57 wait_ms(300);
yuliyasm 3:086f8c1079ff 58 strcpy(message, "");
yuliyasm 3:086f8c1079ff 59 pc.scanf("%s\r\n", message);
yuliyasm 3:086f8c1079ff 60 strcat(message, "\n");
yuliyasm 3:086f8c1079ff 61 pc.printf("%s", message);
yuliyasm 1:f9c784a35e9c 62
yuliyasm 2:d4dad64faadb 63 bool p = parseXYZ(message, point);
yuliyasm 3:086f8c1079ff 64 if(p == false) {
yuliyasm 3:086f8c1079ff 65 pc.printf("Parse fail");
yuliyasm 3:086f8c1079ff 66 continue;
yuliyasm 3:086f8c1079ff 67 }
yuliyasm 3:086f8c1079ff 68 pc.printf("%.2f ", point[0]);
yuliyasm 3:086f8c1079ff 69 pc.printf("%.2f ", point[1]);
yuliyasm 3:086f8c1079ff 70 pc.printf("%.2f \n", point[2]);
yuliyasm 3:086f8c1079ff 71 delta_calcInverse(point, angles);
yuliyasm 1:f9c784a35e9c 72
yuliyasm 3:086f8c1079ff 73 for (int i = 0; i < 3; i++) {
yuliyasm 3:086f8c1079ff 74 pc.printf("%.2f ", angles[i]);
yuliyasm 3:086f8c1079ff 75 Motors[i].moveTo(int(angles[i]*q/1.8));
yuliyasm 3:086f8c1079ff 76 myled = 0;
yuliyasm 3:086f8c1079ff 77 //Motors[i].runToPosition();
yuliyasm 3:086f8c1079ff 78 //myled = 1;
yuliyasm 1:f9c784a35e9c 79 }
yuliyasm 1:f9c784a35e9c 80 }
yuliyasm 0:3cf5622d5b76 81 }
yuliyasm 0:3cf5622d5b76 82 }
yuliyasm 1:f9c784a35e9c 83
yuliyasm 3:086f8c1079ff 84 void MotorsInit()
yuliyasm 3:086f8c1079ff 85 {
yuliyasm 3:086f8c1079ff 86 enb = 0;
yuliyasm 3:086f8c1079ff 87 myled = 0;
yuliyasm 3:086f8c1079ff 88 pc.printf("Hello\r\n");
yuliyasm 3:086f8c1079ff 89
yuliyasm 3:086f8c1079ff 90 for (int i = 0; i < 3; i++) {
yuliyasm 3:086f8c1079ff 91 Motors[i].setMinPulseWidth (100);
yuliyasm 3:086f8c1079ff 92 Motors[i].setMaxSpeed (500);
yuliyasm 3:086f8c1079ff 93 Motors[i].setAcceleration (100);
yuliyasm 3:086f8c1079ff 94 Motors[i].moveTo(-90*q/1.8);
yuliyasm 3:086f8c1079ff 95 Motors[i].setSpeed (50);
yuliyasm 3:086f8c1079ff 96 }
yuliyasm 3:086f8c1079ff 97
yuliyasm 3:086f8c1079ff 98 while(zero[0] || zero[1] || zero[2]) {
yuliyasm 3:086f8c1079ff 99 for (int i = 0; i < 3; i++) {
yuliyasm 3:086f8c1079ff 100 if(zero[i] != 0)
yuliyasm 3:086f8c1079ff 101 Motors[i].runSpeedToPosition();
yuliyasm 3:086f8c1079ff 102 if (!Motors[i].distanceToGo())
yuliyasm 3:086f8c1079ff 103 goto setup;
yuliyasm 3:086f8c1079ff 104 }
yuliyasm 3:086f8c1079ff 105 }
yuliyasm 3:086f8c1079ff 106 setup:
yuliyasm 3:086f8c1079ff 107 for (int i = 0; i < 3; i++) {
yuliyasm 3:086f8c1079ff 108 Motors[i].stop();
yuliyasm 3:086f8c1079ff 109 Motors[i].setCurrentPosition(0);
yuliyasm 3:086f8c1079ff 110 }
yuliyasm 3:086f8c1079ff 111 myled = 1;
yuliyasm 3:086f8c1079ff 112 pc.printf("Ready\r\n");
yuliyasm 3:086f8c1079ff 113 }
yuliyasm 3:086f8c1079ff 114
yuliyasm 3:086f8c1079ff 115
yuliyasm 1:f9c784a35e9c 116 bool parseXYZ(char* m, float* A)
yuliyasm 1:f9c784a35e9c 117 {
yuliyasm 1:f9c784a35e9c 118 char buff[32] = "";
yuliyasm 1:f9c784a35e9c 119 int i = 0;
yuliyasm 1:f9c784a35e9c 120 if (m[i] != 'X')
yuliyasm 1:f9c784a35e9c 121 return false;
yuliyasm 1:f9c784a35e9c 122 i++;
yuliyasm 3:086f8c1079ff 123 while (m[i] != 'Y') {
yuliyasm 3:086f8c1079ff 124 if (m[i] == '\n')
yuliyasm 3:086f8c1079ff 125 return false;
yuliyasm 1:f9c784a35e9c 126 strcat(buff, &m[i]);
yuliyasm 1:f9c784a35e9c 127 i++;
yuliyasm 1:f9c784a35e9c 128 }
yuliyasm 1:f9c784a35e9c 129 A[0] = atof(buff);
yuliyasm 1:f9c784a35e9c 130
yuliyasm 1:f9c784a35e9c 131 strcpy(buff,"");
yuliyasm 1:f9c784a35e9c 132 i++;
yuliyasm 3:086f8c1079ff 133 while (m[i] != 'Z') {
yuliyasm 3:086f8c1079ff 134 if (m[i] == '\n')
yuliyasm 3:086f8c1079ff 135 return false;
yuliyasm 1:f9c784a35e9c 136 strcat(buff, &m[i]);
yuliyasm 1:f9c784a35e9c 137 i++;
yuliyasm 1:f9c784a35e9c 138 }
yuliyasm 1:f9c784a35e9c 139 A[1] = atof(buff);
yuliyasm 1:f9c784a35e9c 140
yuliyasm 1:f9c784a35e9c 141 strcpy(buff,"");
yuliyasm 1:f9c784a35e9c 142 i++;
yuliyasm 3:086f8c1079ff 143 while (m[i] != '\n') {
yuliyasm 3:086f8c1079ff 144 if (i > 32)
yuliyasm 3:086f8c1079ff 145 return false;
yuliyasm 1:f9c784a35e9c 146 strcat(buff, &m[i]);
yuliyasm 1:f9c784a35e9c 147 i++;
yuliyasm 1:f9c784a35e9c 148 }
yuliyasm 1:f9c784a35e9c 149 A[2] = atof(buff);
yuliyasm 1:f9c784a35e9c 150 return true;
yuliyasm 1:f9c784a35e9c 151 }
yuliyasm 1:f9c784a35e9c 152
yuliyasm 1:f9c784a35e9c 153 int delta_calcAngleYZ(float x0, float y0, float z0, float &theta)
yuliyasm 1:f9c784a35e9c 154 {
yuliyasm 1:f9c784a35e9c 155 float y1 = -0.5 * tg30 * f; // f/2 * tg 30
yuliyasm 1:f9c784a35e9c 156 y0 -= 0.5 * tg30 * e; // shift center to edge
yuliyasm 1:f9c784a35e9c 157 // z = a + b*y
yuliyasm 1:f9c784a35e9c 158 float a = (x0 * x0 + y0 * y0 + z0 * z0 + rf * rf - re * re - y1 * y1) / (2 * z0);
yuliyasm 1:f9c784a35e9c 159 float b = (y1 - y0) / z0;
yuliyasm 1:f9c784a35e9c 160 // discriminant
yuliyasm 1:f9c784a35e9c 161 float d = -(a + b * y1) * (a + b * y1) + rf * (b * b * rf + rf);
yuliyasm 1:f9c784a35e9c 162 if (d < 0) return -1; // non-existing point
yuliyasm 1:f9c784a35e9c 163 float yj = (y1 - a * b - sqrt(d)) / (b * b + 1); // choosing outer point
yuliyasm 1:f9c784a35e9c 164 float zj = a + b * yj;
yuliyasm 1:f9c784a35e9c 165 theta = 180.0 * atan(-zj / (y1 - yj)) / pi + ((yj > y1) ? 180.0 : 0.0);
yuliyasm 1:f9c784a35e9c 166 return 0;
yuliyasm 1:f9c784a35e9c 167 }
yuliyasm 1:f9c784a35e9c 168
yuliyasm 1:f9c784a35e9c 169 int delta_calcInverse(float* A, float* B)
yuliyasm 1:f9c784a35e9c 170 {
yuliyasm 1:f9c784a35e9c 171 float x0 = A[0];
yuliyasm 1:f9c784a35e9c 172 float y0 = A[1];
yuliyasm 1:f9c784a35e9c 173 float z0 = A[2];
yuliyasm 1:f9c784a35e9c 174 int status = delta_calcAngleYZ(x0, y0, z0, B[0]);
yuliyasm 1:f9c784a35e9c 175 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 176 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 177 return status;
yuliyasm 3:086f8c1079ff 178 }