Yuliya Smirnova / Mbed 2 deprecated Delta_Robot_2018

Control system of Delta-robot BMSTU 2018

Dependencies:   AccelStepper mbed-STM32F103C8T6 mbed

main.cpp@0:1825dad3704f, 2018-05-21 (annotated)

Committer:
yuliyasm
Date:
Mon May 21 08:05:59 2018 +0000
Revision:
0:1825dad3704f
Child:
1:2bfb79a5ed38
first work

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yuliyasm 0:1825dad3704f 1 //X-300Y22.5Z-300\r\n - пример команды управления
yuliyasm 0:1825dad3704f 2 #include "stm32f103c8t6.h"
yuliyasm 0:1825dad3704f 3 #include "mbed.h"
yuliyasm 0:1825dad3704f 4 #include "AccelStepper.h"
yuliyasm 0:1825dad3704f 5 #include <ctype.h>
yuliyasm 0:1825dad3704f 6
yuliyasm 0:1825dad3704f 7 const float r = 25; // end effector
yuliyasm 0:1825dad3704f 8 const float R = 80; // base
yuliyasm 0:1825dad3704f 9 const float l = 225.5;
yuliyasm 0:1825dad3704f 10 const float L = 100;
yuliyasm 0:1825dad3704f 11 const int q = 8; // микрошаг
yuliyasm 0:1825dad3704f 12
yuliyasm 0:1825dad3704f 13 #define pi 3.14159
yuliyasm 0:1825dad3704f 14 #define cos120 -0.5
yuliyasm 0:1825dad3704f 15 #define sin120 0.866
yuliyasm 0:1825dad3704f 16 #define tg30 0.57735
yuliyasm 0:1825dad3704f 17 #define MAXP 500 // предельные координаты
yuliyasm 0:1825dad3704f 18
yuliyasm 0:1825dad3704f 19 bool parseXYZ(char* m, float* A);
yuliyasm 0:1825dad3704f 20 float readNum(char* m, int i);
yuliyasm 0:1825dad3704f 21 bool delta_calcInverse(float* A, float* B);
yuliyasm 0:1825dad3704f 22 void MotorsInit();
yuliyasm 0:1825dad3704f 23 void Rx_interrupt();
yuliyasm 0:1825dad3704f 24
yuliyasm 0:1825dad3704f 25 PinName stp[3] = {PB_15,PA_8,PA_9};
yuliyasm 0:1825dad3704f 26 PinName dir[3] = {PB_12,PB_13,PB_14};
yuliyasm 0:1825dad3704f 27 AccelStepper axis1(1, stp[0], dir[0]);
yuliyasm 0:1825dad3704f 28 AccelStepper axis2(1, stp[1], dir[1]);
yuliyasm 0:1825dad3704f 29 AccelStepper axis3(1, stp[2], dir[2]);
yuliyasm 0:1825dad3704f 30 AccelStepper Motors[3] = {axis1, axis2, axis3};
yuliyasm 0:1825dad3704f 31 DigitalIn zero1(PB_10);
yuliyasm 0:1825dad3704f 32 DigitalIn zero2(PB_1);
yuliyasm 0:1825dad3704f 33 DigitalIn zero3(PB_0);
yuliyasm 0:1825dad3704f 34 DigitalIn zero[3] = {zero1, zero2, zero3};
yuliyasm 0:1825dad3704f 35 DigitalOut myled(LED1);
yuliyasm 0:1825dad3704f 36 DigitalOut enb(PA_10);
yuliyasm 0:1825dad3704f 37 Timer t;
yuliyasm 0:1825dad3704f 38 Serial pc(PB_10, PB_11);
yuliyasm 0:1825dad3704f 39
yuliyasm 0:1825dad3704f 40 ////////////////////////////////////////////////////////////////////////////////////
yuliyasm 0:1825dad3704f 41 int main()
yuliyasm 0:1825dad3704f 42 {
yuliyasm 0:1825dad3704f 43 confSysClock(); //Configure system clock (72MHz HSE clock, 48MHz USB clock)
yuliyasm 0:1825dad3704f 44 t.start();
yuliyasm 0:1825dad3704f 45 pc.baud(9600);
yuliyasm 0:1825dad3704f 46 pc.attach(&Rx_interrupt, Serial::RxIrq);
yuliyasm 0:1825dad3704f 47 pc.printf("Hello\r\n");
yuliyasm 0:1825dad3704f 48 MotorsInit();
yuliyasm 0:1825dad3704f 49
yuliyasm 0:1825dad3704f 50 while(1) {
yuliyasm 0:1825dad3704f 51 for (int i = 0; i < 3; i++)
yuliyasm 0:1825dad3704f 52 Motors[i].run();
yuliyasm 0:1825dad3704f 53 if (Motors[0].distanceToGo() == 0 && Motors[1].distanceToGo() == 0 && Motors[2].distanceToGo() == 0)
yuliyasm 0:1825dad3704f 54 myled = 1;
yuliyasm 0:1825dad3704f 55 }
yuliyasm 0:1825dad3704f 56 }
yuliyasm 0:1825dad3704f 57 ///////////////////////////////////////////////////////////////////////////////////
yuliyasm 0:1825dad3704f 58
yuliyasm 0:1825dad3704f 59 void MotorsInit()
yuliyasm 0:1825dad3704f 60 {
yuliyasm 0:1825dad3704f 61 enb = 0;
yuliyasm 0:1825dad3704f 62 myled = 0;
yuliyasm 0:1825dad3704f 63
yuliyasm 0:1825dad3704f 64 for (int i = 0; i < 3; i++) {
yuliyasm 0:1825dad3704f 65 Motors[i].setMinPulseWidth (100);
yuliyasm 0:1825dad3704f 66 Motors[i].setMaxSpeed (500);
yuliyasm 0:1825dad3704f 67 Motors[i].setAcceleration (100);
yuliyasm 0:1825dad3704f 68 Motors[i].moveTo(-90*q/1.8);
yuliyasm 0:1825dad3704f 69 Motors[i].setSpeed (50);
yuliyasm 0:1825dad3704f 70 }
yuliyasm 0:1825dad3704f 71 while(Motors[0].distanceToGo() && Motors[1].distanceToGo() && Motors[2].distanceToGo()) {
yuliyasm 0:1825dad3704f 72 for (int i = 0; i < 3; i++) {
yuliyasm 0:1825dad3704f 73 if(zero[i] != 0)
yuliyasm 0:1825dad3704f 74 Motors[i].runSpeedToPosition();
yuliyasm 0:1825dad3704f 75 else
yuliyasm 0:1825dad3704f 76 Motors[i].moveTo(Motors[i].currentPosition());
yuliyasm 0:1825dad3704f 77 }
yuliyasm 0:1825dad3704f 78 }
yuliyasm 0:1825dad3704f 79 for (int i = 0; i < 3; i++) {
yuliyasm 0:1825dad3704f 80 Motors[i].stop();
yuliyasm 0:1825dad3704f 81 Motors[i].setCurrentPosition(0);
yuliyasm 0:1825dad3704f 82 }
yuliyasm 0:1825dad3704f 83 myled = 1;
yuliyasm 0:1825dad3704f 84 pc.printf("Ready\r\n\r\n");
yuliyasm 0:1825dad3704f 85 }
yuliyasm 0:1825dad3704f 86
yuliyasm 0:1825dad3704f 87
yuliyasm 0:1825dad3704f 88 bool parseXYZ(char* m, float* A)
yuliyasm 0:1825dad3704f 89 {
yuliyasm 0:1825dad3704f 90 for(int i = 0; i < 32; i++) {
yuliyasm 0:1825dad3704f 91 if (m[i] == '\n' || m[i] == '\r')
yuliyasm 0:1825dad3704f 92 break;
yuliyasm 0:1825dad3704f 93 /////////////////////////////////
yuliyasm 0:1825dad3704f 94 if (m[i] == 'X') {
yuliyasm 0:1825dad3704f 95 i++;
yuliyasm 0:1825dad3704f 96 A[0] = readNum(m, i);
yuliyasm 0:1825dad3704f 97 }
yuliyasm 0:1825dad3704f 98 ////////////////////////////////
yuliyasm 0:1825dad3704f 99 if (m[i] == 'Y') {
yuliyasm 0:1825dad3704f 100 i++;
yuliyasm 0:1825dad3704f 101 A[1] = readNum(m, i);
yuliyasm 0:1825dad3704f 102 }
yuliyasm 0:1825dad3704f 103 //////////////////////////////////
yuliyasm 0:1825dad3704f 104 if (m[i] == 'Z') {
yuliyasm 0:1825dad3704f 105 i++;
yuliyasm 0:1825dad3704f 106 A[2] = readNum(m, i);
yuliyasm 0:1825dad3704f 107 }
yuliyasm 0:1825dad3704f 108 }
yuliyasm 0:1825dad3704f 109 for(int i = 0; i < 3; i++) { // проверка на адекватность
yuliyasm 0:1825dad3704f 110 if ((A[i] <= -MAXP) || (A[i] >= MAXP))
yuliyasm 0:1825dad3704f 111 return false;
yuliyasm 0:1825dad3704f 112 }
yuliyasm 0:1825dad3704f 113 return true;
yuliyasm 0:1825dad3704f 114 }
yuliyasm 0:1825dad3704f 115
yuliyasm 0:1825dad3704f 116 float readNum(char* m, int i)
yuliyasm 0:1825dad3704f 117 {
yuliyasm 0:1825dad3704f 118 char buff[10] = "";
yuliyasm 0:1825dad3704f 119 int n = 0;
yuliyasm 0:1825dad3704f 120 while ((isdigit(m[i]) || m[i] == '.' || m[i] == '-') && n < 9) {
yuliyasm 0:1825dad3704f 121 buff[n] = m[i];
yuliyasm 0:1825dad3704f 122 i++;
yuliyasm 0:1825dad3704f 123 n++;
yuliyasm 0:1825dad3704f 124 }
yuliyasm 0:1825dad3704f 125 buff[n] = '\0';
yuliyasm 0:1825dad3704f 126 if (n == 0)
yuliyasm 0:1825dad3704f 127 return (MAXP);
yuliyasm 0:1825dad3704f 128 return(atof(buff));
yuliyasm 0:1825dad3704f 129 }
yuliyasm 0:1825dad3704f 130
yuliyasm 0:1825dad3704f 131
yuliyasm 0:1825dad3704f 132 bool delta_calcAngleYZ(float x0, float y0, float z0, float &theta)
yuliyasm 0:1825dad3704f 133 {
yuliyasm 0:1825dad3704f 134 float y1 = -R; // f/2 * tg 30
yuliyasm 0:1825dad3704f 135 y0 = y0-r; // shift center to edge
yuliyasm 0:1825dad3704f 136 // z = a + b*y
yuliyasm 0:1825dad3704f 137 float a = (x0*x0 + y0*y0 + z0*z0 + L*L - l*l - y1*y1) / (2 * z0);
yuliyasm 0:1825dad3704f 138 float b = (y1 - y0) / z0;
yuliyasm 0:1825dad3704f 139 // discriminant
yuliyasm 0:1825dad3704f 140 float D = -(a + b * y1)*(a + b * y1) + L*L * (b*b + 1);
yuliyasm 0:1825dad3704f 141 if (D < 0) return false; // non-existing point
yuliyasm 0:1825dad3704f 142 float yj = (y1 - a * b - sqrt(D)) / (b*b + 1); // choosing outer point
yuliyasm 0:1825dad3704f 143 float zj = a + b * yj;
yuliyasm 0:1825dad3704f 144 theta = atan(-zj / (y1 - yj)) + ((yj > y1) ? pi : 0);
yuliyasm 0:1825dad3704f 145 theta = 180*theta/pi;
yuliyasm 0:1825dad3704f 146 return true;
yuliyasm 0:1825dad3704f 147 }
yuliyasm 0:1825dad3704f 148
yuliyasm 0:1825dad3704f 149 bool delta_calcInverse(float* A, float* B)
yuliyasm 0:1825dad3704f 150 {
yuliyasm 0:1825dad3704f 151 if (A[2] > 0)
yuliyasm 0:1825dad3704f 152 return false;
yuliyasm 0:1825dad3704f 153 float x0 = A[0];
yuliyasm 0:1825dad3704f 154 float y0 = A[1];
yuliyasm 0:1825dad3704f 155 float z0 = A[2];
yuliyasm 0:1825dad3704f 156 int status = delta_calcAngleYZ(x0, y0, z0, B[0]);
yuliyasm 0:1825dad3704f 157 if (status) {
yuliyasm 0:1825dad3704f 158 x0 = A[0]*cos120 + A[1]*sin120;
yuliyasm 0:1825dad3704f 159 y0 = A[1]*cos120 - A[0]*sin120;
yuliyasm 0:1825dad3704f 160 z0 = A[2];
yuliyasm 0:1825dad3704f 161 status = delta_calcAngleYZ(x0, y0, z0, B[1]);
yuliyasm 0:1825dad3704f 162 } // rotate coords to +120 deg
yuliyasm 0:1825dad3704f 163 if (status) {
yuliyasm 0:1825dad3704f 164 x0 = A[0]*cos120 - A[1]*sin120;
yuliyasm 0:1825dad3704f 165 y0 = A[1]*cos120 + A[0]*sin120;
yuliyasm 0:1825dad3704f 166 z0 = A[2];
yuliyasm 0:1825dad3704f 167 status = delta_calcAngleYZ(x0, y0, z0, B[2]);
yuliyasm 0:1825dad3704f 168 } // rotate coords to -120 deg
yuliyasm 0:1825dad3704f 169 return status;
yuliyasm 0:1825dad3704f 170 }
yuliyasm 0:1825dad3704f 171
yuliyasm 0:1825dad3704f 172 void Rx_interrupt()
yuliyasm 0:1825dad3704f 173 {
yuliyasm 0:1825dad3704f 174 char message[32] = "";
yuliyasm 0:1825dad3704f 175 pc.scanf("%[^\n]s", message);
yuliyasm 0:1825dad3704f 176 strcat(message, "\n");
yuliyasm 0:1825dad3704f 177 pc.printf("%s", message);
yuliyasm 0:1825dad3704f 178
yuliyasm 0:1825dad3704f 179 float point[3] = {MAXP, MAXP, MAXP};
yuliyasm 0:1825dad3704f 180 float angles[3] = {0, 0, 0};
yuliyasm 0:1825dad3704f 181
yuliyasm 0:1825dad3704f 182 bool p = parseXYZ(message, point);
yuliyasm 0:1825dad3704f 183 if(p == false) {
yuliyasm 0:1825dad3704f 184 pc.printf("Parse fail\r\n");
yuliyasm 0:1825dad3704f 185 return;
yuliyasm 0:1825dad3704f 186 }
yuliyasm 0:1825dad3704f 187 pc.printf("Point: %.2f ", point[0]);
yuliyasm 0:1825dad3704f 188 pc.printf("%.2f ", point[1]);
yuliyasm 0:1825dad3704f 189 pc.printf("%.2f\r\n", point[2]);
yuliyasm 0:1825dad3704f 190 p = delta_calcInverse(point, angles);
yuliyasm 0:1825dad3704f 191 if(p == false) {
yuliyasm 0:1825dad3704f 192 pc.printf("Kinematic fail\r\n");
yuliyasm 0:1825dad3704f 193 return;
yuliyasm 0:1825dad3704f 194 }
yuliyasm 0:1825dad3704f 195 pc.printf("Angle: ");
yuliyasm 0:1825dad3704f 196 for (int i = 0; i < 3; i++) {
yuliyasm 0:1825dad3704f 197 pc.printf("%.2f ", angles[i]);
yuliyasm 0:1825dad3704f 198 Motors[i].moveTo(int(angles[i]*q/1.8));
yuliyasm 0:1825dad3704f 199 }
yuliyasm 0:1825dad3704f 200 myled = 0;
yuliyasm 0:1825dad3704f 201 pc.printf("\r\nSuccess!\r\n\r\n");
yuliyasm 0:1825dad3704f 202 return;
yuliyasm 0:1825dad3704f 203 }