Operácie

Autíčko na diaľkové ovládanie: Rozdiel medzi revíziami

Z SensorWiki

(→‎Algoritmus a program)
(→‎Overenie)
(4 medziľahlé úpravy od rovnakého používateľa nie sú zobrazené.)
Riadok 59: Riadok 59:
 
#include <util/delay.h>
 
#include <util/delay.h>
 
#include <avr/interrupt.h>
 
#include <avr/interrupt.h>
 +
#include <Servo.h>
  
−
#define IR_PIN_1 PB0 // S1 - uplne na pravo (pin D8)
+
#define IR_PIN_0 4 // S0 - uplne na pravo (pin D4)
−
#define IR_PIN_2 PD4 // S2 (pin D4)
+
#define IR_PIN_1 5 // S1 (pin D5)
−
#define IR_PIN_3 PD5 // S3 (pin D5)
+
#define IR_PIN_2 6 // S2 (pin D6)
−
#define IR_PIN_4 PD6 // S4 (pin D6)
+
#define IR_PIN_3 7 // S3 (pin D7)
−
#define IR_PIN_5 PD7 // S5 (pin D7)
+
#define IR_PIN_4 8 // S4 (pin D8)
−
#define IR_PIN_6 PB3 // S6 (pin D11)
+
#define IR_PIN_5 11 // S5 (pin D11)
 +
#define IR_PIN_6 12 // S6 (pin D12)
 +
#define IR_PIN_7 13 // S7 (pin D13)
  
−
#define SERVO_PIN_1 PB1 // lave servo pripojene na pin D9
+
Servo LeftServo;
−
#define SERVO_PIN_2 PB2 // prave servo pripojene na pin D10
+
#define SERVO_PIN_LEFT 9 // lave servo pripojene na pin D9
  
−
#define TRIGGER_PIN PD2 // (pin D2)
+
Servo RightServo;
−
#define ECHO_PIN PD3  // (pin D3)
+
#define SERVO_PIN_RIGHT 10 // prave servo pripojene na pin D10
  
−
#define OBSTACLE_DISTANCE_THRESHOLD 3 // vzdialenost prekazky od robota pri ktorej sa jej zacne vyhybat
+
#define TRIGGER_PIN 2 // (pin D2)
 +
#define ECHO_PIN 3 // (pin D3)
 +
// #define PING_PIN 3
 +
long duration, distance;
  
−
volatile uint16_t sensorValues[6];
+
#define FAST 110
 +
#define SLOW 80
  
−
void servo_init()  
+
#define OBSTACLE_DISTANCE_THRESHOLD 8 // vzdialenost prekazky od robota v cm pri ktorej sa jej zacne vyhybat
 +
 
 +
void setup()  
 
{
 
{
−
   // Konfiguracia Timer1 pre Fast PWM mode, 8-bit
+
   Serial.begin(9600);
−
  TCCR1A |= (1 << WGM10) | (1 << WGM11) | (1 << COM1A1);
 
−
  TCCR1A |= (1 << WGM20) | (1 << WGM21) | (1 << COM1B1);
 
−
  TCCR1B |= (1 << CS11); // Prescaler of 8
 
  
−
   // Nastavenie OC1A/PB1 a OC1B/PB2 na output
+
   LeftServo.attach(SERVO_PIN_LEFT);
−
   DDRB |= (1 << SERVO_PIN_1) | (1 << SERVO_PIN_2);
+
   RightServo.attach(SERVO_PIN_RIGHT);
−
}
 
  
−
void servo_rotate(uint8_t servo, uint8_t angle)
+
  pinMode(2, OUTPUT);
−
{
+
   pinMode(9, OUTPUT);
−
  // Mapovanie uhla (od 0 do 180 stupnov) na PWM hodnotu (od 0 do 255)
+
   pinMode(10, OUTPUT);
−
  uint8_t pwm_value = (angle * 255) / 360;
 
−
   // Nastavenie PWM hodnoty na specificke servo
 
−
  if (servo == 1)  
 
−
  {
 
−
    OCR1A = -pwm_value; // Servo 1
 
−
  }
 
−
   else if (servo == 2)  
 
−
  {
 
−
    OCR1B = pwm_value; // Servo 2
 
−
  }
 
−
}
 
  
−
void setup()  
+
  pinMode(3, INPUT);
−
{
+
   pinMode(4, INPUT);
−
   servo_init();
+
   pinMode(5, INPUT);
−
 
+
   pinMode(6, INPUT);
−
   // Nastavenie IR pins na inputs
+
  pinMode(7, INPUT);
−
   DDRD &= ~((1 << IR_PIN_2) | (1 << IR_PIN_3) | (1 << IR_PIN_4) | (1 << IR_PIN_5) | (1 << ECHO_PIN));
+
  pinMode(8, INPUT);
−
   DDRB &= ~((1 << IR_PIN_1) | (1 << IR_PIN_6));
+
  pinMode(11, INPUT);
−
   DDRD |= (1 << TRIGGER_PIN);
+
   pinMode(12, INPUT);
 +
   pinMode(13, INPUT);
 +
 
 
   // Povolenie prerusenia pre IR piny
 
   // Povolenie prerusenia pre IR piny
 
   PCICR |= (1 << PCIE2) | (1 << PCIE0); // Povolenie PCIE2 pre piny od PCINT16 do PCINT23 a PCIE0 pre piny od PCINT0 do PCINT7
 
   PCICR |= (1 << PCIE2) | (1 << PCIE0); // Povolenie PCIE2 pre piny od PCINT16 do PCINT23 a PCIE0 pre piny od PCINT0 do PCINT7
Riadok 118: Riadok 114:
 
}
 
}
  
−
ISR(PCINT2_vect)  
+
void loop()
 
{
 
{
−
   // ISR pre PCINT2 (IR piny od 1 po 6)
+
   Serial.print("S: ");
−
   sensorValues[0] = digitalRead(IR_PIN_1);
+
   Serial.print(digitalRead(IR_PIN_0));
−
   sensorValues[1] = digitalRead(IR_PIN_2);
+
   Serial.print(digitalRead(IR_PIN_1));
−
  sensorValues[2] = digitalRead(IR_PIN_3);
+
   Serial.print(digitalRead(IR_PIN_2));
−
   sensorValues[3] = digitalRead(IR_PIN_4);
+
   Serial.print(digitalRead(IR_PIN_3));
−
  sensorValues[4] = digitalRead(IR_PIN_5);
+
   Serial.print(digitalRead(IR_PIN_4));
−
  sensorValues[5] = digitalRead(IR_PIN_6);
+
   Serial.print(digitalRead(IR_PIN_5));
−
}
+
   Serial.print(digitalRead(IR_PIN_6));
−
 
+
   Serial.println(digitalRead(IR_PIN_7));
−
void loop()
 
−
{
 
−
  // pohyb
 
−
   if (sensorValues[2] == LOW && sensorValues[3] == LOW)
 
−
  {
 
−
    // dopredu
 
−
    servo_rotate(1, 90);
 
−
    servo_rotate(2, 90);
 
−
   
 
−
   }
 
−
  if(sensorValues[4] == LOW || sensorValues[3] == LOW)
 
−
  {
 
−
    // do lava
 
−
    servo_rotate(1, 10);
 
−
    servo_rotate(2, 0);  
 
−
   }
 
−
  if(sensorValues[2] == LOW || sensorValues[1] == LOW)
 
−
  {
 
−
    // do prava
 
−
    servo_rotate(1, 0);
 
−
    servo_rotate(2, 10);  
 
−
   }
 
−
  if(sensorValues[4] == LOW || sensorValues[5] == LOW)
 
−
  {
 
−
    // do prava
 
−
    servo_rotate(1, 10);
 
−
    servo_rotate(2, 0);  
 
−
   }
 
−
  if(sensorValues[0] == LOW || sensorValues[1] == LOW)
 
−
  {
 
−
    // do prava
 
−
    servo_rotate(1, 0);
 
−
    servo_rotate(2, 10);  
 
−
  }
 
  
−
   while (check_obstacle() == HIGH)  
+
   // pinMode(PING_PIN, OUTPUT);
−
   {
+
  digitalWrite(TRIGGER_PIN, LOW);
−
    servo_rotate(2, 179);  
+
  delayMicroseconds(2);
−
    servo_rotate(1,179);
+
   digitalWrite(TRIGGER_PIN, HIGH);
−
   }
+
  delayMicroseconds(5);
 +
   digitalWrite(TRIGGER_PIN, LOW);
  
−
  delay(10);
+
  // pinMode(PING_PIN, INPUT);
−
}
+
  duration = pulseIn(ECHO_PIN, HIGH);
 +
  distance = (duration*.0343)/2;
 +
  Serial.print("Distance: ");
 +
  Serial.println(distance);
 +
  // distance = microsecondsToCentimeters(duration);
  
 +
  // pohyb
 +
    if(digitalRead(IR_PIN_3) == 1 || digitalRead(IR_PIN_4) == 1)
 +
    {
 +
      // dopredu
 +
      LeftServo.write(90-SLOW);
 +
      RightServo.write(90+SLOW);
 +
    }
 +
    else if(digitalRead(IR_PIN_5) == 1 || digitalRead(IR_PIN_6) == 1)
 +
    {
 +
      // dolava
 +
      LeftServo.write(90-SLOW);
 +
      RightServo.write(90-SLOW);
 +
    }
 +
    else if(digitalRead(IR_PIN_1) == 1 || digitalRead(IR_PIN_2) == 1)
 +
    {
 +
      // do prava
 +
      LeftServo.write(90+SLOW);
 +
      RightServo.write(90+SLOW);
 +
    }
 +
    else if(digitalRead(IR_PIN_0) == 1)
 +
    {
 +
      // ostro do prava
 +
      LeftServo.write(90+FAST);
 +
      RightServo.write(90+FAST);
 +
    }
 +
    else if(digitalRead(IR_PIN_7) == 1)
 +
    {
 +
      // ostro dolava
 +
      LeftServo.write(90-FAST);
 +
      RightServo.write(90-FAST);
 +
    }
 +
    else if(distance <= OBSTACLE_DISTANCE_THRESHOLD)
 +
    {
 +
      LeftServo.write(90+FAST);
 +
      RightServo.write(90+FAST);
 +
      delay(10);
 +
      LeftServo.write(90-FAST);
 +
      RightServo.write(90-FAST);
 +
    }
 
    
 
    
−
// Funkcia na kontrolu prekazok pomocou HC-SR04
+
   delay(5);
−
bool check_obstacle()
 
−
{
 
−
  // Generate trigger pulse for HC-SR04
 
−
  PORTD |= (1 << TRIGGER_PIN);
 
−
  _delay_us(10);
 
−
   PORTD &= ~(1 << TRIGGER_PIN);
 
−
 
 
−
  // Zmeranie sirky ozveny z echo pinu
 
−
  uint32_t pulse_width = 0;
 
−
  while (!(PIND & (1 << ECHO_PIN))); // Cakanie na nabeznu hranu
 
−
  TCNT1 = 0; // Resetovanie casovaca
 
−
  while (PIND & (1 << ECHO_PIN))
 
−
  { // Cakanie na dobeznu hranu
 
−
    if (TCNT1 > 23200) // Maximalna povolena sirka impulzu (400cm)
 
−
      return false; // Ziadna prekazka nebola zaznamenana
 
 
   }
 
   }
−
   pulse_width = TCNT1; // Ziskaj sirku impulzu
+
    
−
 
+
   // long microsecondsToCentimeters(long microseconds)
−
   // Premena sirky impulzu na vzdialenost v cm
+
   // {
−
   uint16_t distance = pulse_width / 58;
+
   // return microseconds / 29 / 2;
−
 
+
   // }
−
   // Kontrola ci prekazka je aspom 3cm od robota
 
−
  if (distance >= OBSTACLE_DISTANCE_THRESHOLD)
 
−
    return false; // V prahovej vzdialenosti nebola zistena ziadna prekazka
 
−
 
 
−
   return true; // Zistena prekazka
 
−
}
 
  
  
 
</source></tab>
 
</source></tab>
 
</tabs>
 
</tabs>
−
Zdrojový kód: [[Médiá:semestralnyProjektKunakova.txt|semestralnyProjektKunakova.txt]]
+
Zdrojový kód: [[Médiá:ProjektMIPS_Kunakova.txt|ProjektMIPS_Kunakova.txt]]
  
 
=== Overenie ===
 
=== Overenie ===
Riadok 215: Riadok 200:
  
 
'''Video:'''
 
'''Video:'''
−
<center><youtube></youtube></center>
+
<center><youtube>https://youtu.be/5oAotqgNLbw</youtube></center>
  
 
Kľúčové slová 'Category', ktoré sú na konci stránky nemeňte.  
 
Kľúčové slová 'Category', ktoré sú na konci stránky nemeňte.  
  
 
[[Category:AVR]] [[Category:MIPS]]
 
[[Category:AVR]] [[Category:MIPS]]

Verzia zo dňa a času 16:15, 12. jún 2024

Záverečný projekt predmetu MIPS / LS2024 - Andrea Kuňáková


Zadanie

Mojou úlohou bolo zapojiť, naprogramovať a overiť funkčnosť path following robota ktorý sa vie vyhýbať prekážkam. Na tento projekt som použila dosku Acrob, 6 infračervených TCRT5000 diód, ultrazvukový senzor HC-SR04 a dve Paralax Continuous Rotation servá.

Vývojová doska ACROB.

TCRT5000

TCRT5000 sú reflektívne senzory, na doske je napájkovaná vysielacia a prijímacia dióda, ktorá je potiahnutá špeciálnym filmom blokujúcim viditeľné žiarenie. Modul je možné použiť pri detekcií prekážok alebo robotoch sledujúcich čiernu čiaru.

IR Senzor TCRT5000.

HC-SR04

Ultrazvukový senzor na meranie vzdialenosti.

Špecifikácie:

Operačné napätie 3.8 – 5.5 V, spotreba 8 mA, frekvencia ultrazvuku 40 kHz, maximálna vzdialenosť 300 cm, minimálna vzdialenosť 1 cm, presnosťou na 0.3 cm, trigger puls 10 mikrosekúnd, úhol detekcie 15°.

HC-SR40 utrazvukový senzor.

Paralax Continuous Rotation Servo

Parallax Continuous Rotation Servo je populárny komponent v robotike vďaka svojej schopnosti rotovať o 360 stupňov, čo umožňuje jeho použitie na pohyb robotov. Tento servo motor sa vyznačuje jednoduchosťou ovládania, keďže smer a rýchlosť rotácie je možné riadiť pomocou PWM signálu. Serva tohto typu sú ideálne pre projekty ako mobilné roboty, kde je potrebná nepretržitá rotácia kolies. Poskytuje dostatočný krútiaci moment pre malé a stredne veľké roboty, čím zaručuje spoľahlivý a efektívny výkon. Jeho napájacie napätie je 4,8 až 6 V a poskytuje maximálny krútiaci moment 38 oz-in (2,7 kg-cm) pri 6 V. Rýchlosť otáčania je približne 60 otáčok za minútu (RPM) pri plnom napájaní, čo umožňuje spoľahlivé a plynulé ovládanie v rôznych robotických aplikáciách​.

Paralax Continuous Rotation Servo.

Literatúra:


Analýza a opis riešenia

Opíšte sem čo a ako ste spravili, ak treba, doplňte obrázkami...

RGB LED.

Nezabudnite doplniť schému zapojenia!

Schéma zapojenia LCD displeja.


Algoritmus a program

#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>
#include <Servo.h>

#define IR_PIN_0 4 // S0 - uplne na pravo (pin D4)
#define IR_PIN_1 5 // S1 (pin D5)
#define IR_PIN_2 6 // S2 (pin D6)
#define IR_PIN_3 7 // S3 (pin D7)
#define IR_PIN_4 8 // S4 (pin D8)
#define IR_PIN_5 11 // S5 (pin D11)
#define IR_PIN_6 12 // S6 (pin D12)
#define IR_PIN_7 13 // S7 (pin D13)

Servo LeftServo;
#define SERVO_PIN_LEFT 9 // lave servo pripojene na pin D9

Servo RightServo;
#define SERVO_PIN_RIGHT 10 // prave servo pripojene na pin D10

#define TRIGGER_PIN 2 // (pin D2)
#define ECHO_PIN 3  // (pin D3)
// #define PING_PIN 3
long duration, distance;

#define FAST 110
#define SLOW 80

#define OBSTACLE_DISTANCE_THRESHOLD 8 // vzdialenost prekazky od robota v cm pri ktorej sa jej zacne vyhybat

void setup() 
{
  Serial.begin(9600);

  LeftServo.attach(SERVO_PIN_LEFT);
  RightServo.attach(SERVO_PIN_RIGHT);

  pinMode(2, OUTPUT);
  pinMode(9, OUTPUT);
  pinMode(10, OUTPUT);

  pinMode(3, INPUT);
  pinMode(4, INPUT);
  pinMode(5, INPUT);
  pinMode(6, INPUT);
  pinMode(7, INPUT);
  pinMode(8, INPUT);
  pinMode(11, INPUT);
  pinMode(12, INPUT);
  pinMode(13, INPUT);
  
  // Povolenie prerusenia pre IR piny
  PCICR |= (1 << PCIE2) | (1 << PCIE0); // Povolenie PCIE2 pre piny od PCINT16 do PCINT23 a PCIE0 pre piny od PCINT0 do PCINT7
  PCMSK2 |= (1 << PCINT20) | (1 << PCINT21) | (1 << PCINT22) | (1 << PCINT23); // Povolenie preruseni pre PD4 az PD7
  PCMSK0 |= (1 << PCINT0) | (1 << PCINT3); // Povolenie preruseni pre PB0 a PB3
  sei();
}

void loop()
{
  Serial.print("S: ");
  Serial.print(digitalRead(IR_PIN_0));
  Serial.print(digitalRead(IR_PIN_1));
  Serial.print(digitalRead(IR_PIN_2));
  Serial.print(digitalRead(IR_PIN_3));
  Serial.print(digitalRead(IR_PIN_4));
  Serial.print(digitalRead(IR_PIN_5));
  Serial.print(digitalRead(IR_PIN_6));
  Serial.println(digitalRead(IR_PIN_7));

  // pinMode(PING_PIN, OUTPUT);
  digitalWrite(TRIGGER_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIGGER_PIN, HIGH);
  delayMicroseconds(5);
  digitalWrite(TRIGGER_PIN, LOW);

  // pinMode(PING_PIN, INPUT);
  duration = pulseIn(ECHO_PIN, HIGH);
  distance = (duration*.0343)/2;
  Serial.print("Distance: ");
  Serial.println(distance);
  // distance = microsecondsToCentimeters(duration);

  // pohyb
    if(digitalRead(IR_PIN_3) == 1 || digitalRead(IR_PIN_4) == 1) 
    { 
      // dopredu
      LeftServo.write(90-SLOW);
      RightServo.write(90+SLOW);
    }
    else if(digitalRead(IR_PIN_5) == 1 || digitalRead(IR_PIN_6) == 1)
    {
      // dolava
      LeftServo.write(90-SLOW);
      RightServo.write(90-SLOW);
    }
    else if(digitalRead(IR_PIN_1) == 1 || digitalRead(IR_PIN_2) == 1)
    {
      // do prava
      LeftServo.write(90+SLOW);
      RightServo.write(90+SLOW);
    }
    else if(digitalRead(IR_PIN_0) == 1)
    {
      // ostro do prava
      LeftServo.write(90+FAST);
      RightServo.write(90+FAST);
    }
    else if(digitalRead(IR_PIN_7) == 1)
    {
      // ostro dolava
      LeftServo.write(90-FAST);
      RightServo.write(90-FAST);
    }
    else if(distance <= OBSTACLE_DISTANCE_THRESHOLD)
    {
      LeftServo.write(90+FAST);
      RightServo.write(90+FAST);
      delay(10);
      LeftServo.write(90-FAST);
      RightServo.write(90-FAST);
    }
  
  delay(5);
  }
  
  // long microsecondsToCentimeters(long microseconds) 
  // {
  // return microseconds / 29 / 2;
  // }

Zdrojový kód: ProjektMIPS_Kunakova.txt

Overenie

Robot na čiare I.
Robot na čiare II.

Video:

Kľúčové slová 'Category', ktoré sú na konci stránky nemeňte.

Zdroj: „https://senzor.robotika.sk/sensorwiki/index.php?title=Autíčko_na_diaľkové_ovládanie&oldid=16640“