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!:
alexssey
 
Сообщения: 12
Зарегистрирован: 30 июл 2010, 22:23
Откуда: тольятти
прог. языки: code vision avr
ФИО: алексей

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

Сообщение avr123.nm.ru » 20 фев 2011, 03:09

Чертова реклама ! Задолбала !
Читайте !
Аватара пользователя
avr123.nm.ru
отсылающий читать курс
 
Сообщения: 14195
Зарегистрирован: 06 ноя 2005, 04:18
Откуда: Москва
Предупреждения: -8

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: Не заметил что вы уже разобрались :)
Аватара пользователя
osc
 
Сообщения: 333
Зарегистрирован: 07 сен 2009, 20:06
прог. языки: c, c#
ФИО: Евгений

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, все нормально, сбоев не происходит. Подключил к питанию, в спокойном состоянии есть небольшие пульсации, скорей всего из за стабилизатора, но эти пульсации ни как не увеличиваются при шевелении ногой.
Куда дальше капать уже не представляю, стал отчаиваться в своем начинании.
Помогите, направте в нужную сторону.
KOS
 
Сообщения: 3
Зарегистрирован: 20 мар 2011, 11:49
Откуда: Красноярск

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

Сообщение HarryStar » 21 мар 2011, 13:49

У вас чуть не вся логика написана в прерывании, которая формирует серво импульсы.
Это не правильно. К сожалению МК не такой быстрый, чтоб все успевать.
Логику надо вынести в майн, а в прерывании ТОЛЬКО формирование импульсов, без всяких лишних вычислений. Поэтому у вас в простом случае впритык все нормально, а как более сложный случай, импульсы начинают плыть, т.к. у МК не хватает времени на обработку.
Аватара пользователя
HarryStar
 
Сообщения: 995
Зарегистрирован: 15 ноя 2010, 13:56
Откуда: Нижний Новгород
прог. языки: С, С++, РНР

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

Сообщение KOS » 21 мар 2011, 15:41

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

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

Сообщение HarryStar » 21 мар 2011, 22:28

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

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

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

И еще. Если хотите т.о. управлять не 3-мя сервами, а например 18-ю, то вскоре вам придется делать по другому, т.к. быстродействия МК не хватит. На форуме уже обсуждались различные алгоритмы серво-контроллера на большое кол-во серв. Мне лично нравится вариант с массивами, хранящими по 256 выборок одного порта. Минус этого решения большой расход памяти. 256 байт на каждые 8 серв.
Аватара пользователя
HarryStar
 
Сообщения: 995
Зарегистрирован: 15 ноя 2010, 13:56
Откуда: Нижний Новгород
прог. языки: С, С++, РНР


Вернуться в Новичкам или основы основ роботостроения.

Кто сейчас на конференции

Сейчас этот форум просматривают: нет зарегистрированных пользователей и гости: 13