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?
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 <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 | } |