Ovládanie robotického ramienka joystickom
Zo stránky SensorWiki
Záverečný projekt predmetu MIPS / LS2025 - Juraj Krasnovský
Zadanie
Cieľom projektu bolo ovládať robotické ramienko s tromi servo motormi pomocou pomocou jedného joysticku.
Literatúra:
- Dokumentácia k doske Arduino Uno
- Dokumentácia k použitému servomotorčeku
- Dokumentácia k použitému joysticku
Analýza a opis riešenia
Použíté komponenty:
Joystick funguje ako dva potenciometre, kde zmena polohy mení hodnotu v dvoch smeroch, x a y. Pomocou AD prevodníka som odčítal, že sa tieto hodnoty menia od 0 do 1023, kde 512 je stred. Na ovládanie dvoch servomotorov som použil Timer1. Pre tretí servomotor som musel, kvôli obmedzeniam arduino uno, použiť 8-bitový časovač Timer2,ktorý má síce menší rozsah pre nastavenie PWM ako Timer1, ale pre tento projekt je dostatočný. PWM signál má štandardnú dĺžku impulzov od 1 ms do 2ms. Pre Timer2 som použil prescaler rovný 256, pomocou ktorého som vypočítal dĺžku jedného kroku a potom pomocou nej určil rozsah OCR2:
Rovnaký výpočet som robil aj pre Timer1, menil sa ale iba prescaler (je rovný 8), tak napíšem iba výsledný rozsah OCR1:
Nezabudnite doplniť schému zapojenia! V texte by ste mali opísať základné veci zo zapojenia, samotná schéma nie je dostačujúci opis.
Algoritmus a program
Program zabezpečuje ovládanie robotického ramena pomocou odčítaných hodnôt "x" a "y" z AD prevodníka. Rameno sa skladá z troch servomotorov, horný, stredný a dolný. Stredný servomotor je ovládaný neustále podľa hodnoty "y" z joysticku. Horný a dolný servomotor sú ovládané pomocou hodnoty "x".Samotné servá sa pohybujú pomocou premenných dx a dy, ktoré predstavujú "kroky" servomotorčeka. Tieto premenné sa pričítajú/odčítajú v ďaľších premenný posx a posy, podľa hodnôt "x" a "y". Použité funkcie: adc_read – čítanie analógovej hodnoty z joysticku uart_init – inicializácia sériovej komunikácie UART uart_getc – čítanie znaku z UART delay – funkcia na oneskorenie Medzi horným a dolným servomotorom je treba prepínať, keďže chceme v jednom čase ovládať iba jeden z nich. Výber a ovládanie iba jedného je založené na dvoch režimoch, medzi ktorými sa prepína pomocou premennej „rezim“, do ktorej je funkciou getc() načítaný znak. Režim+ (znak = ' + ') zabezpečuje ovládanie spodného motora, tento režim je predvolený. Režim- (znak =' - ') zabezpečuje ovládanie horného motora. Ak do premennej „znak“ nie je načítané "+" ani "-" UART vypíše chybu a ovládanie sa zablokuje.
#define BAUD 9600
#define F_CPU 16000000UL
#include <stdio.h>
#include <avr/io.h>
#include "uart.h"
#include "adc.h"
#include <util/delay.h>
#define set_bit(ADDRESS,BIT) (ADDRESS |= (1<<BIT))
#define clear_bit(ADDRESS,BIT) (ADDRESS &= ~(1<<BIT))
FILE mystdout = FDEV_SETUP_STREAM(uart_putc, NULL, _FDEV_SETUP_WRITE);
void delay(int delay)
{
for (int i=1; i<=delay; i++)
_delay_ms(1);
}
int main(void)
{
unsigned int x, y, stred = 512, dolna, horna, posx, posy,dx,dy,xmax,xmin,ymax,ymin;
char rezim = '+';
adc_init();
uart_init();
stdout = &mystdout;
DDRB|=(1<<PB1); //dolne servo
DDRB |= (1<<PB2); //horne servo
DDRB|=(1<<PB3); //stredne servo
TCNT1 = 0;
ICR1 = 39999;
TCCR1A|=(1<<COM1A1)|(1<<COM1B1)|(1<<WGM11); //Timer1
TCCR1B|=(1<<WGM12)|(1<<WGM13)|(1<<CS11);
TCCR2A |= (1 << COM2A1) | (1 << WGM21) | (1 << WGM20); //Timer2
TCCR2B |= (1 << CS21) | (1 << CS22);
OCR1A=3000; //dolne servo
OCR1B=3000; //horne servo
OCR2A =98; //stredne servo
posx = 3000; //stredna hodnota horneho a dolneho serva
posy = 98; //stredna hodnota stredneho serva
dx = 40; //zmena x-ovej hodnoty
dy = 1; //zmena y-ovej hodnoty
//krajne hodnoty
xmax = 4000;
xmin = 2000;
ymax = 125;
ymin = 62;
//deadzone
dolna = stred-40;
horna = stred+40;
while(1)
{
x = adc_read(4);
y = adc_read(5);
//ukladanie zapisaneho znaku
if (UCSR0A & (1 << RXC0)) {
rezim = uart_getc();
}
if ( (x < dolna)){
if(posx >xmin) posx -= dx;
else posx = xmin;
}
else if(x > horna){
if(posx <xmax) posx += dx;
else posx = xmax;
}
if ( (y < dolna)){
if(posy >ymin) posy -= dy;
else posy = ymin;
}
else if(y > horna){
if(posy <ymax) posy += dy;
else posy = ymax;
}
if(rezim == '+'){ //rezim +
OCR1A = posx;
OCR2A = posy;
printf("rezim:%c\n",rezim);
}
else if(rezim == '-'){ //rezim -
OCR1B = posx;
OCR2A = posy;
printf("rezim:%c\n",rezim);
}
else if( (rezim != '+') && (rezim != '-')) //chybny vstup
{
uart_puts("CHYBA zle zvoleny rezim\n");
}
delay(5);
}
}
#include <avr/io.h>
void uart_init( void );
void uart_putc( char c );
void uart_puts( const char *s );
char uart_getc( void );
#define BAUD 9600
#define F_CPU 16000000UL
#define BAUDRATE 9600
#define BAUD_PRESCALE 103 // vzor?ek z datasheetu
#include <avr/io.h>
#include <util/setbaud.h>
void uart_init( void )
{
UBRR0H = UBRRH_VALUE;
UBRR0L = UBRRL_VALUE;
#if USE_2X
UCSR0A |= _BV(U2X0);
#else
UCSR0A &= ~(_BV(U2X0));
#endif
UCSR0C = _BV(UCSZ01) | _BV(UCSZ00); /* 8-bit data */
UCSR0B = _BV(RXEN0) | _BV(TXEN0); /* Enable RX and TX */
}
void uart_putc(char c)
{
if (c == '\n')
{
uart_putc('\r');
}
loop_until_bit_is_set(UCSR0A, UDRE0); /* Wait until data register empty. */
UDR0 = c;
}
void uart_puts(const char *s)
{
while(*s!='\0')
{
uart_putc(*s++);
}
}
char uart_getc(void) {
loop_until_bit_is_set(UCSR0A, RXC0); /* Wait until data exists. */
return UDR0;
}
#include <avr/io.h>
void adc_init(void); // A/D converter initialization
unsigned int adc_read(char a_pin);
#include <avr/io.h>
void adc_init(void)
{
ADMUX = (1<<REFS0); // Vref bude Avcc
ADCSRA = (1<<ADEN) // AD Enable - "zapnutie" ADC
|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0); // nastavenie preddelica
}
unsigned int adc_read(char channel)
{
channel &= 0x0F;
ADMUX = (ADMUX & 0xF0)|channel;
ADCSRA |= (1<<ADSC); // spustenie prevodu
while(ADCSRA & (1<<ADSC))
{ /* cakaj tu */ } // pockam na dokoncenie prevodu
return (ADC);
}
Pridajte sem aj zbalený kompletný projekt, napríklad takto (použite jednoznačné pomenovanie, nemôžeme mať na serveri 10x zdrojaky.zip:
Zdrojový kód: zdrojaky.zip
Overenie
Fungovanie programu som overil zapojením a následným vyskúšaním na robotickom ramienku s tromi servomotorčekmi. Na videu je vidno ako sa rameno pomocou joysticku ovláda a ako sa menia režimy,
Video:
Kľúčové slová 'Category', ktoré sú na konci stránky nemeňte.