File content as of revision 3:e47c0c98f515:

#include "servo.hpp"
#include "mbed.h"
//#include "rtos.h"
#include "math.h"
#include "main.hpp"

//#define PIN_SERVO_ONE D10
//#define PIN_SERVO_TWO D11

#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;
        } 
        tempK = tempK * speed;
        while(isEndUpdate); // ждем завершения обработки ШИМ сигнала
        dAngleOne = tempK;
        isServoSpeed = 1;
        break;
        case 1:
        // найдем максимальный прирост угла
        // и вычислим требуемый
        if (speed > 0) {
            tempK = (servoMaxAngle - angleOne) / divK;
        } else
        if (speed < 0) {
            tempK = (angleTwo - servoMinAngle) / divK;
        } else
        if (speed == 0) {
            while(isEndUpdate); // ждем завершения обработки ШИМ сигнала
            dAngleTwo = 0;
            isServoSpeed = 1;
            break;
        } 
        tempK = tempK * speed;
        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);
}