Yaroslav Barabanov
/
turret_2017
для управления турелью
Diff: servo.cpp
- Revision:
- 0:690effcc5be0
- Child:
- 1:efb82d7dd043
diff -r 000000000000 -r 690effcc5be0 servo.cpp --- /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); +} + +