Yaroslav Barabanov
для управления турелью
Diff: servo.cpp
- Revision:
- 0:690effcc5be0
- Child:
- 1:efb82d7dd043
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/servo.cpp Sun Jan 15 22:16:03 2017 +0000
@@ -0,0 +1,263 @@
+#include "servo.hpp"
+#include "mbed.h"
+#include "rtos.h"
+#include "math.h"
+
+#ifdef S8466N
+// параметры длины импульса
+// для сервопривода с тягой 30 кг/см
+// угол +-60 градусов
+const unsigned short servoMinPulse = 500; // мкс
+const unsigned short servoMaxPulse = 2500;
+const unsigned short servoDiffPulse = servoMaxPulse - servoMinPulse;
+const unsigned short servoPulsePeriod = 20000;
+const double servoMinAngle = 0.0;
+const double servoMaxAngle = 180.0;
+const double servoDiffAngle = 180.0; // максимальный угол размаха рычага
+const double servoSecAngle = 0.64; // количество секунд на угол servoDiffAngle
+#else
+#ifdef S8466M
+// параметры длины импульса
+// для сервопривода с тягой 30 кг/см
+// угол +-60 градусов
+const unsigned short servoMinPulse = 900; // мкс
+const unsigned short servoMaxPulse = 2100;
+const unsigned short servoDiffPulse = servoMaxPulse - servoMinPulse;
+const unsigned short servoPulsePeriod = 20000;
+const double servoMinAngle = -90.0;
+const double servoMaxAngle = 90.0;
+const double servoDiffAngle = 180.0; // максимальный угол размаха рычага
+const double servoSecAngle = 0.42; // количество секунд на угол servoDiffAngle
+#else
+#ifdef S8166B
+// параметры длины импульса
+// для сервопривода с тягой 30 кг/см
+// угол +-60 градусов
+const unsigned short servoMinPulse = 900; // мкс
+const unsigned short servoMaxPulse = 2100;
+const unsigned short servoDiffPulse = servoMaxPulse - servoMinPulse;
+const unsigned short servoPulsePeriod = 20000;
+const double servoMinAngle = -60.0;
+const double servoMaxAngle = 60.0;
+const double servoDiffAngle = 120.0; // максимальный угол размаха рычага
+const double servoSecAngle = 0.42; // количество секунд на угол servoDiffAngle
+#else
+const unsigned short servoMinPulse = 1000;
+const unsigned short servoMaxPulse = 2000;
+const unsigned short servoDiffPulse = servoMaxPulse - servoMinPulse;
+const unsigned short servoPulsePeriod = 20000;
+const double servoMinAngle = -90.0;
+const double servoMaxAngle = 90.0;
+const double servoDiffAngle = 180.0;
+const double servoSecAngle60 = 0.4; // количество секунд на угол servoDiffAngle
+#endif
+#endif
+#endif
+
+const unsigned short servoThreadDelay = 20; // мс
+const double servoMaxSpeed = 1.0; // максимальная скорость
+
+Timeout timerOne;
+Timeout timerTwo;
+// пин вывода ШИМ сигнала для первого сервопривода
+DigitalOut servoOne(PIN_SERVO_ONE);
+// пин вывода ШИМ сигнала для второго сервопривода
+DigitalOut servoTwo(PIN_SERVO_TWO);
+
+// переменные для хранения задержек
+static int onDelayOne = 0;
+static int offDelayOne = 0;
+static int onDelayTwo = 0;
+static int offDelayTwo = 0;
+// переменные для хранения углов
+static double angleOne = 0.0;
+static double angleTwo = 0.0;
+// переменные для хранения "скорости" изменения углов
+static double dAngleOne = 0.0;
+static double dAngleTwo = 0.0;
+// флаг разрешения обновления углов сервопривода
+static unsigned char isEndUpdate = 0; // флаг окончания обаботки угла сервопривода
+static unsigned char isServoSpeed = 0; // флаг режима сервопривода (фиксированный угол или скорость)
+
+
+void toggleOffOne(void);
+void toggleOffTwo(void);
+void toggleOnOne(void);
+void toggleOnTwo(void);
+
+void initServo(void) {
+ timerOne.detach();
+ timerTwo.detach();
+ toggleOnOne();
+ toggleOnTwo();
+}
+
+// функция возвращает угол сервопривода
+double getServoAngle(unsigned char numServo) {
+ switch (numServo) {
+ case 0:
+ return angleOne;
+ case 1:
+ return angleTwo;
+ }
+ return 0.0;
+}
+
+void setServoAngle(double angle, unsigned char numServo) {
+ // ограничим углы
+ if (angle > servoMaxAngle && angle > 0) {
+ angle = servoMaxAngle;
+ } else
+ if (angle < servoMinAngle && angle < 0) {
+ angle = servoMinAngle;
+ }
+ while(isEndUpdate); // ждем завершения обработки ШИМ сигнала
+ switch (numServo) {
+ case 0:
+ angleOne = angle; // установим новый угол
+ isServoSpeed = 0;
+ break;
+ case 1:
+ angleTwo = angle;
+ isServoSpeed = 0;
+ break;
+ }
+}
+
+void setServoSpeed(double speed, unsigned char numServo) {
+ // ограничим углы
+ if (speed > servoMaxSpeed && speed > 0) {
+ speed = servoMaxSpeed;
+ } else
+ if (speed < -servoMaxSpeed && speed < 0) {
+ speed = -servoMaxSpeed;
+ }
+ // сколько за время разворота сервопривода
+ // пройдет циклов в потоке обнолвения углов сервопривода,
+ // угол надо разделить на данное число
+ double tempK;
+ double divK = (1000.0 * (servoSecAngle / (double)servoThreadDelay));
+ switch (numServo) {
+ case 0:
+ // найдем максимальный прирост угла
+ // и вычислим требуемый
+ if (speed > 0) {
+ tempK = (servoMaxAngle - angleOne) / divK;
+ } else
+ if (speed < 0) {
+ tempK = (angleOne - servoMinAngle) / divK;
+ } else
+ if (speed == 0) {
+ while(isEndUpdate); // ждем завершения обработки ШИМ сигнала
+ dAngleOne = 0;
+ isServoSpeed = 1;
+ break;
+ }
+ printf("speed = %f k = %f\n", speed, tempK);
+ tempK = tempK * speed;
+ printf("dAngleOne = %f\n", tempK);
+ while(isEndUpdate); // ждем завершения обработки ШИМ сигнала
+ dAngleOne = tempK;
+ isServoSpeed = 1;
+ break;
+ case 1:
+ // найдем максимальный прирост угла
+ // и вычислим требуемый
+ if (speed > 0) {
+ tempK = (servoMaxAngle - angleTwo) / tempK;
+ } else
+ if (speed < 0) {
+ tempK = (angleTwo - servoMinAngle) / tempK;
+ } else
+ if (speed == 0) {
+ while(isEndUpdate); // ждем завершения обработки ШИМ сигнала
+ dAngleTwo = 0;
+ isServoSpeed = 1;
+ break;
+ }
+ tempK = tempK * (speed / servoMaxSpeed);
+ while(isEndUpdate); // ждем завершения обработки ШИМ сигнала
+ dAngleTwo = tempK;
+ isServoSpeed = 1;
+ break;
+ }
+}
+
+void toggleOnOne(void) {
+ isEndUpdate = 1;
+ if (isServoSpeed == 1) {
+ // если используется режим скорости сервопривода
+ angleOne += dAngleOne; // изменим угол
+ // ограничим рост угла
+ if (angleOne > servoMaxAngle && angleOne > 0) {
+ angleOne = servoMaxAngle;
+ } else
+ if (angleOne < servoMinAngle && angleOne < 0) {
+ angleOne = servoMinAngle;
+ }
+ } else {
+ // иначе угол сервопривода строго задан
+ // ограничим углы
+ if (angleOne > servoMaxAngle && angleOne > 0) {
+ angleOne = servoMaxAngle;
+ } else
+ if (angleOne < servoMinAngle && angleOne < 0) {
+ angleOne = servoMinAngle;
+ }
+ }
+ double angle = angleOne - servoMinAngle;
+ angle = angle / servoDiffAngle;
+ // произведем расчет длины импульса
+ unsigned short pulse = servoMinPulse + (unsigned short)(servoDiffPulse * angle);
+ onDelayOne = pulse;
+ offDelayOne = servoPulsePeriod - onDelayOne;
+ servoOne = 1; // установим ножку ШИМ в лог. 1
+ // настроим таймер
+ timerOne.attach_us(toggleOffOne, onDelayOne);
+ isEndUpdate = 0;
+}
+
+void toggleOnTwo(void) {
+ isEndUpdate = 1;
+ if (isServoSpeed == 1) {
+ // если используется режим скорости сервопривода
+ angleTwo += dAngleTwo; // изменим угол
+ // ограничим рост угла
+ if (angleTwo > servoMaxAngle && angleTwo > 0) {
+ angleTwo = servoMaxAngle;
+ } else
+ if (angleTwo < servoMinAngle && angleTwo < 0) {
+ angleTwo = servoMinAngle;
+ }
+ } else {
+ // иначе угол сервопривода строго задан
+ // ограничим углы
+ if (angleTwo > servoMaxAngle && angleTwo > 0) {
+ angleTwo = servoMaxAngle;
+ } else
+ if (angleTwo < servoMinAngle && angleTwo < 0) {
+ angleTwo = servoMinAngle;
+ }
+ }
+ double angle = angleTwo - servoMinAngle;
+ angle = angle / servoDiffAngle;
+ // произведем расчет длины импульса
+ unsigned short pulse = servoMinPulse + (unsigned short)(servoDiffPulse * angle);
+ onDelayTwo = pulse;
+ offDelayTwo = servoPulsePeriod - onDelayTwo;
+ servoTwo = 1;
+ timerTwo.attach_us(toggleOffTwo, onDelayTwo);
+ isEndUpdate = 0;
+}
+
+void toggleOffOne(void) {
+ servoOne = 0;
+ timerOne.attach_us(toggleOnOne, offDelayOne);
+}
+
+void toggleOffTwo(void) {
+ servoTwo = 0;
+ timerTwo.attach_us(toggleOnTwo, offDelayTwo);
+}
+
+