Ovládanie servomotorčekov na manipulátore joystickmi
Zo stránky SensorWiki
Záverečný projekt predmetu MIPS / LS2026 - Marek Kecskés
Zadanie
Zadaním projektu bolo navrhnúť zapojenie a následne naprogramovať riadenie dvoch servomotorčekov, ktoré ovládajú kolesá manipulátora, pomocou jedného analógového joysticku.
Literatúra:
Analýza a opis riešenia
Cieľom riešenia bolo vytvoriť jednoduchý manipulátor, ktorého dva servomotorčeky sa dajú ovládať jedným analógovým joystickom. Joystick má dve osi. Jedna os slúži na ovládanie pohybu hore/dole, a druhá os na ovládanie pohybu vľavo/vpravo. Výstup z joysticku je analógový, preto je pripojený na analógové vstupy Arduina a číta sa pomocou A/D prevodníka.
Použité komponenty:
- Arduino UNO
- Analógový joystick
- 2x modifikovaný servomotorček
- Externý batériový zdroj 6 V pre servomotorčeky
- Breadboard a vodiče
Arduino UNO bolo napájané cez USB z počítača. Joystick bol napájaný z 5 V pinu Arduina a jeho výstupy boli pripojené na analógové vstupy A0 a A1. Výstup A0 sa používa pre os hore/dole a výstup A1 pre os vľavo/vpravo. Signály pre servomotorčeky boli pripojené na piny D9 a D10, ktoré zodpovedajú výstupom OC1A a OC1B časovača Timer1. Samotné servomotorčeky boli napájané z externého batériového zdroja 6 V. Zem batériového zdroja bola spojená so zemou Arduina, aby mali Arduino a servomotorčeky spoločnú referenciu pre PWM signál
Algoritmus a program
Algoritmus programu využíva A/D prevodník na čítanie polohy joysticku, Timer1 na generovanie PWM signálu pre servomotorčeky a UART komunikáciu na kontrolný výpis hodnôt do programu PuTTY. Knižnica pre A/D prevodník bola použitá z cvičení, preto ju v zdrojovom kóde neuvádzam. Pre riadenie servomotorčekov bola vytvorená knižnica servo.h a servo.c. V nej sa nastavuje Timer1 mikrokontroléra ATmega328P do režimu Fast PWM. Výstupy OC1A a OC1B sú na doske Arduino UNO pripojené na piny D9 a D10, ktoré ovládajú ľavý a pravý servomotorček. Vo funkcii servo_init() sa nastavia piny PB1 a PB2 ako výstupy a Timer1 sa nastaví tak, aby generoval PWM signál s periódou približne 20 ms, teda 50 Hz. Šírka impulzu sa nastavuje cez registre OCR1A a OCR1B. Hodnota približne 3000 predstavuje STOP, nižšia a vyššia hodnota určujú smer otáčania. V hlavnom programe sa najskôr volajú funkcie adc_init(), servo_init() a uart_init(). Potom sa zavolá servo_stop_vsetko(), aby servomotorčeky po zapnutí stáli. V slučke while(1) sa čítajú hodnoty joysticku z A0 a A1 pomocou adc_read(), vypisujú sa do PuTTY a podľa ich veľkosti sa volajú funkcie servo_lavekoleso(), servo_pravekoleso() alebo servo_stop_vsetko().
#define F_CPU 16000000UL
#include <avr/io.h>
#include <util/delay.h>
#include <stdlib.h>
#include "adc.h"
#include "servo.h"
// Nastavenie aby Arduino posielalo text a cisla do PuTTY cez seriovy port
// Inicializacia UART - 9600 baud, vypocet: UBRR = 16000000 / (16 * 9600) - 1 = 103
void uart_init(void)
{
UBRR0H = 0;
UBRR0L = 103; // 9600 baud pri 16MHz
UCSR0B = (1 << TXEN0); // Zapinanie odosielanie dat
UCSR0C = (1 << UCSZ01) | (1 << UCSZ00); // Nastavenie formatu komunikacie na 8 datovych bitov
}
// Odosle jeden znak cez UART
void uart_send_char(char c)
{
while (!(UCSR0A & (1 << UDRE0))); // Cakaj kym je buffer volny
UDR0 = c;
}
// Odosle retazec znakov cez UART
void uart_send_string(char *s)
{
while (*s) { uart_send_char(*s); s++; }
}
// Odosle cislo ako retazec cez UART
void uart_send_number(unsigned int num)
{
char buffer[10];
itoa(num, buffer, 10);
uart_send_string(buffer);
}
int main(void)
{
unsigned int adc_x; // Os vlavo a vpravo
unsigned int adc_y; // Os hore a dole
// Inicializacia periferii
adc_init();
servo_init();
uart_init();
servo_stop_vsetko();
uart_send_string("=== START ===\r\n");
_delay_ms(1000);
while (1)
{
// Citanie joysticku
adc_x = adc_read(1); // A1 = os vlavo a vpravo
adc_y = adc_read(0); // A0 = os hore a dole
// Vypis hodnot do PuTTY
uart_send_string("X=");
uart_send_number(adc_x);
uart_send_string(" Y=");
uart_send_number(adc_y);
uart_send_string(" | ");
// Dopredu: joystick hore, adc_y ide k 0
if (adc_y < 300)
{
servo_lavekoleso(SERVO_MAX_VZADU);
servo_pravekoleso(SERVO_MAX_VPREDU);
uart_send_string("DOPREDU\r\n");
}
// Dozadu: joystick dole, adc_y ide k 1023
else if (adc_y > 700)
{
servo_lavekoleso(SERVO_MAX_VPREDU);
servo_pravekoleso(SERVO_MAX_VZADU);
uart_send_string("DOZADU\r\n");
}
// Doprava: joystick vpravo, adc_x ide k 0
else if (adc_x < 300)
{
servo_lavekoleso(SERVO_MAX_VZADU);
servo_pravekoleso(SERVO_MAX_VZADU);
uart_send_string("DOPRAVA\r\n");
}
// Dolava: joystick vlavo, adc_x ide k 1023
else if (adc_x > 700)
{
servo_lavekoleso(SERVO_MAX_VPREDU);
servo_pravekoleso(SERVO_MAX_VPREDU);
uart_send_string("DOLAVA\r\n");
}
// STOP: joystick v strede
else
{
servo_stop_vsetko();
uart_send_string("STOP\r\n");
}
_delay_ms(100); // Kratke oneskorenie pre stabilitu ADC
}
return 0;
}
#include <avr/io.h>
// Timer1 vrchna hodnota 20ms, 50Hz
#define SERVO_ICR1_TOP 39999U
// OCR hodnoty pre riadenie serva
#define SERVO_STOP 2999U // servo stoji
#define SERVO_MAX_VPREDU 4000U // max rychlost vpredu
#define SERVO_MAX_VZADU 2000U // max rychlost vzadu
// Prahove hodnoty joysticku
#define JOYSTICK_STRED 512U // stredova hodnota
#define JOYSTICK_DEAD_ZONE 80U // martva zona okolo stredu
void servo_init(void); // Nastavi piny PB1 a PB2 ako vystupy
void servo_lavekoleso(unsigned int hodnota); // Nastavi rychlost laveho kolesa a zapisuje do OCR1A
void servo_pravekoleso(unsigned int hodnota); // Nastavi rychlost praveho kolesa a zapisuje do OCR1B
void servo_stop_vsetko(void); // Zastavi obe kolesa a zapise SERVO_STOP do OCR1A aj OCR1B
#include <avr/io.h>
#include "servo.h"
void servo_init(void)
{
DDRB |= (1 << PB1) | (1 << PB2); // PB1 a PB2 ako vystupy
TCNT1 = 0;
ICR1 = SERVO_ICR1_TOP; // Vrch = 39999
OCR1A = SERVO_STOP; // Lave koleso STOP na zaciatku
OCR1B = SERVO_STOP; // Prave koleso STOP na zaciatku
TCCR1A = (1 << COM1A1) | (1 << COM1B1) | (1 << WGM11); // Timer1 sa nastavuje do rezimu Fast PWM, kde periodu urcuje register ICR1
TCCR1B = (1 << WGM13) | (1 << WGM12) | (1 << CS11); // Nastavenie preddelicky 8, pri 16 ICR1 = 39999 vznikne PWM signal s frekvenciou 50 Hz.
}
void servo_lavekoleso(unsigned int hodnota)
{
if (hodnota < SERVO_MAX_VZADU) hodnota = SERVO_MAX_VZADU; // Orezanie rozsahu
if (hodnota > SERVO_MAX_VPREDU) hodnota = SERVO_MAX_VPREDU;
OCR1A = hodnota;
}
void servo_pravekoleso(unsigned int hodnota)
{
if (hodnota < SERVO_MAX_VZADU) hodnota = SERVO_MAX_VZADU; // Orezanie rozsahu
if (hodnota > SERVO_MAX_VPREDU) hodnota = SERVO_MAX_VPREDU;
OCR1B = hodnota;
}
void servo_stop_vsetko(void)
{
OCR1A = SERVO_STOP; // Zastav lave koleso
OCR1B = SERVO_STOP; // Zastav prave koleso
}
Zdrojový kód: zdrojaky.zip
Overenie
Projekt som overoval postupne po častiach. Najskôr som otestoval, či sa program správne nahráva do dosky Arduino UNO. Potom som otestoval UART komunikáciu cez PuTTY. Do programu som pridal jednoduchý výpis textu a následne aj výpis hodnôt joysticku. V PuTTY som sledoval hodnoty X a Y pri rôznych polohách joysticku. Pri joysticku v stredovej polohe boli hodnoty približne okolo 512. Pri vychýlení joysticku do krajných polôh sa hodnoty menili smerom k 0 alebo k 1023. Po overení joysticku som pripojil servomotorčeky. Najskôr som testoval hodnotu STOP, pri ktorej sa servomotorčeky nemali točiť. Potom som skúšal hodnoty pre oba smery otáčania. Keďže servomotorčeky sú na manipulátore osadené oproti sebe, bolo potrebné upraviť hodnoty pre ľavý a pravý servomotorček tak, aby pohyb manipulátora zodpovedal polohe joysticku. Testované stavy:
- joystick v strede - obidva servomotorčeky stoja
- joystick hore - manipulátor sa pohybuje jedným smerom
- joystick dole - manipulátor sa pohybuje opačným smerom
- joystick doprava - manipulátor sa otáča doprava
- joystick doľava - manipulátor sa otáča doľava
- výpis hodnôt joysticku v PuTTY
Video:
Čo by som urobil inak
Pri ďalšom vylepšení projektu by som sa zameral hlavne na plynulejšie riadenie rýchlosti. V aktuálnej verzii je ovládanie riešené pomocou jednoduchých prahových hodnôt joysticku. Bolo by lepšie mapovať hodnotu joysticku plynule na PWM hodnotu servomotorčekov. Manipulátor by sa potom mohol pohybovať pomalšie alebo rýchlejšie podľa toho, ako veľmi je joystick vychýlený. Ďalším vylepšením by mohlo byť doplnenie snímačov polohy aby manipulator nenarazil.