Yuliya Smirnova / Mbed 2 deprecated Speed_Control

Одновременное управление 3 шаговыми двигателями

Dependencies:   AccelStepper mbed-STM32F103C8T6 mbed

Fork of Delta_Robot_2018 by Yuliya Smirnova

main.cpp@1:2b8f94cdaaea, 2018-07-08 (annotated)

Committer:
yuliyasm
Date:
Sun Jul 08 20:46:00 2018 +0000
Revision:
1:2b8f94cdaaea
Parent:
0:1825dad3704f 
1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yuliyasm 1:2b8f94cdaaea 1 //X300Y500Z1000\r\n - пример команды управления (3 угла в градусах)
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 int q = 8; // микрошаг
yuliyasm 1:2b8f94cdaaea 8 #define MSpeed 2000 // максимальная скорость
yuliyasm 0:1825dad3704f 9
yuliyasm 0:1825dad3704f 10 bool parseXYZ(char* m, float* A);
yuliyasm 0:1825dad3704f 11 float readNum(char* m, int i);
yuliyasm 0:1825dad3704f 12 void MotorsInit();
yuliyasm 0:1825dad3704f 13 void Rx_interrupt();
yuliyasm 0:1825dad3704f 14
yuliyasm 0:1825dad3704f 15 PinName stp[3] = {PB_15,PA_8,PA_9};
yuliyasm 0:1825dad3704f 16 PinName dir[3] = {PB_12,PB_13,PB_14};
yuliyasm 0:1825dad3704f 17 AccelStepper axis1(1, stp[0], dir[0]);
yuliyasm 0:1825dad3704f 18 AccelStepper axis2(1, stp[1], dir[1]);
yuliyasm 0:1825dad3704f 19 AccelStepper axis3(1, stp[2], dir[2]);
yuliyasm 0:1825dad3704f 20 AccelStepper Motors[3] = {axis1, axis2, axis3};
yuliyasm 1:2b8f94cdaaea 21
yuliyasm 0:1825dad3704f 22 DigitalOut myled(LED1);
yuliyasm 0:1825dad3704f 23 DigitalOut enb(PA_10);
yuliyasm 0:1825dad3704f 24 Timer t;
yuliyasm 1:2b8f94cdaaea 25 Timer t_my;
yuliyasm 0:1825dad3704f 26 Serial pc(PB_10, PB_11);
yuliyasm 0:1825dad3704f 27
yuliyasm 1:2b8f94cdaaea 28 bool flags[3] = {true, true, true};
yuliyasm 0:1825dad3704f 29 ////////////////////////////////////////////////////////////////////////////////////
yuliyasm 0:1825dad3704f 30 int main()
yuliyasm 0:1825dad3704f 31 {
yuliyasm 0:1825dad3704f 32 confSysClock(); //Configure system clock (72MHz HSE clock, 48MHz USB clock)
yuliyasm 0:1825dad3704f 33 t.start();
yuliyasm 1:2b8f94cdaaea 34 t_my.start();
yuliyasm 0:1825dad3704f 35 pc.baud(9600);
yuliyasm 0:1825dad3704f 36 pc.attach(&Rx_interrupt, Serial::RxIrq);
yuliyasm 0:1825dad3704f 37 pc.printf("Hello\r\n");
yuliyasm 0:1825dad3704f 38 MotorsInit();
yuliyasm 0:1825dad3704f 39 while(1) {
yuliyasm 1:2b8f94cdaaea 40 for (int i = 0; i < 3; i++) {
yuliyasm 1:2b8f94cdaaea 41 Motors[i].runSpeedToPosition();
yuliyasm 1:2b8f94cdaaea 42 if (Motors[i].distanceToGo() == 0 && flags[i] == false) {
yuliyasm 1:2b8f94cdaaea 43 flags[i] = true;
yuliyasm 1:2b8f94cdaaea 44 pc.printf("%d - finish in %d msec\r\n", i, t_my.read_ms());
yuliyasm 1:2b8f94cdaaea 45 }
yuliyasm 1:2b8f94cdaaea 46 }
yuliyasm 0:1825dad3704f 47 if (Motors[0].distanceToGo() == 0 && Motors[1].distanceToGo() == 0 && Motors[2].distanceToGo() == 0)
yuliyasm 0:1825dad3704f 48 myled = 1;
yuliyasm 0:1825dad3704f 49 }
yuliyasm 0:1825dad3704f 50 }
yuliyasm 0:1825dad3704f 51 ///////////////////////////////////////////////////////////////////////////////////
yuliyasm 0:1825dad3704f 52
yuliyasm 0:1825dad3704f 53 void MotorsInit()
yuliyasm 0:1825dad3704f 54 {
yuliyasm 0:1825dad3704f 55 enb = 0;
yuliyasm 0:1825dad3704f 56 myled = 0;
yuliyasm 0:1825dad3704f 57 for (int i = 0; i < 3; i++) {
yuliyasm 1:2b8f94cdaaea 58 Motors[i].setMinPulseWidth (20);
yuliyasm 1:2b8f94cdaaea 59 Motors[i].setMaxSpeed (MSpeed*2);
yuliyasm 0:1825dad3704f 60 Motors[i].setCurrentPosition(0);
yuliyasm 0:1825dad3704f 61 }
yuliyasm 0:1825dad3704f 62 myled = 1;
yuliyasm 0:1825dad3704f 63 pc.printf("Ready\r\n\r\n");
yuliyasm 0:1825dad3704f 64 }
yuliyasm 0:1825dad3704f 65
yuliyasm 0:1825dad3704f 66
yuliyasm 0:1825dad3704f 67 bool parseXYZ(char* m, float* A)
yuliyasm 0:1825dad3704f 68 {
yuliyasm 0:1825dad3704f 69 for(int i = 0; i < 32; i++) {
yuliyasm 0:1825dad3704f 70 if (m[i] == '\n' || m[i] == '\r')
yuliyasm 0:1825dad3704f 71 break;
yuliyasm 0:1825dad3704f 72 /////////////////////////////////
yuliyasm 0:1825dad3704f 73 if (m[i] == 'X') {
yuliyasm 0:1825dad3704f 74 i++;
yuliyasm 0:1825dad3704f 75 A[0] = readNum(m, i);
yuliyasm 0:1825dad3704f 76 }
yuliyasm 0:1825dad3704f 77 ////////////////////////////////
yuliyasm 0:1825dad3704f 78 if (m[i] == 'Y') {
yuliyasm 0:1825dad3704f 79 i++;
yuliyasm 0:1825dad3704f 80 A[1] = readNum(m, i);
yuliyasm 0:1825dad3704f 81 }
yuliyasm 0:1825dad3704f 82 //////////////////////////////////
yuliyasm 0:1825dad3704f 83 if (m[i] == 'Z') {
yuliyasm 0:1825dad3704f 84 i++;
yuliyasm 0:1825dad3704f 85 A[2] = readNum(m, i);
yuliyasm 0:1825dad3704f 86 }
yuliyasm 0:1825dad3704f 87 }
yuliyasm 0:1825dad3704f 88 return true;
yuliyasm 0:1825dad3704f 89 }
yuliyasm 0:1825dad3704f 90
yuliyasm 0:1825dad3704f 91 float readNum(char* m, int i)
yuliyasm 0:1825dad3704f 92 {
yuliyasm 0:1825dad3704f 93 char buff[10] = "";
yuliyasm 0:1825dad3704f 94 int n = 0;
yuliyasm 0:1825dad3704f 95 while ((isdigit(m[i]) || m[i] == '.' || m[i] == '-') && n < 9) {
yuliyasm 0:1825dad3704f 96 buff[n] = m[i];
yuliyasm 0:1825dad3704f 97 i++;
yuliyasm 0:1825dad3704f 98 n++;
yuliyasm 0:1825dad3704f 99 }
yuliyasm 1:2b8f94cdaaea 100 if (n == 0) {
yuliyasm 1:2b8f94cdaaea 101 buff[n] = '0';
yuliyasm 1:2b8f94cdaaea 102 n++;
yuliyasm 1:2b8f94cdaaea 103 }
yuliyasm 0:1825dad3704f 104 buff[n] = '\0';
yuliyasm 0:1825dad3704f 105 return(atof(buff));
yuliyasm 0:1825dad3704f 106 }
yuliyasm 0:1825dad3704f 107
yuliyasm 0:1825dad3704f 108
yuliyasm 1:2b8f94cdaaea 109 void constTime()
yuliyasm 0:1825dad3704f 110 {
yuliyasm 1:2b8f94cdaaea 111 float DeltaMax = 0;
yuliyasm 1:2b8f94cdaaea 112 for (int i = 0; i < 3; i++) {
yuliyasm 1:2b8f94cdaaea 113 if(abs(Motors[i].distanceToGo()) > DeltaMax)
yuliyasm 1:2b8f94cdaaea 114 DeltaMax = abs(Motors[i].distanceToGo());
yuliyasm 1:2b8f94cdaaea 115 }
yuliyasm 1:2b8f94cdaaea 116 float k = MSpeed/DeltaMax;
yuliyasm 1:2b8f94cdaaea 117 for (int i = 0; i < 3; i++) {
yuliyasm 1:2b8f94cdaaea 118 Motors[i].setSpeed(k*abs(Motors[i].distanceToGo()));
yuliyasm 1:2b8f94cdaaea 119 pc.printf("\r\nSpeed %d", int(k*abs(Motors[i].distanceToGo())));
yuliyasm 1:2b8f94cdaaea 120 }
yuliyasm 0:1825dad3704f 121 }
yuliyasm 0:1825dad3704f 122
yuliyasm 0:1825dad3704f 123 void Rx_interrupt()
yuliyasm 0:1825dad3704f 124 {
yuliyasm 0:1825dad3704f 125 char message[32] = "";
yuliyasm 0:1825dad3704f 126 pc.scanf("%[^\n]s", message);
yuliyasm 0:1825dad3704f 127 strcat(message, "\n");
yuliyasm 1:2b8f94cdaaea 128 pc.printf("\r\n%s", message);
yuliyasm 0:1825dad3704f 129
yuliyasm 1:2b8f94cdaaea 130 float angles[3] = {0, 0, 0};
yuliyasm 1:2b8f94cdaaea 131 parseXYZ(message, angles);
yuliyasm 0:1825dad3704f 132 pc.printf("Angle: ");
yuliyasm 0:1825dad3704f 133 for (int i = 0; i < 3; i++) {
yuliyasm 0:1825dad3704f 134 pc.printf("%.2f ", angles[i]);
yuliyasm 1:2b8f94cdaaea 135 Motors[i].move(int(angles[i]*q/1.8));
yuliyasm 1:2b8f94cdaaea 136 flags[i] = false;
yuliyasm 0:1825dad3704f 137 }
yuliyasm 1:2b8f94cdaaea 138 constTime();
yuliyasm 0:1825dad3704f 139 myled = 0;
yuliyasm 1:2b8f94cdaaea 140 t_my.stop();
yuliyasm 1:2b8f94cdaaea 141 t_my.reset();
yuliyasm 1:2b8f94cdaaea 142 t_my.start();
yuliyasm 1:2b8f94cdaaea 143 pc.printf("\r\nWait!\r\n");
yuliyasm 0:1825dad3704f 144 return;
yuliyasm 0:1825dad3704f 145 }