Arduino robotplatvorm

• 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

• Jooneandur QTR-3A

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
#define S_MOTOR_LEFT 5
#define S_MOTOR_RIGHT 6
 
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, sensLeftSide, sensRightSide;
  //Calibration in progress inidicator HIGH
  digitalWrite(LED, HIGH);
  while (digitalRead(SWITCH) == LOW);
  delay(200);
 
  for (int i = 0; i < 5; 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);
      }
      if (i == 3) {
        sensLeftSide = map(analogRead(POTPIN), 0, 1023, 0, 180);
        sensM.write(sensLeftSide);
      }
      if (i == 4) {
        sensRightSide = map(analogRead(POTPIN), 0, 1023, 0, 180);
        sensM.write(sensRightSide);
      }
    }
    delay(500);
  }
  while (digitalRead(SWITCH) == LOW);
  delay(1000);
  leftM.write(0);
  rightM.write(180);
  while (digitalRead(SWITCH) == HIGH);
  delay(1000);
    while (digitalRead(SWITCH) == HIGH) {
      fwdLeft = 0;
      fwdRight = map(analogRead(POTPIN), 0, 1023, rightVal, 180);
      rightM.write(fwdRight);
    }
    delay(1000);
    while (digitalRead(SWITCH) == HIGH) {
      fwdRight = 180;
      fwdLeft = map(analogRead(POTPIN), 0, 1023, leftVal, 0);
      leftM.write(fwdLeft);
    }
  //Stop motors
  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(S_MOTOR_LEFT, sensLeftSide);
  EEPROM.update(S_MOTOR_RIGHT, sensRightSide);
  EEPROM.update(L_MOTOR_FWD, fwdLeft);
  EEPROM.update(R_MOTOR_FWD, fwdRight);
  //Calibration end inidicator LOW
  digitalWrite(LED, LOW);
  //Print out values
  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.
• Keerata potentsiomeetrit kuni ultrahelianduri servo on suunatud umbes 60 kraadi roboti keskteljest vasakule.
• Vajutada korraks nuppu.
• Keerata potentsiomeetrit kuni ultrahelianduri servo on suunatud umbes 60 kraadi roboti keskteljest paremale.
• Vajutada korraks nuppu.
• Roboti mõlemad mootorid hakkavad pöörlema maksimum kiirusel. Paigutada robot maha ja vaadata kummale poole robot kaldub. Kui kaldub vasakule, siis tuleb võtta vasaku mootori kiirus maksimum kiiruseks, et robot liiguks otse. Vastasel juhul parema mootori kiirus. Vajadusel võib aeglasemaks liikumiskiiruseks mõlema mootori kiirust vähendada.
• Vajutada korraks nuppu.
• Keerata potentsiomeetrit kuni mootori pöörlemiskiirus on sobiv.
• Vajutada korraks nuppu.
• Keerata potentsiomeetrit kuni roboti teine mootor liigub sama kiirusega ja roboti trajektoor on sirge.
• 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 enne kalibratsiooni algust jadapordi monitor ja see jätta lahti kogu kalibrerimise ajaks.
• Kalibratsiooni lõpus on võimalik potentsiomeetriga testida ultrahelianduri servo liikumist.

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

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>
 
//Parameters
#define TURNDLY 800 //90deg turn delay
#define SMIN 0 //distance from which to activate small turns
#define SMAX 200 //distance from which to deactivate small turns
 
//Servo objects create
Servo leftM, rightM, sensM;
//Sonra distance sensor pins reference and object create
#define TRIG 12
#define ECHO 11
NewPing sonar(TRIG, ECHO, 200);
//Define constants for pin reference
#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
#define S_MOTOR_LEFT 5
#define S_MOTOR_RIGHT 6
 
//Global variables
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 sensLeft = EEPROM.read(S_MOTOR_LEFT);
const int sensRight = EEPROM.read(S_MOTOR_RIGHT);
const int leftFwd = EEPROM.read(L_MOTOR_FWD);
const int rightFwd = EEPROM.read(R_MOTOR_FWD);
 
 
int sonarDist[3];
void motorTurn(int dir);
 
//Functions prototypes
int sweepRun(int pos ,int moveOnly);
void motorStop(int servo);
void motorFwd(void);
 
void setup() {
  //Servo object to Arduino pin setup
  leftM.attach(L_SERVO);
  rightM.attach(R_SERVO);
  sensM.attach(S_SERVO);
  //Serial port setup
  Serial.begin(9600);
  //Pins direction setup
  pinMode(POTPIN, INPUT);
  pinMode(IRSENS, INPUT);
  pinMode(SWITCH, INPUT);
  digitalWrite(SWITCH, HIGH);
  pinMode(LED, OUTPUT);
}
 
//Main program loop
void loop() {
 
  int frontWall;
  int fwdTime = 1000;
  int sweepPos = 0;
  int sweepDir = -1;  // -1 left and 1 right
  unsigned long lastSweep = 0;
  int scanDone = 0;
  unsigned long lastTurn = 0;
 
  //Write initial values to servos
  leftM.write(leftStop);
  rightM.write(rightStop);
  sensM.write(sensCentr);
  //Wait until switch is pressed
  while (digitalRead(SWITCH) == HIGH) {
    digitalWrite(LED, HIGH);
    delay(200);
    digitalWrite(LED, LOW);
    delay(100);
  }
  //Algorithm main loop
  while (1) {
    //check if something is blocking way
    frontWall = digitalRead(IRSENS);
 
    //Sweeping motion distance measure
    if(millis() - lastSweep > 350){
      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();
 
    Serial.print("V: ");
    Serial.print(sonarDist[0]);
    Serial.print("K: ");
    Serial.print(sonarDist[1]);
    Serial.print("P: ");
    Serial.println(sonarDist[2]);
    }
 
    if (frontWall == 0) {
      motorStop(0);
      sweepRun(0, 0);
      if(sonarDist[0] >= sonarDist[2]) motorTurn(-1, TURNDLY);
      else motorTurn(1, TURNDLY);
      lastTurn = millis();
    }
    else if(scanDone == 1 && (millis() - lastTurn > 2000))
    {  
      if (sonarDist[0] < 25) {
        motorStop(0);
        motorTurn(1, 200);
      }
      else if(sonarDist[2] < 25){ 
        motorStop(0);
        motorTurn(-1, 200);
      }
      scanDone = 0;
    }
    else{
      leftM.write(leftFwd);
      rightM.write(rightFwd);
    }
  }
}
//Motors stopping function
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);
  }
}
//Forward moving function
void motorFwd(void){
    leftM.write(leftFwd);
    rightM.write(rightFwd);
}
//Robot on the spot turning function
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);
}
//Sonar sweeping function for servo motor movement
int sweepRun(int pos, int moveOnly){
  if(pos == 2){
    sensM.write(sensCentr);
    delay(400);
    sonarDist[1] = sonar.ping_cm();
    sensM.write(sensLeft);
    delay(400);
    sonarDist[0] = sonar.ping_cm();
    sensM.write(sensRight);
    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(sensLeft);
    if(moveOnly) return 0;
    sonarDist[0] = sonar.ping_cm();
    return sonarDist[0];
  }
  else if(pos == 1){
    sensM.write(sensRight);
    if(moveOnly) return 0;
    sonarDist[2] = sonar.ping_cm();
    return sonarDist[2];
  }
  return 0;
}