Search in topic
Замедлитель сервомашинки (подскажите статьи/инструкции)

sashaNar

Спасибо! Как раз нано есть, вожусь над приемом сигнала.)

sashaNar

Он вешает проц на 20мс примерно.) Так что нашел другой метод. Вот, написал для В-тейл микшера, но что то зараза такое ощущение что частота обновления очень низкая - немного подергивается. Может это сериальный порт перегружает проц?
Пины: 2,3 - входы, 3,4 -выходы.

#include <math.h>
#include <Servo.h>

int Chan1Interrupt = 0; // pin 18
int Chan2Interrupt = 1; // pin 19
volatile double Chan1_startPulse, Chan2_startPulse;
volatile double Chan1_val, Chan2_val;
volatile double Chan1_val_last, Chan2_val_last;
volatile double servo1_val,servo2_val;
long StartMillis=0;
long FrameCounter=0;

Servo ServoArray[4];

/*void serviceServos(long pFrameCounter)
{
ServoArray[1].writeMicroseconds(servo1_val);
ServoArray[2].writeMicroseconds(servo2_val);
}
*/
void setup()
{
attachInterrupt(Chan1Interrupt, Chan1_begin, RISING);
attachInterrupt(Chan2Interrupt, Chan2_begin, RISING);

int i;
// for (uint8_t i=0;i<2;i++)
// ServoArray[i].attach(4+i, 700, 2300);
ServoArray[1].attach(4, 700, 2300);
ServoArray[2].attach(5, 700, 2300);
Serial.begin(9600);
StartMillis = millis();
}

void loop()
{

long LocalMillis;
long LocalFrameCounter;
LocalMillis = millis();
/* LocalFrameCounter = (LocalMillis - StartMillis) / 20;

if (LocalFrameCounter > FrameCounter)
{
FrameCounter = LocalFrameCounter;
serviceServos(FrameCounter);
}
*/

Serial.print(Chan1_val);
Serial.print(’ ‘);
Serial.print(Chan2_val);
Serial.print(’ ‘);
servo1_val=(((1500-Chan1_val)+(1500-Chan2_val))/2)+1500;
servo2_val=(((1500-Chan1_val)-(1500-Chan2_val))/2)+1500;
Serial.print(servo1_val);
Serial.print(’ ');
Serial.println(servo2_val);

ServoArray[1].writeMicroseconds(servo1_val);
ServoArray[2].writeMicroseconds(servo2_val);
}

void Chan1_begin() // enter Chan1_begin when interrupt pin goes HIGH.
{
Chan1_startPulse = micros(); // record microseconds() value as Chan1_startPulse
detachInterrupt(Chan1Interrupt); // after recording the value, detach the interrupt from Chan1_begin
attachInterrupt(Chan1Interrupt, Chan1_end, FALLING); // re-attach the interrupt as Chan1_end, so we can record the value when it goes low
}

void Chan1_end()
{
Chan1_val = micros() - Chan1_startPulse; // when interrupt pin goes LOW, record the total pulse length by subtracting previous start value from current micros() vlaue.
detachInterrupt(Chan1Interrupt); // detach and get ready to go HIGH again
attachInterrupt(Chan1Interrupt, Chan1_begin, RISING);
if (Chan1_val < 1000 || Chan1_val > 2000) { Chan1_val = Chan1_val_last;}
else {Chan1_val_last = Chan1_val;}
}

void Chan2_begin() // enter Chan2_begin when interrupt pin goes HIGH.
{
Chan2_startPulse = micros(); // record microseconds() value as Chan2_startPulse
detachInterrupt(Chan2Interrupt); // after recording the value, detach the interrupt from Chan2_begin
attachInterrupt(Chan2Interrupt, Chan2_end, FALLING); // re-attach the interrupt as Chan2_end, so we can record the value when it goes low
}

void Chan2_end()
{
Chan2_val = micros() - Chan2_startPulse; // when interrupt pin goes LOW, record the total pulse length by subtracting previous start value from current micros() vlaue.
detachInterrupt(Chan2Interrupt); // detach and get ready to go HIGH again
attachInterrupt(Chan2Interrupt, Chan2_begin, RISING);
if (Chan2_val < 1000 || Chan2_val > 2000) { Chan2_val = Chan2_val_last;}
else {Chan2_val_last = Chan2_val;}
}

EDIT: вырубил сериальный порт, все работает нормально! С гирой тоже, но подрагивают сервы. Фильтр шума, что ли добавить?
EDIT2: Или это у меня сарвы агрелись? Пусть подостынут…

SGordon
sashaNar:

servo1_val=(((1500-Chan1_val)+(1500-Chan2_val))/2)+1500;
servo2_val=(((1500-Chan1_val)-(1500-Chan2_val))/2)+1500;

понравилось вот это выражение …

если скобки раскрыть то будет

servo1_val=(-Chan1_val-Chan2_val)/2 + 3000;
servo2_val=(-Chan1_val+Chan2_val)/2+ 1500;

не так ли?

sashaNar

Так.) Я над математикой вообще не думал, так, написал как вспомнил. Полюбому, еще две арифметические операции погоды не делают.
Кстати, сервы все же самую малость подергиваются. Уходят из предыдущего положения и тут же возвращаьтся обратно. Может кто у себя проверит?
Платка - ардуина нано 168.

22 days later