Yaroslav Barabanov / Mbed 2 deprecated turret_2017

для управления турелью

Dependencies:   mbed

servo.cpp@0:690effcc5be0, 2017-01-15 (annotated)

Committer:
Yar
Date:
Sun Jan 15 22:16:03 2017 +0000
Revision:
0:690effcc5be0
Child:
1:efb82d7dd043
new program for turret

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Yar 0:690effcc5be0 1 #include "servo.hpp"
Yar 0:690effcc5be0 2 #include "mbed.h"
Yar 0:690effcc5be0 3 #include "rtos.h"
Yar 0:690effcc5be0 4 #include "math.h"
Yar 0:690effcc5be0 5
Yar 0:690effcc5be0 6 #ifdef S8466N
Yar 0:690effcc5be0 7 // параметры длины импульса
Yar 0:690effcc5be0 8 // для сервопривода с тягой 30 кг/см
Yar 0:690effcc5be0 9 // угол +-60 градусов
Yar 0:690effcc5be0 10 const unsigned short servoMinPulse = 500; // мкс
Yar 0:690effcc5be0 11 const unsigned short servoMaxPulse = 2500;
Yar 0:690effcc5be0 12 const unsigned short servoDiffPulse = servoMaxPulse - servoMinPulse;
Yar 0:690effcc5be0 13 const unsigned short servoPulsePeriod = 20000;
Yar 0:690effcc5be0 14 const double servoMinAngle = 0.0;
Yar 0:690effcc5be0 15 const double servoMaxAngle = 180.0;
Yar 0:690effcc5be0 16 const double servoDiffAngle = 180.0; // максимальный угол размаха рычага
Yar 0:690effcc5be0 17 const double servoSecAngle = 0.64; // количество секунд на угол servoDiffAngle
Yar 0:690effcc5be0 18 #else
Yar 0:690effcc5be0 19 #ifdef S8466M
Yar 0:690effcc5be0 20 // параметры длины импульса
Yar 0:690effcc5be0 21 // для сервопривода с тягой 30 кг/см
Yar 0:690effcc5be0 22 // угол +-60 градусов
Yar 0:690effcc5be0 23 const unsigned short servoMinPulse = 900; // мкс
Yar 0:690effcc5be0 24 const unsigned short servoMaxPulse = 2100;
Yar 0:690effcc5be0 25 const unsigned short servoDiffPulse = servoMaxPulse - servoMinPulse;
Yar 0:690effcc5be0 26 const unsigned short servoPulsePeriod = 20000;
Yar 0:690effcc5be0 27 const double servoMinAngle = -90.0;
Yar 0:690effcc5be0 28 const double servoMaxAngle = 90.0;
Yar 0:690effcc5be0 29 const double servoDiffAngle = 180.0; // максимальный угол размаха рычага
Yar 0:690effcc5be0 30 const double servoSecAngle = 0.42; // количество секунд на угол servoDiffAngle
Yar 0:690effcc5be0 31 #else
Yar 0:690effcc5be0 32 #ifdef S8166B
Yar 0:690effcc5be0 33 // параметры длины импульса
Yar 0:690effcc5be0 34 // для сервопривода с тягой 30 кг/см
Yar 0:690effcc5be0 35 // угол +-60 градусов
Yar 0:690effcc5be0 36 const unsigned short servoMinPulse = 900; // мкс
Yar 0:690effcc5be0 37 const unsigned short servoMaxPulse = 2100;
Yar 0:690effcc5be0 38 const unsigned short servoDiffPulse = servoMaxPulse - servoMinPulse;
Yar 0:690effcc5be0 39 const unsigned short servoPulsePeriod = 20000;
Yar 0:690effcc5be0 40 const double servoMinAngle = -60.0;
Yar 0:690effcc5be0 41 const double servoMaxAngle = 60.0;
Yar 0:690effcc5be0 42 const double servoDiffAngle = 120.0; // максимальный угол размаха рычага
Yar 0:690effcc5be0 43 const double servoSecAngle = 0.42; // количество секунд на угол servoDiffAngle
Yar 0:690effcc5be0 44 #else
Yar 0:690effcc5be0 45 const unsigned short servoMinPulse = 1000;
Yar 0:690effcc5be0 46 const unsigned short servoMaxPulse = 2000;
Yar 0:690effcc5be0 47 const unsigned short servoDiffPulse = servoMaxPulse - servoMinPulse;
Yar 0:690effcc5be0 48 const unsigned short servoPulsePeriod = 20000;
Yar 0:690effcc5be0 49 const double servoMinAngle = -90.0;
Yar 0:690effcc5be0 50 const double servoMaxAngle = 90.0;
Yar 0:690effcc5be0 51 const double servoDiffAngle = 180.0;
Yar 0:690effcc5be0 52 const double servoSecAngle60 = 0.4; // количество секунд на угол servoDiffAngle
Yar 0:690effcc5be0 53 #endif
Yar 0:690effcc5be0 54 #endif
Yar 0:690effcc5be0 55 #endif
Yar 0:690effcc5be0 56
Yar 0:690effcc5be0 57 const unsigned short servoThreadDelay = 20; // мс
Yar 0:690effcc5be0 58 const double servoMaxSpeed = 1.0; // максимальная скорость
Yar 0:690effcc5be0 59
Yar 0:690effcc5be0 60 Timeout timerOne;
Yar 0:690effcc5be0 61 Timeout timerTwo;
Yar 0:690effcc5be0 62 // пин вывода ШИМ сигнала для первого сервопривода
Yar 0:690effcc5be0 63 DigitalOut servoOne(PIN_SERVO_ONE);
Yar 0:690effcc5be0 64 // пин вывода ШИМ сигнала для второго сервопривода
Yar 0:690effcc5be0 65 DigitalOut servoTwo(PIN_SERVO_TWO);
Yar 0:690effcc5be0 66
Yar 0:690effcc5be0 67 // переменные для хранения задержек
Yar 0:690effcc5be0 68 static int onDelayOne = 0;
Yar 0:690effcc5be0 69 static int offDelayOne = 0;
Yar 0:690effcc5be0 70 static int onDelayTwo = 0;
Yar 0:690effcc5be0 71 static int offDelayTwo = 0;
Yar 0:690effcc5be0 72 // переменные для хранения углов
Yar 0:690effcc5be0 73 static double angleOne = 0.0;
Yar 0:690effcc5be0 74 static double angleTwo = 0.0;
Yar 0:690effcc5be0 75 // переменные для хранения "скорости" изменения углов
Yar 0:690effcc5be0 76 static double dAngleOne = 0.0;
Yar 0:690effcc5be0 77 static double dAngleTwo = 0.0;
Yar 0:690effcc5be0 78 // флаг разрешения обновления углов сервопривода
Yar 0:690effcc5be0 79 static unsigned char isEndUpdate = 0; // флаг окончания обаботки угла сервопривода
Yar 0:690effcc5be0 80 static unsigned char isServoSpeed = 0; // флаг режима сервопривода (фиксированный угол или скорость)
Yar 0:690effcc5be0 81
Yar 0:690effcc5be0 82
Yar 0:690effcc5be0 83 void toggleOffOne(void);
Yar 0:690effcc5be0 84 void toggleOffTwo(void);
Yar 0:690effcc5be0 85 void toggleOnOne(void);
Yar 0:690effcc5be0 86 void toggleOnTwo(void);
Yar 0:690effcc5be0 87
Yar 0:690effcc5be0 88 void initServo(void) {
Yar 0:690effcc5be0 89 timerOne.detach();
Yar 0:690effcc5be0 90 timerTwo.detach();
Yar 0:690effcc5be0 91 toggleOnOne();
Yar 0:690effcc5be0 92 toggleOnTwo();
Yar 0:690effcc5be0 93 }
Yar 0:690effcc5be0 94
Yar 0:690effcc5be0 95 // функция возвращает угол сервопривода
Yar 0:690effcc5be0 96 double getServoAngle(unsigned char numServo) {
Yar 0:690effcc5be0 97 switch (numServo) {
Yar 0:690effcc5be0 98 case 0:
Yar 0:690effcc5be0 99 return angleOne;
Yar 0:690effcc5be0 100 case 1:
Yar 0:690effcc5be0 101 return angleTwo;
Yar 0:690effcc5be0 102 }
Yar 0:690effcc5be0 103 return 0.0;
Yar 0:690effcc5be0 104 }
Yar 0:690effcc5be0 105
Yar 0:690effcc5be0 106 void setServoAngle(double angle, unsigned char numServo) {
Yar 0:690effcc5be0 107 // ограничим углы
Yar 0:690effcc5be0 108 if (angle > servoMaxAngle && angle > 0) {
Yar 0:690effcc5be0 109 angle = servoMaxAngle;
Yar 0:690effcc5be0 110 } else
Yar 0:690effcc5be0 111 if (angle < servoMinAngle && angle < 0) {
Yar 0:690effcc5be0 112 angle = servoMinAngle;
Yar 0:690effcc5be0 113 }
Yar 0:690effcc5be0 114 while(isEndUpdate); // ждем завершения обработки ШИМ сигнала
Yar 0:690effcc5be0 115 switch (numServo) {
Yar 0:690effcc5be0 116 case 0:
Yar 0:690effcc5be0 117 angleOne = angle; // установим новый угол
Yar 0:690effcc5be0 118 isServoSpeed = 0;
Yar 0:690effcc5be0 119 break;
Yar 0:690effcc5be0 120 case 1:
Yar 0:690effcc5be0 121 angleTwo = angle;
Yar 0:690effcc5be0 122 isServoSpeed = 0;
Yar 0:690effcc5be0 123 break;
Yar 0:690effcc5be0 124 }
Yar 0:690effcc5be0 125 }
Yar 0:690effcc5be0 126
Yar 0:690effcc5be0 127 void setServoSpeed(double speed, unsigned char numServo) {
Yar 0:690effcc5be0 128 // ограничим углы
Yar 0:690effcc5be0 129 if (speed > servoMaxSpeed && speed > 0) {
Yar 0:690effcc5be0 130 speed = servoMaxSpeed;
Yar 0:690effcc5be0 131 } else
Yar 0:690effcc5be0 132 if (speed < -servoMaxSpeed && speed < 0) {
Yar 0:690effcc5be0 133 speed = -servoMaxSpeed;
Yar 0:690effcc5be0 134 }
Yar 0:690effcc5be0 135 // сколько за время разворота сервопривода
Yar 0:690effcc5be0 136 // пройдет циклов в потоке обнолвения углов сервопривода,
Yar 0:690effcc5be0 137 // угол надо разделить на данное число
Yar 0:690effcc5be0 138 double tempK;
Yar 0:690effcc5be0 139 double divK = (1000.0 * (servoSecAngle / (double)servoThreadDelay));
Yar 0:690effcc5be0 140 switch (numServo) {
Yar 0:690effcc5be0 141 case 0:
Yar 0:690effcc5be0 142 // найдем максимальный прирост угла
Yar 0:690effcc5be0 143 // и вычислим требуемый
Yar 0:690effcc5be0 144 if (speed > 0) {
Yar 0:690effcc5be0 145 tempK = (servoMaxAngle - angleOne) / divK;
Yar 0:690effcc5be0 146 } else
Yar 0:690effcc5be0 147 if (speed < 0) {
Yar 0:690effcc5be0 148 tempK = (angleOne - servoMinAngle) / divK;
Yar 0:690effcc5be0 149 } else
Yar 0:690effcc5be0 150 if (speed == 0) {
Yar 0:690effcc5be0 151 while(isEndUpdate); // ждем завершения обработки ШИМ сигнала
Yar 0:690effcc5be0 152 dAngleOne = 0;
Yar 0:690effcc5be0 153 isServoSpeed = 1;
Yar 0:690effcc5be0 154 break;
Yar 0:690effcc5be0 155 }
Yar 0:690effcc5be0 156 printf("speed = %f k = %f\n", speed, tempK);
Yar 0:690effcc5be0 157 tempK = tempK * speed;
Yar 0:690effcc5be0 158 printf("dAngleOne = %f\n", tempK);
Yar 0:690effcc5be0 159 while(isEndUpdate); // ждем завершения обработки ШИМ сигнала
Yar 0:690effcc5be0 160 dAngleOne = tempK;
Yar 0:690effcc5be0 161 isServoSpeed = 1;
Yar 0:690effcc5be0 162 break;
Yar 0:690effcc5be0 163 case 1:
Yar 0:690effcc5be0 164 // найдем максимальный прирост угла
Yar 0:690effcc5be0 165 // и вычислим требуемый
Yar 0:690effcc5be0 166 if (speed > 0) {
Yar 0:690effcc5be0 167 tempK = (servoMaxAngle - angleTwo) / tempK;
Yar 0:690effcc5be0 168 } else
Yar 0:690effcc5be0 169 if (speed < 0) {
Yar 0:690effcc5be0 170 tempK = (angleTwo - servoMinAngle) / tempK;
Yar 0:690effcc5be0 171 } else
Yar 0:690effcc5be0 172 if (speed == 0) {
Yar 0:690effcc5be0 173 while(isEndUpdate); // ждем завершения обработки ШИМ сигнала
Yar 0:690effcc5be0 174 dAngleTwo = 0;
Yar 0:690effcc5be0 175 isServoSpeed = 1;
Yar 0:690effcc5be0 176 break;
Yar 0:690effcc5be0 177 }
Yar 0:690effcc5be0 178 tempK = tempK * (speed / servoMaxSpeed);
Yar 0:690effcc5be0 179 while(isEndUpdate); // ждем завершения обработки ШИМ сигнала
Yar 0:690effcc5be0 180 dAngleTwo = tempK;
Yar 0:690effcc5be0 181 isServoSpeed = 1;
Yar 0:690effcc5be0 182 break;
Yar 0:690effcc5be0 183 }
Yar 0:690effcc5be0 184 }
Yar 0:690effcc5be0 185
Yar 0:690effcc5be0 186 void toggleOnOne(void) {
Yar 0:690effcc5be0 187 isEndUpdate = 1;
Yar 0:690effcc5be0 188 if (isServoSpeed == 1) {
Yar 0:690effcc5be0 189 // если используется режим скорости сервопривода
Yar 0:690effcc5be0 190 angleOne += dAngleOne; // изменим угол
Yar 0:690effcc5be0 191 // ограничим рост угла
Yar 0:690effcc5be0 192 if (angleOne > servoMaxAngle && angleOne > 0) {
Yar 0:690effcc5be0 193 angleOne = servoMaxAngle;
Yar 0:690effcc5be0 194 } else
Yar 0:690effcc5be0 195 if (angleOne < servoMinAngle && angleOne < 0) {
Yar 0:690effcc5be0 196 angleOne = servoMinAngle;
Yar 0:690effcc5be0 197 }
Yar 0:690effcc5be0 198 } else {
Yar 0:690effcc5be0 199 // иначе угол сервопривода строго задан
Yar 0:690effcc5be0 200 // ограничим углы
Yar 0:690effcc5be0 201 if (angleOne > servoMaxAngle && angleOne > 0) {
Yar 0:690effcc5be0 202 angleOne = servoMaxAngle;
Yar 0:690effcc5be0 203 } else
Yar 0:690effcc5be0 204 if (angleOne < servoMinAngle && angleOne < 0) {
Yar 0:690effcc5be0 205 angleOne = servoMinAngle;
Yar 0:690effcc5be0 206 }
Yar 0:690effcc5be0 207 }
Yar 0:690effcc5be0 208 double angle = angleOne - servoMinAngle;
Yar 0:690effcc5be0 209 angle = angle / servoDiffAngle;
Yar 0:690effcc5be0 210 // произведем расчет длины импульса
Yar 0:690effcc5be0 211 unsigned short pulse = servoMinPulse + (unsigned short)(servoDiffPulse * angle);
Yar 0:690effcc5be0 212 onDelayOne = pulse;
Yar 0:690effcc5be0 213 offDelayOne = servoPulsePeriod - onDelayOne;
Yar 0:690effcc5be0 214 servoOne = 1; // установим ножку ШИМ в лог. 1
Yar 0:690effcc5be0 215 // настроим таймер
Yar 0:690effcc5be0 216 timerOne.attach_us(toggleOffOne, onDelayOne);
Yar 0:690effcc5be0 217 isEndUpdate = 0;
Yar 0:690effcc5be0 218 }
Yar 0:690effcc5be0 219
Yar 0:690effcc5be0 220 void toggleOnTwo(void) {
Yar 0:690effcc5be0 221 isEndUpdate = 1;
Yar 0:690effcc5be0 222 if (isServoSpeed == 1) {
Yar 0:690effcc5be0 223 // если используется режим скорости сервопривода
Yar 0:690effcc5be0 224 angleTwo += dAngleTwo; // изменим угол
Yar 0:690effcc5be0 225 // ограничим рост угла
Yar 0:690effcc5be0 226 if (angleTwo > servoMaxAngle && angleTwo > 0) {
Yar 0:690effcc5be0 227 angleTwo = servoMaxAngle;
Yar 0:690effcc5be0 228 } else
Yar 0:690effcc5be0 229 if (angleTwo < servoMinAngle && angleTwo < 0) {
Yar 0:690effcc5be0 230 angleTwo = servoMinAngle;
Yar 0:690effcc5be0 231 }
Yar 0:690effcc5be0 232 } else {
Yar 0:690effcc5be0 233 // иначе угол сервопривода строго задан
Yar 0:690effcc5be0 234 // ограничим углы
Yar 0:690effcc5be0 235 if (angleTwo > servoMaxAngle && angleTwo > 0) {
Yar 0:690effcc5be0 236 angleTwo = servoMaxAngle;
Yar 0:690effcc5be0 237 } else
Yar 0:690effcc5be0 238 if (angleTwo < servoMinAngle && angleTwo < 0) {
Yar 0:690effcc5be0 239 angleTwo = servoMinAngle;
Yar 0:690effcc5be0 240 }
Yar 0:690effcc5be0 241 }
Yar 0:690effcc5be0 242 double angle = angleTwo - servoMinAngle;
Yar 0:690effcc5be0 243 angle = angle / servoDiffAngle;
Yar 0:690effcc5be0 244 // произведем расчет длины импульса
Yar 0:690effcc5be0 245 unsigned short pulse = servoMinPulse + (unsigned short)(servoDiffPulse * angle);
Yar 0:690effcc5be0 246 onDelayTwo = pulse;
Yar 0:690effcc5be0 247 offDelayTwo = servoPulsePeriod - onDelayTwo;
Yar 0:690effcc5be0 248 servoTwo = 1;
Yar 0:690effcc5be0 249 timerTwo.attach_us(toggleOffTwo, onDelayTwo);
Yar 0:690effcc5be0 250 isEndUpdate = 0;
Yar 0:690effcc5be0 251 }
Yar 0:690effcc5be0 252
Yar 0:690effcc5be0 253 void toggleOffOne(void) {
Yar 0:690effcc5be0 254 servoOne = 0;
Yar 0:690effcc5be0 255 timerOne.attach_us(toggleOnOne, offDelayOne);
Yar 0:690effcc5be0 256 }
Yar 0:690effcc5be0 257
Yar 0:690effcc5be0 258 void toggleOffTwo(void) {
Yar 0:690effcc5be0 259 servoTwo = 0;
Yar 0:690effcc5be0 260 timerTwo.attach_us(toggleOnTwo, offDelayTwo);
Yar 0:690effcc5be0 261 }
Yar 0:690effcc5be0 262
Yar 0:690effcc5be0 263