Hi i'm having some trouble with getting the zumo robot to move autonomously as its not responding as expected to my ultrasonic sensor readings. i believe my problem is in the if statements i have used but if theres anything else thats wrong that would be a great help too!
propercode.ino (3.59 KB)
#include <ZumoMotors.h>
#include "DHT.h"
#include <Adafruit_Sensor.h>
ZumoMotors motors;
const int motorSpeedR = 200;
const int motorSpeedL = 210;
#define trigPin1 2 //right sensor
#define echoPin1 3
#define trigPin2 4 //left sensor
#define echoPin2 5
#define trigPin3 6 //front sensor
#define echoPin3 12
long duration, distance, rightSensor, frontSensor, leftSensor;
#define DHTPIN 11 // what digital pin we're connected to
// Uncomment whatever type you're using!
//#define DHTTYPE DHT11 // DHT 11
#define DHTTYPE DHT22 // DHT 22 (AM2302), AM2321
//#define DHTTYPE DHT21 // DHT 21 (AM2301)
DHT dht(DHTPIN, DHTTYPE);
void setup()
{
Serial.begin (9600);
pinMode(trigPin1, OUTPUT);
pinMode(echoPin1, INPUT);
pinMode(trigPin2, OUTPUT);
pinMode(echoPin2, INPUT);
pinMode(trigPin3, OUTPUT);
pinMode(echoPin3, INPUT);
Serial.println("DHTxx test!");
dht.begin();
}
void loop() {
SonarSensor(trigPin1, echoPin1);
rightSensor = distance;
delay(50);
SonarSensor(trigPin2, echoPin2);
leftSensor = distance;
delay(50);
SonarSensor(trigPin3, echoPin3);
frontSensor = distance;
delay(50);
// Reading temperature or humidity takes about 250 milliseconds!
// Sensor readings may also be up to 2 seconds 'old' (its a very slow sensor)
float h = dht.readHumidity();
// Read temperature as Celsius (the default)
float t = dht.readTemperature();
// Read temperature as Fahrenheit (isFahrenheit = true)
float f = dht.readTemperature(true);
// Check if any reads failed and exit early (to try again).
if (isnan(h) || isnan(t) || isnan(f)) {
Serial.println("Failed to read from DHT sensor!");
return;
}
// Compute heat index in Fahrenheit (the default)
float hif = dht.computeHeatIndex(f, h);
// Compute heat index in Celsius (isFahreheit = false)
float hic = dht.computeHeatIndex(t, h, false);
if(distance >= 20){ //ignore distances over 20cm
distance = 20;
}
Serial.print(leftSensor);
Serial.print(" - ");
Serial.print(frontSensor);
Serial.print(" - ");
Serial.println(rightSensor);
Serial.print("Humidity: ");
Serial.print(h);
Serial.print(" %\t");
Serial.print("Temperature: ");
Serial.print(t);
Serial.print(" *C ");
Serial.print(f);
Serial.print(" *F\t");
Serial.print("Heat index: ");
Serial.print(hic);
Serial.print(" *C ");
Serial.print(hif);
Serial.println(" *F");
//if statements commanding autonomous control
if(rightSensor & leftSensor & frontSensor >5){ //no close obstruction
forward();
}
else if(frontSensor <= 5 && rightSensor & leftSensor > frontSensor){ //front obstruction
turnRight();
forward();
}
else if(rightSensor <= 5 && frontSensor & leftSensor > rightSensor){ //right obstruction
turnLeft();
forward();
}
else if(leftSensor <= 5 && frontSensor & rightSensor > leftSensor){ //left obstruction
turnRight();
forward();
}
else if(rightSensor & leftSensor & frontSensor <= 5){ //dead end
reverse();
turnRight();
forward();
}
else if(leftSensor & rightSensor <= 5 && frontSensor > 5){ //narrow gap
forward();
}
halt(); //stop after manouver
delay(500);
}
void SonarSensor(int trigPin,int echoPin)
{
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance = (duration/2) / 29.1;
}
//motor controls
void forward(){
motors.setSpeeds(motorSpeedL, motorSpeedR);
delay(500);
}
void turnLeft(){
motors.setSpeeds(-motorSpeedL, motorSpeedR);
delay(250);
}
void turnRight(){
motors.setSpeeds(motorSpeedL, -motorSpeedR);
delay(250);
}
void halt(){
motors.setSpeeds(0, 0);
delay(250);
}
void reverse(){
motors.setSpeeds(-motorSpeedL, -motorSpeedR);
delay(500);
}
Why not add some prints to your code, so your robot can tell you what it is doing?
I believe that sumo requires responsiveness, which is very unlikely, given all the delays in your code.
Why does a sumo bot have a temperature/humidity sensor? To detect the sweat of the other player?
Your code needs some clean-up - here's a start.
// rather than lame names that have to be commented, incorporate the name
const byte rightTrigPin = 2;
const byte rightEchoPin = 3;
const byte leftTrigPin = 4;
const byte lefEtchoPin = 5;
const byte frontTrigPin = 6;
const byte frontEchoPin = 12;
//in setup
pinMode(rightTrigPin, OUTPUT);
digitalWrite (rightTrigPin, LOW);
pinMode(rightEchoPin, INPUT);
pinMode(leftTrigPin, OUTPUT);
digitalWrite (leftTrigPin, LOW);
pinMode(leftEchoPin, INPUT);
pinMode(frontTrigPin, OUTPUT);
digitalWrite (frontTrigPin, LOW);
pinMode(frontEchoPin, INPUT);
// better still, use an array.
// don't use globals to pass return values, use the function mechanism.
unsigned long sonarRange (const byte trigPin, const byte echoPin)
{
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
unsigned long duration = pulseIn(echoPin, HIGH);
return (duration/2) / 29.1;
}