roboforum.ru

Технический форум по робототехнике.

Помогите доделать код для сервы.

Помогите доделать код для сервы.

alexssey » 20 фев 2011, 02:38

Привет всем у меня такая проблема собрал таракана только четырёхлапого и надо теперь код написать.Мне дали код я зашил в мк и он только трясётся и всё.В 13 задаче я ничего не понял одна реклама.Мне надо только чтоб сервы туда сюда бегали и всё а как сделать не знаю.Вот исходник который мне дали.
#include <mega16.h>

// My bits definitions http://avr123.nm.ru/m8_128.h
#include <m8_128.h>
// в этом файле я сделал определения битов для МК

#include <delay.h> // библиотека пауз CVAVR

// Alphanumeric LCD Module functions
// строки нужны для работы LCD 2 строки по 16 символов
// схема подключения в http://avr123.nm.ru/z5.htm
#asm
.equ __lcd_port=0x1B ;PORTA
#endasm

#include <lcd.h> // библиотека для LCD

// ++++++ переменные добавленые в задаче 13

// индекс состояний PORTC для работы в прерывании
char index_st = 0;

// массив констант содержащий 2 состояний PORTC :
flash char pc_state[3]={
0b00000001, // "1" на servo на PC0
0b00000010, // "1" на servo на PC1

0b00000000 };// "0" на всех servo


// сколько осталось мкС досчитать до 20 мС
unsigned int ctr_last = 20000;


char servo_poz[2]={255,-255};
// массив для хранения положения servo

char servo_num = 0;
// переменная хранит номер servo

// Timer 1 output compare A interrupt service routine
interrupt [TIM1_COMPA] void timer1_compa_isr(void)
{
// Place your code here

TCNT1H=0x00; // обнулить счет таймера
TCNT1L=0x00;

// PORTC ^= 255; // инвертировать регистр PORTC

// Вывести в PORTC новое состояние
PORTC = pc_state[index_st];

if (index_st == 2){


OCR1A = ctr_last; // прерываться после отсчёта остатка времени

ctr_last = 20000; // обновить остаток времени на 20 мС

index_st = 0; // обнулить индекс
}
else {

// Вычислить длину имп. для сервы и вписать в OCR1A
OCR1A=992+(((unsigned int)servo_poz[index_st])<<2);
/* тут servo_poz[index_st] умножается на 4 сдвигом на 2
в лево и затем плюсуется к 992. пример - среднее положение
сервы команда 127, умножим на 4 будет 508, прибавим 992 -
получим 1500 мкС - это и есть длина импульса для
среднего положения вала srvo. */

// Вычесть длину имп. сервы из периода в 20 мС
ctr_last -= OCR1A ;
// увеличить индекс на 1
index_st ++ ;
};

}
//++++++++++++++++++++++++++++++++++++++++++++++++
// Standard Input/Output functions
#include <stdio.h>


// Константы ! строки в памяти программ
flash char string_1[]="Prog Start";
flash char string_2[]="The END";
// Declare your global variables here
// глобальные переменные для примера
// вывода температуры на ЖКИ
//++++++++++++++++++++++++++++++++++++++++++++++++

// Главная функция

void main(void)
{
// Declare your local variables here

// Input/Output Ports initialization
// Port A initialization
// Func7=In Func6=In Func5=In Func4=In Func3=Out Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=0 State2=T State1=T State0=T
PORTA=0x00;
DDRA=0x08;

PORTC=0; // PORTC - "0" во все биты регистра
DDRC=0xFF; // PORTC - все выводы ВЫХоды


// Timer/Counter 1 initialization
// Clock source: System Clock
// Clock value: 1000,000 kHz
// Mode: Normal top=FFFFh
// OC1A output: Discon.
// OC1B output: Discon.
// Noise Canceler: Off
// Input Capture on Falling Edge
// Timer 1 Overflow Interrupt: Off
// Input Capture Interrupt: Off
// Compare A Match Interrupt: On
// Compare B Match Interrupt: Off
TCCR1A=0x00;
TCCR1B=0x02;
TCNT1H=0x00;
TCNT1L=0x00;
ICR1H=0x00;
ICR1L=0x00;
// OCR1AH=0x3F;
// OCR1AL=0xFF;

OCR1A = 1500; //

OCR1BH=0x00;
OCR1BL=0x00;


// Timer(s)/Counter(s) Interrupt(s) initialization
TIMSK=0x10;

// Analog Comparator initialization
// Analog Comparator: Off
// Analog Comparator Input Capture by Timer/Counter 1: Off
ACSR=0x80;
SFIOR=0x00;

// LCD module initialization
// lcd_init(16);

// Global enable interrupts
#asm("sei")

putsf("Hello, world!");


while (1) {
char i = 0;
for(i=0, i<255, i++)
{
servo_poz[0] = i;
servo_poz[1] = i;

delay_ms(10);
}

for(i=255, i>1, i--)
{
servo_poz[0] = i;
servo_poz[1] = i;

delay_ms(10);
}

};
};
}

Добавлено спустя 2 часа 9 минут 56 секунд:
Всё я разобрался надо было прошить фьюзы на 8мгц. :Yahoo!:

Re: Помогите доделать код для сервы.

avr123.nm.ru » 20 фев 2011, 03:09

Чертова реклама ! Задолбала !

Re: Помогите доделать код для сервы.

osc » 20 фев 2011, 21:12

alexssey, есть клон того сайта: http://123avr.com/ он более читабельный.
Держите простой код для управления двумя сервами при помощи аппаратного ШИМа:
Код: Выделить всёРазвернуть
/*
Code Vision AVR C Compiler
MCU atmega16
CLOCK 8 MHz RC INT
*/
#include<mega16.h>
#define TOP 10000
void servo1_position(int pos)
{
OCR1A = pos/2;
}

void servo2_position(int pos)
{
OCR1B = pos/2;
}

void main(void)
{
//настраиваем Timer1
TCCR1A=0xA0;
TCCR1B=0x12;
TCNT1H=0x00;
TCNT1L=0x00;
ICR1H=TOP>>8;
ICR1L=TOP; //максимальное значение таймера
OCR1B=0x00;
OCR1A=0x00;

DDRD=0x30; //PORTD4 & PORTD5 сконфигурированы на выход

servo1_position(1500); //на PD5 появится ШИМ с частотой 50 гц и длит. импульса 1500 мксек, серва встанет по центру
servo2_position(1300); //на PD4 аналогично, но длительность импульса уже 1300 мксек
while(1);
}

Как сделать чтобы серва туда сюда ходила наверно разберетесь?
Обратите внимание на то, от чего микроконтроллер тактируется и с какой частотой:
Code Vision AVR C Compiler
MCU atmega16
CLOCK 8 MHz RC INT

PS: Пользуйтесь тегом code.
PPS: Не заметил что вы уже разобрались :)

Re: Помогите доделать код для сервы.

KOS » 20 мар 2011, 13:06

Не стал создавать новую тему, надеюсь не против, что задам вопрос(ы) в этой.
Решил сделать изначально радио управляемого "жука/паука" на подобии проекта HEXAPOD Anubis.
В программированиии МК нуб-самоучка, написал код как смог.
МК ATtiny 2313, сервоприводы MG995 - 3шт.
Пишу в CVAVR

Код: Выделить всёРазвернуть
/*****************************************************
Chip type               : ATtiny2313
AVR Core Clock frequency: 8,000000 MHz
*****************************************************/

#include <tiny2313.h>

#define lapa PORTD.5
#define locot PORTD.4
#define plecho PORTD.3

int A=0;
int B=0;

char PL=61; //Начальное положение Плеча
char LO=61; //Начальное положение Локтя
char LA=61; //Начальное положение Лапы

char PL2; //вспомогательная Переменная Плеча
char LO2; //вспомогательная Переменная Локтя
char LA2; //вспомогательная Переменная Лапы

char PL3=-2;  //поправка Плеча
char LO3=2;  //поправка Локтя
char LA3=-4;  //поправка Лапы

char PL1=0; //счетчик Плеча
char LO1=0; //счетчик Локтя
char LA1=0; //счетчик Лапы

int prerPL; //Длительность нижнего уровня Плеча
int prerLO; //Длительность нижнего уровня Локтя
int prerLA; //Длительность нижнего уровня Лапы

int prerPLL=0; //счетчик нижнего уровня Плеча
int prerLOO=0; //счетчик нижнего уровня Локтя
int prerLAA=0; //счетчик нижнего уровня Лапы

/******
D5 - Лапа
D4 - Локоть
D3 - Плечо
******/

// Прерывание по таймеру0
interrupt [TIM0_COMPA] void timer0_compa_isr(void)
{
A++;
// Расчет переменных с поправками
PL2=PL+PL3;
LO2=LO+LO3;
LA2=LA+LA3;

// Длительности нижних уровней
prerPL=570-PL2;
prerLO=570-LO2;
prerLA=570-LA2;

if (lapa)//Фармируем импульсы Лапы
{
  if (LA2==LA1)
  {
  lapa=0;
  LA1=0;
  }else LA1++;

}else if (prerLA==prerLAA)
      {
      lapa=1;
      prerLAA=0;
      }else prerLAA++;
     
if (locot)//Фармируем импульсы Локтя
{
  if (LO2==LO1)
  {
  locot=0;
  LO1=0;
  }else LO1++;

}else if (prerLO==prerLOO)
      {
      locot=1;
      prerLOO=0;
      }else prerLOO++;     

if (plecho)//Фармируем импульмы Плеча
{
  if (PL2==PL1)
  {
  plecho=0;
  PL1=0;
  }else PL1++;

}else if (prerPL==prerPLL)
      {
      plecho=1;
      prerPLL=0;
      }else prerPLL++; 
   
TCNT0=0x00; //Сброс счетчика таймера
}

void main(void)
{

#pragma optsize-
CLKPR=0x80;
CLKPR=0x00;
#ifdef _OPTIMIZE_SIZE_
#pragma optsize+
#endif

// Port B initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=P State3=T State2=P State1=P State0=P
PORTB=0x1F;
DDRB=0x00;

// Port D initialization
// Func6=In Func5=Out Func4=Out Func3=Out Func2=In Func1=In Func0=In
// State6=P State5=0 State4=0 State3=0 State2=T State1=T State0=T
PORTD=0x40;
DDRD=0x38;

// Timer/Counter 0 initialization
// Clock source: System Clock
// Clock value: 8000,000 kHz
// Mode: Normal top=FFh
// OC0A output: Disconnected
// OC0B output: Disconnected
TCCR0A=0x00;
TCCR0B=0x01;
TCNT0=0x00;
OCR0A=100;
OCR0B=0x00;

// Timer(s)/Counter(s) Interrupt(s) initialization
TIMSK=0x01;

ACSR=0x80;

#asm("sei")

while (1)
{

if(B==2&A>=30000)
{
PL=61;
LO=61;
LA=61;
B=3;
A=0;
}

if(B==1&A>=30000)
{
PL=70;
LO=61;
LA=61;
B=2;
A=0;
}

if(B==0&A>=30000)
{
PL=52;
LO=61;
LA=61;
B=1;
A=0;
}

if(B==3)
{
A=0;
B=0;
}
         
};
}


Тактирование МК от внешнего кварца 8МГц. Питание МК 78L05, питание серв отдельно L7805CV.
Так вот, если закоментировать основной код в теле while (1), то все отлично, стоит не дергается. Ни каких сбоев ни чего. Осцилограмма ровная, все времЕные интервалы выдержаны
А если начинать выполнять этот код, то происходит что то не понятное. Импульсы все сбиваются, крайние и нейтральное положение уходят. При условии что меняем импульс только одной сервы, другие то же начинают немного дергаться, то вверх, то вниз. Через некоторое время(примерно 1мин), все останавливается и ни происходит ни каких действий, а потом через 5-10сек опять начинает шевелиться.
Сначало грешил на питание. Подключил осцил к ноге Res, все нормально, сбоев не происходит. Подключил к питанию, в спокойном состоянии есть небольшие пульсации, скорей всего из за стабилизатора, но эти пульсации ни как не увеличиваются при шевелении ногой.
Куда дальше капать уже не представляю, стал отчаиваться в своем начинании.
Помогите, направте в нужную сторону.

Re: Помогите доделать код для сервы.

HarryStar » 21 мар 2011, 13:49

У вас чуть не вся логика написана в прерывании, которая формирует серво импульсы.
Это не правильно. К сожалению МК не такой быстрый, чтоб все успевать.
Логику надо вынести в майн, а в прерывании ТОЛЬКО формирование импульсов, без всяких лишних вычислений. Поэтому у вас в простом случае впритык все нормально, а как более сложный случай, импульсы начинают плыть, т.к. у МК не хватает времени на обработку.

Re: Помогите доделать код для сервы.

KOS » 21 мар 2011, 15:41

То есть, все вычесления перенести в майн? А в прерывании оставить только -
Код: Выделить всёРазвернуть
if (lapa)//Фармируем импульсы Лапы
...
if (locot)//Фармируем импульсы Локтя
...
if (plecho)//Фармируем импульмы Плеча
...
TCNT0=0x00; //Сброс счетчика таймера

Re: Помогите доделать код для сервы.

HarryStar » 21 мар 2011, 22:28

в прерывании должно быть постоянное формирование импульсов на сервы.
Понятие лапа, локоть и плечо - этого вообще не должно быть там.

в простейшем случае например для 3х серв делаем глобальный массив
servo_delay[3]
где храним в микросекундах длительность импульса
в прерывании стартуем 3 импульса и гасим их по таймеру в соответствии с этим массивом.
все. Больше никаких локтей, плечей, операторов IF и т.д. там быть не должно. И так то быстродействия хватит не на много.

а вот расчет длительности импульса в зависимости от нужного положения сервы размещаете в отдельной функции, а всю логику с локтями и коленями пихаете в майн.

И еще. Если хотите т.о. управлять не 3-мя сервами, а например 18-ю, то вскоре вам придется делать по другому, т.к. быстродействия МК не хватит. На форуме уже обсуждались различные алгоритмы серво-контроллера на большое кол-во серв. Мне лично нравится вариант с массивами, хранящими по 256 выборок одного порта. Минус этого решения большой расход памяти. 256 байт на каждые 8 серв.


Rambler\'s Top100 Mail.ru counter