Yaroslav Barabanov
/
turret_2017
для управления турелью
servo.cpp
- Committer:
- Yar
- Date:
- 2017-01-15
- Revision:
- 0:690effcc5be0
- Child:
- 1:efb82d7dd043
File content as of revision 0:690effcc5be0:
#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); }