Ovládanie robotického ramienka joystickom II.
Zo stránky SensorWiki
Záverečný projekt predmetu MIPS / LS2025 - Ľuboš Hreňo
Zadanie
Cieľom môjho zadania bolo napísanie programu a následne aj praktické otestovanie ovládania na fyzickom modely robotického ramienka pomocou 2 joystickov a servo-motorčekov, ktoré boli pripojené pomocou dosky Arduino UNO.
Literatúra:
- Dokumentácia k doske Arduino UNO
- Datasheet k 328P
- Datasheet k servo-motoru SG90
- Dokumentácia k Joysticku
- Datasheet k použitému Shieldu
Analýza a opis riešenia
Tento projekt funguje na základe ovládanie štyroch servo-motorčekov s dvomi joystickmi. Pri riešení tohto projektu využívam šírku PWM signálov pre jednotlivé servo-motorčeky. Ak si pozrieme vlastnosti týchto servo-motorov, môžeme si všimnúť, že motorčeky sa vedia otáčať od 0 až 180° . Musíme si uvedomiť tiež ako funguje joystick. Joystick funguje ako 2 potenciometre jeden pre os X a druhý pre os Y. V princípe tieto servá nám pomocou joysticka rozširujú a zároveň skracujú dĺžky PWM signálov pre jednotlivé servo-motory a tým pádom vieme ovládať ich pohyby. Poslednou časťou pre zapojenie bol SHIELD, na ktorý som pripájal jednotlivé komponenty podľa schémy zapojenia, tento shield slúži iba zjednodušenie pripojenia a šetrenia káblov.
Pri zostavení projektu som využil: 2x Joystick
4x Servo-motorček
Arduino UNO senzor shield V5
Schéma zapojenia:
Algoritmus a program
Začneme rozoberať program podľa jednotlivých funkcií. Prvou časťou bude incialízaciou ADC napätia pre joysticky. Následnou funckiou bolo čitanie tohoto napätia, keďže pracujem s 2 servo-motormi a ich prepočet pre 16bitové počítadlo ako si môžeme všimnúť v druhej funkcií. Poslednou využitou funkciou je inicialízácia jednotlivých servo-motorov. Algoritmus využiva Timer0 a Timer1 z dôvodu využitie štyroch servo-motorčekov, tiež 8-bitové a 16-bitové počítadlá, keďže Timer0 pracuje na 8-bitovom a Timer1 na 16-bitovom. Pričom pri zapojení servo motorčekov do konkrétného ramena uvažujem nad tým , že Timer1 je citlivejší ako Timer0. Kde som musel separátne zadávať rýchlost pre jednotlivé servá na Timer0 a Timer1 a tiež zároveň som zadal jednotlivé rozsahy pre konkrétné servá na Timer0 a Timer1 pohybujú. V hlavnej časti programu pracujem s funkciou na čítanie ADC pre jednotlivé potenciometre v joysticku (mám 2 joysticky obe ma 2 osi takže mi treba vybrať 4 hodnoty), ktoré z mojej zaujímavosti vypisujem pomocou funckie uart cez seriový prenos. Nasledujú už iba podmienky pre jednotlivé pohyby joysticka a zmeny polôh servo-motorv s ošetrením hraničkých hodnôt.
Výpis kódu:
#define F_CPU 16000000UL
#define BAUD 9600
#include <avr/io.h>
#include <util/delay.h>
#include "uart.h"
#include <stdio.h>
FILE mystdout = FDEV_SETUP_STREAM(uart_putc, NULL, _FDEV_SETUP_WRITE);
void inicializaciaADC() {
ADMUX = (1 << REFS0); // Referenčné napätie AVCC
ADCSRA = (1 << ADEN) | (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0);
}
uint16_t citanieADC(uint8_t kanal) {
ADMUX = (ADMUX & 0xF0) | kanal;
ADCSRA |= (1 << ADSC);
while (ADCSRA & (1 << ADSC));
return ADC;
}
void inicializaciaServa() {
DDRB |= (1 << PB1) | (1 << PB2); // Servá 1 a 2 (TIMER1)
DDRD |= (1 << PD6) | (1 << PD5); // Servá 3 a 4 (TIMER0)
ICR1 = 39999; // PWM 50 Hz pre TIMER1 (16-bit)
TCCR1A |= (1 << COM1A1) | (1 << COM1B1) | (1 << WGM11);
TCCR1B |= (1 << WGM12) | (1 << WGM13) | (1 << CS11); // Fast PWM, prescaler 8
TCCR0A |= (1 << COM0A1) | (1 << COM0B1) | (1 << WGM01) | (1 << WGM00);
TCCR0B |= (1 << CS01) | (1 << CS00); // Prescaler 64 (8-bit PWM)
}
int main(void) {
uart_init();
stdout = &mystdout;
inicializaciaADC();
inicializaciaServa();
uint16_t natocenie1 = 2000, natocenie2 = 2500;
uint16_t natocenie3 = 128, natocenie4 = 128;
OCR1A = natocenie1;
OCR1B = natocenie2;
OCR0A = natocenie3;
OCR0B = natocenie4;
const uint16_t krok = 80,krok2=10; // rýchlosť pohybu, krok2 musí byt kvôli TIMER0 mat polovičnu velkost (8-bit PWM)
const uint16_t dead_zone = 50;
const uint16_t min_PWM1_s1 = 1500, max_PWM1_s1 = 4200,min_PWM1_s2 = 1500, max_PWM1_s2 = 4200; //nastavenie polomeru otáčania pre každé servo zvlášť pri Timer1
const uint16_t min_PWM0_s3 = 60, max_PWM0_s3 = 180,min_PWM0_s4 = 60, max_PWM0_s4 = 180;//nastavenie polomeru otáčanie pre každé servo zvlášť pri Timer0
while (1) {
uint16_t adcX1 = citanieADC(0);
uint16_t adcY1 = citanieADC(1);
uint16_t adcX2 = citanieADC(2);
uint16_t adcY2 = citanieADC(3);
// vypis pomocou UART
printf("Joystick1 X:%4d Y:%4d | Joystick2 X:%4d Y:%4d\n", adcX1, adcY1, adcX2, adcY2);
// Servo 1
if (adcX1 < (512 - dead_zone) && (natocenie1 > min_PWM1_s1))
{
natocenie1 -= krok;
}
else if (adcX1 > (512 + dead_zone) && (natocenie1 < max_PWM1_s1))
{
natocenie1 += krok;
}
OCR1A = natocenie1;
// Servo 2
if (adcY1 < (512 - dead_zone) && (natocenie2 > min_PWM1_s2))
{
natocenie2 -= krok;
}
else if (adcY1 > (512 + dead_zone) && (natocenie2 < max_PWM1_s2))
{
natocenie2 += krok;
}
OCR1B = natocenie2;
// Servo 3
if (adcX2 < (512 - dead_zone) && (natocenie3 > min_PWM0_s3))
{
natocenie3 -= krok2;
}
else if (adcX2 > (512 + dead_zone) && (natocenie3 < max_PWM0_s3))
{
natocenie3 += krok2;
}
OCR0A = natocenie3;
// Servo 4
if (adcY2 < (512 - dead_zone) && (natocenie4 > min_PWM0_s4))
{
natocenie4 -= krok2;
}
else if (adcY2 > (512 + dead_zone) && (natocenie4 < max_PWM0_s4))
{
natocenie4 += krok2;
}
OCR0B = natocenie4;
_delay_ms(10); // pre rýchlejšie reakcie 10ms
}
}
#include "uart.h"
#include <avr/io.h>
#include <util/setbaud.h>
#define F_CPU 16000000UL
#define BAUDRATE 9600
#define BAUD_PRESCALE (((F_CPU / (BAUDRATE * 16UL))) - 1) // vzor?ek z datasheetu
void uart_init( void )
{
UBRR0H = UBRRH_VALUE;
UBRR0L = UBRRL_VALUE;
UBRR0 = (unsigned char)BAUD_PRESCALE; // Set baud rate: Load the UBRR register
UCSR0C = (1<<UCSZ01)|(1<<UCSZ00); // Set format: 8data, 1stop bit
UCSR0B = (1 << RXEN0) | (1 << TXEN0); // Enable receiver and transmitter
#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)
{
/* toto je vasa uloha */
}
char uart_getc(void) {
loop_until_bit_is_set(UCSR0A, RXC0); /* Wait until data exists. */
return UDR0;
}
#ifndef UART_H_
#define UART_H_
#define F_CPU 16000000UL
#define BAUD 9600
#define BAUD_PRESCALE (((F_CPU / (BAUD * 16UL))) - 1) // vzor?ek z datasheetu
void uart_init( void );
void uart_putc( char c );
void uart_puts( const char *s );
char uart_getc( void );
#endif /* UART_H_ */
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
Pri overovaní tohoto projektu som pôvodne chcel využiť rameno so štyrmi servo-motorčekmi ale nakoniec som využil len s tromi, pričom program funguje na štyri servo-motorčeky. Princíp fungovania a algoritmu tiež popis algorimu si môžeme všimnúť vyššie. V ďalšej časti tejto sekcie si môžeme všimnúť obrázok zapojenia ale už bez ramena pretože pri vyhotovovaní videa pod týmto obrazkom v laboratóriu som zabudol odfotiť zapojenie pre ramienko.
Video:
Kľúčové slová 'Category', ktoré sú na konci stránky nemeňte.