Arduino robotplatvorm

Ülevaade

Iteaduino UNO
2 standard suurusega servo mootorit (360 kraadi pöörlemisega)
mini servo (180 kraadi pöörlemine)
Infrapuna lähedusandur 3-80 cm (digitaal)
Ultraheliandur HC-SR04 2-500cm
Nupp 8×8 mm ON-(OFF)
Potentsiomeeter 10kohm
vineerist roboti põhi
6-AA patarei pesa
mõõdud: 15,5 x 12 x 10 cm
kaal: 350 g

Robotplatvormi kokkupanek

Lisamoodulid

Jooneandur QTR-3A

Näiteprogrammid

Mootorite kalibreerimine

Programmi kaudu leiab servo mootorite peatumise ja otse liikumise väärtused. Lisaks leitakse keskel asetsemise väärtus anduri servole. Saadud väärtused salvestatakse EEPROM püsimällu, kust järgnevad programmid saavad neid juba kasutada.

#include <Servo.h>
#include <EEPROM.h>
 
//Servo objektide loomine
Servo leftM, rightM, sensM;
//Roboti osadele viikude määramine
#define LED 13
#define POTPIN A0
#define IRSENS A1
#define SWITCH A2
#define L_SERVO 10
#define R_SERVO 9
#define S_SERVO 8
//EEPROM bytes
#define L_MOTOR_STOP 0
#define R_MOTOR_STOP 1
#define S_MOTOR_CENTR 2
#define L_MOTOR_FWD 3
#define R_MOTOR_FWD 4
 
void setup() {
  //Servo objekti ja viigu sidumine
  leftM.attach(L_SERVO);
  rightM.attach(R_SERVO);
  sensM.attach(S_SERVO);
  //Jadaliidese käivitamine
  Serial.begin(9600);
  //viikude seadistamine
  pinMode(POTPIN, INPUT);
  pinMode(IRSENS, INPUT);
  pinMode(SWITCH, INPUT);
  digitalWrite(SWITCH, HIGH);
  pinMode(LED, OUTPUT);
}
 
void calibMotors() {
  int leftVal, rightVal, sensVal, fwdLeft, fwdRight;
  ////Kalibratsiooni algus. Indikaatori tuli läheb põlema. (Viigul 13 led)
  digitalWrite(LED, HIGH);
  while (digitalRead(SWITCH) == LOW);
  delay(200);
 
  for (int i = 0; i < 3; i++) {
    while (digitalRead(SWITCH) == HIGH) {
      if (i == 0) {
        leftVal = map(analogRead(POTPIN), 0, 1023, 50, 120);
        leftM.write(leftVal);
      }
      if (i == 1) {
        rightVal = map(analogRead(POTPIN), 0, 1023, 50, 120);
        rightM.write(rightVal);
      }
      if (i == 2) {
        sensVal = map(analogRead(POTPIN), 0, 1023, 30, 150);
        sensM.write(sensVal);
      }
    }
    delay(500);
  }
  while (digitalRead(SWITCH) == LOW);
  delay(1000);
  leftM.write(0);
  rightM.write(180);
  while (digitalRead(SWITCH) == HIGH);
  delay(1000);
  if (digitalRead(IRSENS) == 1) {
    while (digitalRead(SWITCH) == HIGH) {
      fwdLeft = 0;
      fwdRight = map(analogRead(POTPIN), 0, 1023, rightVal, 180);
      rightM.write(fwdRight);
    }
  }
  else {
    while (digitalRead(SWITCH) == HIGH) {
      fwdRight = 180;
      fwdLeft = map(analogRead(POTPIN), 0, 1023, leftVal, 0);
      leftM.write(fwdLeft);
    }
  }
  //Peata mootorid
  leftM.write(leftVal);
  rightM.write(rightVal);
  sensM.write(sensVal);
  delay(1000);
  while (digitalRead(SWITCH) == LOW);
  delay(200);
 
 
  EEPROM.update(L_MOTOR_STOP, leftVal);
  EEPROM.update(R_MOTOR_STOP, rightVal);
  EEPROM.update(S_MOTOR_CENTR, sensVal);
  EEPROM.update(L_MOTOR_FWD, fwdLeft);
  EEPROM.update(R_MOTOR_FWD, fwdRight);
  //Kalibratsiooni lõpp. Indikaatori tuli kustub. (Viigul 13 led)
  digitalWrite(LED, LOW);
  //Prindi välja väärtused jadaporti
  Serial.print("L: ");
  Serial.println(EEPROM.read(L_MOTOR_STOP));
  Serial.print("R: ");
  Serial.println(EEPROM.read(R_MOTOR_STOP));
  Serial.print("S: ");
  Serial.println(EEPROM.read(S_MOTOR_CENTR));
  Serial.print("FWD L: ");
  Serial.println(EEPROM.read(L_MOTOR_FWD));
  Serial.print("FWD R: ");
  Serial.println(EEPROM.read(R_MOTOR_FWD));
}
 
void loop() {
  while (digitalRead(SWITCH) == HIGH);
  calibMotors();
  delay(1000);
  while (1) {
    sensM.write(map(analogRead(POTPIN), 0, 1023, 0, 180));
  }
}

Programmi töö selgitus:

Vajutada korraks nuppu
Keerata potentsiomeetrit kuni vasak mootor jääb seisma
Vajutada korraks nuppu
Keerata potentsiomeetrit kuni parem mootor jääb seisma
Vajutada korraks nuppu
Keerata potentsiomeetrit kuni ultrahelianduri servo on võimalikult otse suunatud.
Vajutada korraks nuppu
Paigutada robot maha ja vaadata kummale poole robot kaldub. Kui kaldub vasakule, siis tuleb korrigeerida parema mootori kiirust. Teisel juhul vastupidi.
Vajutada korraks nuppu (Kui hoida kätt digitaalse infrapunaanduri ees, siis korrigeeritakse vasakut mootorit. Kui anduril takistust ees ei ole, siis korrigeeritakse paremat mootorit.)
Keerata potentsiomeetrit kuni robot sõidab enamvähem otse
Vajutada korraks nuppu
Kalibratsioon on lõpule jõudnud ja väärtused EEPROM mällu salvestatud. Kalibratsiooni väärtusi on võimalik näha kui Arduino IDE-s avada juba kalibratsiooni alguses jadapordi monitor.

Lihtne navigeerimine

Programm näeb ette roboti navigeerimist ruumis. Robot sõidab otse kuni ette tuleb sein, siis robot pöörab vasakule ja jätkab otse sõitu.

#include <Servo.h>
#include <EEPROM.h>
 
//Servo objektide loomine
Servo leftM, rightM, sensM;
//Roboti osadele viikude määramine
#define LED 13
#define POTPIN A0
#define IRSENS A1
#define SWITCH A2
#define L_SERVO 10
#define R_SERVO 9
#define S_SERVO 8
//EEPROM bytes
#define L_MOTOR_STOP 0
#define R_MOTOR_STOP 1
#define S_MOTOR_CENTR 2
#define L_MOTOR_FWD 3
#define R_MOTOR_FWD 4
 
void setup() {
  //Servo objekti ja viigu sidumine
  leftM.attach(L_SERVO);
  rightM.attach(R_SERVO);
  sensM.attach(S_SERVO);
  //Jadaliidese käivitamine
  Serial.begin(9600);
  //viikude seadistamine
  pinMode(POTPIN, INPUT);
  pinMode(IRSENS, INPUT);
  pinMode(SWITCH, INPUT);
  digitalWrite(SWITCH, HIGH);
  pinMode(LED, OUTPUT);
}
 
void loop() {
 
  int frontWall;
  int leftStop = EEPROM.read(L_MOTOR_STOP);
  int rightStop = EEPROM.read(R_MOTOR_STOP);
  int sensCentr = EEPROM.read(S_MOTOR_CENTR);
  int leftFwd = EEPROM.read(L_MOTOR_FWD);
  int rightFwd = EEPROM.read(R_MOTOR_FWD);
 
  //Kirjuta servodele algväärtused
  leftM.write(leftStop);
  rightM.write(rightStop);
  sensM.write(sensCentr);
  //Ootab kuni lülitit vajutatakse
  while (digitalRead(SWITCH) == HIGH) {
    digitalWrite(LED, HIGH);
    delay(200);
    digitalWrite(LED, LOW);
    delay(100);
  }
  //Algoritmi lõputu tükkel
  while (1) {
    frontWall = digitalRead(IRSENS);
 
    //Kui sein ees, siis pööra 1000 millisekundit vasakule
    if (frontWall == 0) {
      leftM.write(180);
      rightM.write(0);
      delay(500);
      leftM.write(180);
      rightM.write(180);
      delay(600);
    }
    else {
      leftM.write(leftFwd);
      rightM.write(rightFwd);
    }
  }
}

Keerukas navigeerimine

Programm näeb ette roboti navigeerimist labürindis. Robot jälgib pidevalt kolme suunda: vasak, otse, parem. Lisaks jälgib otse suunda pidevalt infrapuna lähedusandur. Ultrahelianduri lugemiseks on vaja eelnevalt alla tõmmata NewPing teek.

NewPing teek Arduinole

Programm on tükeldatud mitmeteks funktsioonideks, et oleks lihtsam koostada algoritmi. Programmi põhitsükkel kontrollib pidevalt, et ees poleks ühtegi objekti.

#include <Servo.h>
#include <EEPROM.h>
#include <NewPing.h>
 
//Parameetrid
#define SONARDIFF 10 //miinimum kahe suuna vahe, et teha väikest pööret
#define TURNDLY 800 //90 kraadise pöörde tegemise viide
#define SMIN 40 //kaugus millest alates aktiveerida väikse pöörde tegemine
#define SMAX 200 //kaugus, millest alates lõpetada väikse pöörde tegemine
 
//Servo objektide loomine
Servo leftM, rightM, sensM;
//Ultrahelianduri(Sonar) objekti loomine ja viikude määramine
#define TRIG 12
#define ECHO 11
NewPing sonar(TRIG, ECHO, 200);
//Roboti osadele viikude määramine
#define LED 13
#define POTPIN A0
#define IRSENS A1
#define SWITCH A2
#define L_SERVO 10
#define R_SERVO 9
#define S_SERVO 8
//EEPROM bitid
#define L_MOTOR_STOP 0
#define R_MOTOR_STOP 1
#define S_MOTOR_CENTR 2
#define L_MOTOR_FWD 3
#define R_MOTOR_FWD 4
 
//Globaalsed muutujad
const int leftStop = EEPROM.read(L_MOTOR_STOP);
const int rightStop = EEPROM.read(R_MOTOR_STOP);
const int sensCentr = EEPROM.read(S_MOTOR_CENTR);
const int leftFwd = EEPROM.read(L_MOTOR_FWD);
const int rightFwd = EEPROM.read(R_MOTOR_FWD);
int sonarDist[3];
void motorTurn(int dir);
 
//Funktsioonide prototüübid
int sweepRun(int pos ,int moveOnly);
void motorStop(int servo);
void motorFwd(void);
 
void setup() {
  //Servo objekti ja viigu sidumine
  leftM.attach(L_SERVO);
  rightM.attach(R_SERVO);
  sensM.attach(S_SERVO);
  //Jadaliidese käivitamine
  Serial.begin(9600);
  //viikude seadistamine
  pinMode(POTPIN, INPUT);
  pinMode(IRSENS, INPUT);
  pinMode(SWITCH, INPUT);
  digitalWrite(SWITCH, HIGH);
  pinMode(LED, OUTPUT);
}
 
void loop() {
  //Lokaalsed muutujad
  int frontWall;
  int fwdTime = 1000;
  int sweepPos = 0;
  int sweepDir = -1;  // -1 vasakule ja 1 paremale
  unsigned long lastSweep = 0;
  int scanDone = 0;
  int sonarDiff;
  unsigned long lastTurn = 0;
 
  //Kirjuta servodele algväärtused
  leftM.write(leftStop);
  rightM.write(rightStop);
  sensM.write(sensCentr);
  //Wait for switch press to start
  while (digitalRead(SWITCH) == HIGH) {
    digitalWrite(LED, HIGH);
    delay(200);
    digitalWrite(LED, LOW);
    delay(100);
  }
  //Algoritmi lõputu tsükkel
  while (1) {
    //Loe ees oleva infrapunaanduri väärtus
    frontWall = digitalRead(IRSENS);
    //Ultraheliandur käib edasi tagasi, et pidevalt vaadata kolme suunda
    if(millis() - lastSweep > 400){
      sweepRun(sweepPos, 0);
      if(sweepPos == -1 || sweepPos == 1) scanDone = 1;
      // increment or decrement position
      if(sweepDir == 1) sweepPos++;
      else sweepPos--;
      // change direction if edge reached
      if(sweepDir == 1 && sweepPos == 1){ sweepDir = -1;}
      else if(sweepDir == -1 && sweepPos == -1){ sweepDir = 1;}
      sweepRun(sweepPos, 1);
      lastSweep = millis();
    }
 
    if (frontWall == 0) {
      motorStop(0);
      sweepRun(0, 0);
      if(sonarDist[0] >= sonarDist[2]) motorTurn(-1, TURNDLY);
      else motorTurn(1, TURNDLY);
      lastTurn = millis();
    }
    else if(sonarDist[0] > 0 && sonarDist[2] > 0 && scanDone == 1 && sonarDist[1] > SMIN && sonarDist[1] < SMAX && (millis() - lastTurn > 2000))
    {
      //calculate sonar sensors distance difference
      sonarDiff = sonarDist[0] - sonarDist[2];
 
      if (sonarDiff > SONARDIFF) {
        motorStop(0);
        motorTurn(-1, 200);
      }
      else if(sonarDiff < -1*SONARDIFF){ 
        motorStop(0);
        motorTurn(1, 200);
      }
      scanDone = 0;
    }
    else{
      leftM.write(leftFwd);
      rightM.write(rightFwd);
    }
  }
}
 
//Mootorite koos või eraldi peatamise funktsioon
void motorStop(int wheel){
  if(wheel == 0){
    leftM.write(leftStop);
    rightM.write(rightStop);
  }
  else if(wheel == -1){
    leftM.write(leftStop);
  }
  else if(wheel == 1){
    rightM.write(rightStop);
  }
}
 
//Otse liikumise funktsioon
void motorFwd(void){
    leftM.write(leftFwd);
    rightM.write(rightFwd);
}
 
//Pööramise funktsioon
void motorTurn(int dir, int dly){
  //left
  if(dir == -1){
    leftM.write(180);
    rightM.write(180);
  }
  //right
  else if(dir == 1){
    leftM.write(0);
    rightM.write(0);
  }
  delay(dly);
}
 
//Ultrahelianduri lugemise funktsioon
int sweepRun(int pos, int moveOnly){
  if(pos == 2){
    sensM.write(sensCentr);
    delay(400);
    sonarDist[1] = sonar.ping_cm();
    sensM.write(180);
    delay(400);
    sonarDist[0] = sonar.ping_cm();
    sensM.write(0);
    delay(800);
    sonarDist[2] = sonar.ping_cm();
    sensM.write(sensCentr);
  }
  else if(pos == 0){
    sensM.write(sensCentr);
    if(moveOnly) return 0;
    sonarDist[1] = sonar.ping_cm();
    return sonarDist[1];
  }
  else if(pos == -1){
    sensM.write(180);
    if(moveOnly) return 0;
    sonarDist[0] = sonar.ping_cm();
    return sonarDist[0];
  }
  else if(pos == 1){
    sensM.write(0);
    if(moveOnly) return 0;
    sonarDist[2] = sonar.ping_cm();
    return sonarDist[2];
  }
  return 0;
}

et/projects/arduino.1451306763.txt.gz · Last modified: 2020/07/20 09:00 (external edit)

Table of Contents