Ovládanie robotického ramienka joystickom

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.

Vývojová doska Arduino Uno.

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:

Použitý joystick

Použitý servomotor

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:

Pre Timer2 teda hodnota 62,5 zodpovedá 0 z joysticku a 125 zodpovedá 1023.

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:

Pre Timer1 zodpovedá hodnota 2000 hodnote 0 z joysticku a 4000 zodpovedá 1023.

Schéma zapojenia.

Algoritmus a program

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

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". Hodnoty "posx" a "posy" sú potom načítané do OCR, takto je zaistené, že si servomotorčeky udržia nastavenú polohu. Medzi horným a dolným servomotorom je treba prepínať, keďže chceme v jednom čase ovládať iba jeden z nich. Voľba ovládania 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.

programuart.huart.cadc.hadc.c

#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);
}

Zdrojový kód: Ovladanaie_ramena.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.

Zapojenie.

Video: