Yuliya Smirnova / Mbed 2 deprecated Delta_Robot_2018

Control system of Delta-robot BMSTU 2018

Dependencies:   AccelStepper mbed-STM32F103C8T6 mbed

main.cpp@1:2bfb79a5ed38, 2018-06-20 (annotated)

Committer:
yuliyasm
Date:
Wed Jun 20 08:12:18 2018 +0000
Revision:
1:2bfb79a5ed38
Parent:
0:1825dad3704f 
final

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 1:2bfb79a5ed38 31 DigitalIn zero1(PA_15);
yuliyasm 1:2bfb79a5ed38 32 DigitalIn zero2(PA_12);
yuliyasm 1:2bfb79a5ed38 33 DigitalIn zero3(PA_11);
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 }
yuliyasm 1:2bfb79a5ed38 70 while(Motors[0].distanceToGo() || Motors[1].distanceToGo() || Motors[2].distanceToGo()) {
yuliyasm 0:1825dad3704f 71 for (int i = 0; i < 3; i++) {
yuliyasm 0:1825dad3704f 72 if(zero[i] != 0)
yuliyasm 1:2bfb79a5ed38 73 Motors[i].run();
yuliyasm 0:1825dad3704f 74 else
yuliyasm 0:1825dad3704f 75 Motors[i].moveTo(Motors[i].currentPosition());
yuliyasm 0:1825dad3704f 76 }
yuliyasm 0:1825dad3704f 77 }
yuliyasm 0:1825dad3704f 78 for (int i = 0; i < 3; i++) {
yuliyasm 0:1825dad3704f 79 Motors[i].stop();
yuliyasm 0:1825dad3704f 80 Motors[i].setCurrentPosition(0);
yuliyasm 1:2bfb79a5ed38 81 Motors[i].setMaxSpeed (2000);
yuliyasm 1:2bfb79a5ed38 82 Motors[i].setAcceleration (500);
yuliyasm 0:1825dad3704f 83 }
yuliyasm 0:1825dad3704f 84 myled = 1;
yuliyasm 0:1825dad3704f 85 pc.printf("Ready\r\n\r\n");
yuliyasm 0:1825dad3704f 86 }
yuliyasm 0:1825dad3704f 87
yuliyasm 0:1825dad3704f 88
yuliyasm 0:1825dad3704f 89 bool parseXYZ(char* m, float* A)
yuliyasm 0:1825dad3704f 90 {
yuliyasm 0:1825dad3704f 91 for(int i = 0; i < 32; i++) {
yuliyasm 0:1825dad3704f 92 if (m[i] == '\n' || m[i] == '\r')
yuliyasm 0:1825dad3704f 93 break;
yuliyasm 0:1825dad3704f 94 /////////////////////////////////
yuliyasm 0:1825dad3704f 95 if (m[i] == 'X') {
yuliyasm 0:1825dad3704f 96 i++;
yuliyasm 0:1825dad3704f 97 A[0] = readNum(m, i);
yuliyasm 0:1825dad3704f 98 }
yuliyasm 0:1825dad3704f 99 ////////////////////////////////
yuliyasm 0:1825dad3704f 100 if (m[i] == 'Y') {
yuliyasm 0:1825dad3704f 101 i++;
yuliyasm 0:1825dad3704f 102 A[1] = readNum(m, i);
yuliyasm 0:1825dad3704f 103 }
yuliyasm 0:1825dad3704f 104 //////////////////////////////////
yuliyasm 0:1825dad3704f 105 if (m[i] == 'Z') {
yuliyasm 0:1825dad3704f 106 i++;
yuliyasm 0:1825dad3704f 107 A[2] = readNum(m, i);
yuliyasm 0:1825dad3704f 108 }
yuliyasm 0:1825dad3704f 109 }
yuliyasm 0:1825dad3704f 110 for(int i = 0; i < 3; i++) { // проверка на адекватность
yuliyasm 0:1825dad3704f 111 if ((A[i] <= -MAXP) || (A[i] >= MAXP))
yuliyasm 0:1825dad3704f 112 return false;
yuliyasm 0:1825dad3704f 113 }
yuliyasm 0:1825dad3704f 114 return true;
yuliyasm 0:1825dad3704f 115 }
yuliyasm 0:1825dad3704f 116
yuliyasm 0:1825dad3704f 117 float readNum(char* m, int i)
yuliyasm 0:1825dad3704f 118 {
yuliyasm 0:1825dad3704f 119 char buff[10] = "";
yuliyasm 0:1825dad3704f 120 int n = 0;
yuliyasm 0:1825dad3704f 121 while ((isdigit(m[i]) || m[i] == '.' || m[i] == '-') && n < 9) {
yuliyasm 0:1825dad3704f 122 buff[n] = m[i];
yuliyasm 0:1825dad3704f 123 i++;
yuliyasm 0:1825dad3704f 124 n++;
yuliyasm 0:1825dad3704f 125 }
yuliyasm 0:1825dad3704f 126 buff[n] = '\0';
yuliyasm 0:1825dad3704f 127 if (n == 0)
yuliyasm 0:1825dad3704f 128 return (MAXP);
yuliyasm 0:1825dad3704f 129 return(atof(buff));
yuliyasm 0:1825dad3704f 130 }
yuliyasm 0:1825dad3704f 131
yuliyasm 0:1825dad3704f 132
yuliyasm 0:1825dad3704f 133 bool delta_calcAngleYZ(float x0, float y0, float z0, float &theta)
yuliyasm 0:1825dad3704f 134 {
yuliyasm 0:1825dad3704f 135 float y1 = -R; // f/2 * tg 30
yuliyasm 0:1825dad3704f 136 y0 = y0-r; // shift center to edge
yuliyasm 0:1825dad3704f 137 // z = a + b*y
yuliyasm 0:1825dad3704f 138 float a = (x0*x0 + y0*y0 + z0*z0 + L*L - l*l - y1*y1) / (2 * z0);
yuliyasm 0:1825dad3704f 139 float b = (y1 - y0) / z0;
yuliyasm 0:1825dad3704f 140 // discriminant
yuliyasm 0:1825dad3704f 141 float D = -(a + b * y1)*(a + b * y1) + L*L * (b*b + 1);
yuliyasm 0:1825dad3704f 142 if (D < 0) return false; // non-existing point
yuliyasm 0:1825dad3704f 143 float yj = (y1 - a * b - sqrt(D)) / (b*b + 1); // choosing outer point
yuliyasm 0:1825dad3704f 144 float zj = a + b * yj;
yuliyasm 0:1825dad3704f 145 theta = atan(-zj / (y1 - yj)) + ((yj > y1) ? pi : 0);
yuliyasm 0:1825dad3704f 146 theta = 180*theta/pi;
yuliyasm 0:1825dad3704f 147 return true;
yuliyasm 0:1825dad3704f 148 }
yuliyasm 0:1825dad3704f 149
yuliyasm 0:1825dad3704f 150 bool delta_calcInverse(float* A, float* B)
yuliyasm 0:1825dad3704f 151 {
yuliyasm 0:1825dad3704f 152 if (A[2] > 0)
yuliyasm 0:1825dad3704f 153 return false;
yuliyasm 0:1825dad3704f 154 float x0 = A[0];
yuliyasm 0:1825dad3704f 155 float y0 = A[1];
yuliyasm 0:1825dad3704f 156 float z0 = A[2];
yuliyasm 0:1825dad3704f 157 int status = delta_calcAngleYZ(x0, y0, z0, B[0]);
yuliyasm 0:1825dad3704f 158 if (status) {
yuliyasm 0:1825dad3704f 159 x0 = A[0]*cos120 + A[1]*sin120;
yuliyasm 0:1825dad3704f 160 y0 = A[1]*cos120 - A[0]*sin120;
yuliyasm 0:1825dad3704f 161 z0 = A[2];
yuliyasm 0:1825dad3704f 162 status = delta_calcAngleYZ(x0, y0, z0, B[1]);
yuliyasm 0:1825dad3704f 163 } // rotate coords to +120 deg
yuliyasm 0:1825dad3704f 164 if (status) {
yuliyasm 0:1825dad3704f 165 x0 = A[0]*cos120 - A[1]*sin120;
yuliyasm 0:1825dad3704f 166 y0 = A[1]*cos120 + A[0]*sin120;
yuliyasm 0:1825dad3704f 167 z0 = A[2];
yuliyasm 0:1825dad3704f 168 status = delta_calcAngleYZ(x0, y0, z0, B[2]);
yuliyasm 0:1825dad3704f 169 } // rotate coords to -120 deg
yuliyasm 0:1825dad3704f 170 return status;
yuliyasm 0:1825dad3704f 171 }
yuliyasm 0:1825dad3704f 172
yuliyasm 0:1825dad3704f 173 void Rx_interrupt()
yuliyasm 0:1825dad3704f 174 {
yuliyasm 0:1825dad3704f 175 char message[32] = "";
yuliyasm 0:1825dad3704f 176 pc.scanf("%[^\n]s", message);
yuliyasm 0:1825dad3704f 177 strcat(message, "\n");
yuliyasm 0:1825dad3704f 178 pc.printf("%s", message);
yuliyasm 0:1825dad3704f 179
yuliyasm 0:1825dad3704f 180 float point[3] = {MAXP, MAXP, MAXP};
yuliyasm 0:1825dad3704f 181 float angles[3] = {0, 0, 0};
yuliyasm 0:1825dad3704f 182
yuliyasm 0:1825dad3704f 183 bool p = parseXYZ(message, point);
yuliyasm 0:1825dad3704f 184 if(p == false) {
yuliyasm 0:1825dad3704f 185 pc.printf("Parse fail\r\n");
yuliyasm 0:1825dad3704f 186 return;
yuliyasm 0:1825dad3704f 187 }
yuliyasm 0:1825dad3704f 188 pc.printf("Point: %.2f ", point[0]);
yuliyasm 0:1825dad3704f 189 pc.printf("%.2f ", point[1]);
yuliyasm 0:1825dad3704f 190 pc.printf("%.2f\r\n", point[2]);
yuliyasm 0:1825dad3704f 191 p = delta_calcInverse(point, angles);
yuliyasm 0:1825dad3704f 192 if(p == false) {
yuliyasm 0:1825dad3704f 193 pc.printf("Kinematic fail\r\n");
yuliyasm 0:1825dad3704f 194 return;
yuliyasm 0:1825dad3704f 195 }
yuliyasm 0:1825dad3704f 196 pc.printf("Angle: ");
yuliyasm 0:1825dad3704f 197 for (int i = 0; i < 3; i++) {
yuliyasm 0:1825dad3704f 198 pc.printf("%.2f ", angles[i]);
yuliyasm 0:1825dad3704f 199 Motors[i].moveTo(int(angles[i]*q/1.8));
yuliyasm 0:1825dad3704f 200 }
yuliyasm 0:1825dad3704f 201 myled = 0;
yuliyasm 0:1825dad3704f 202 pc.printf("\r\nSuccess!\r\n\r\n");
yuliyasm 0:1825dad3704f 203 return;
yuliyasm 0:1825dad3704f 204 }