Can't figure this out....tried a bunch of different things.
I want it to move forward until the sensor sees an object, then stop, look both ways, pick the longest route, turn, then continue forward.
I feel like the sensor isn't working. I'm using 6 AA batteries hooked up to the motor shield's external power.
I copied most of this code from other examples on other OA robots but the code never fully worked right for me so I've somewhat made it my own. I almost had it working right but it was a bit too fast and didn't respond to walls in time and when I tried to fine tune it I somehow lost my previous code and screwed everything all up. Now I'm pretty much back to square one.
(Also the wheels move way too fast for my liking but I can't seem to slow them down by playing with motor.setSpeed. Only the voltage seems to make a difference on motor speed.)
#include <AFMotor.h>
#include <Servo.h>
#include <NewPing.h>
#define TRIG_PIN A4 //Pin A4 to Ultrasonic Sensor
#define ECHO_PIN A5 //Pin A5 to Ultrasonic Sensor
#define MAX_DISTANCE 200 // Maximum Sensor Distance 200cm
#define COLL_DIST 20 //set distance of stop and reverse to 20cm
#define TURN_DIST 40 // sets distance that robot veers from objects to 40cm
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE); //Set up sensor library to correct pins to measure distance
Servo servo;
AF_DCMotor motor1(1); //Left Wheel
AF_DCMotor motor2(2); //Right Wheel
int leftDistance, rightDistance; //distances on either side
int curDist = 0;
String motorSet ="";
int speedSet = 0;
//-----------------------------------------------------------------------
void setup()
{
servo.attach(9);
servo.write(90); //Set Servo to 90 degrees//facing forward
delay(500);
}
//-----------------------------------------------------------------------
void loop()
{
servo.write(90); //sensor forward
delay(100);
curDist = readPing(); //read distance
if (curDist>COLL_DIST){
changePath();
} //if forward is blocked, change direction
moveForward(); //move forward for 1/2 second
delay(500);
}
//===========================================
void changePath(){
moveStop();
servo.write(36); //check right distance
delay(500);
rightDistance = readPing(); //set right distance
delay(500);
servo.write(144); //check left distance
delay(700);
leftDistance = readPing(); //set left distance
delay(500);
servo.write(90); //center servo
delay(100);
compareDistance();
}
//++++++++++++++++++++++++++++++++++++++++++++++
void compareDistance() //find longest distance
{
if (leftDistance>rightDistance)
{
turnLeft();
}
else if (rightDistance>leftDistance)
{
turnRight();
}
else
{
turnAround();
}
}
//=============================================
int readPing() //read Ultrasonic Sensor
{
delay(70);
int uS = sonar.ping();
int cm = uS/US_ROUNDTRIP_CM;
return cm;
}
//============================================
void moveStop() //Stop motors
{
motor1.run(RELEASE);
motor2.run(RELEASE);
}
//=============================================
void moveForward()
{
motorSet = "FORWARD";
motor1.run(FORWARD);
motor2.run(FORWARD);
motor1.setSpeed(100);
motor2.setSpeed(100);
}
//=============================================
void moveBackward()
{
motorSet = "BACKWARD";
motor1.run(BACKWARD);
motor2.run(BACKWARD);
motor1.setSpeed(50);
motor2.setSpeed(50);
delay(10);
}
//=============================================
void turnRight()
{
motorSet = "RIGHT";
motor1.run(FORWARD);
motor2.run(BACKWARD);
delay(400);
motorSet = "FORWARD";
motor1.run(FORWARD);
motor2.run(FORWARD);
}
//===============================================
void turnLeft()
{
motorSet = "LEFT";
motor1.run(BACKWARD);
motor2.run(FORWARD);
delay(400);
motorSet = "FORWARD";
motor1.run(FORWARD);
motor2.run(FORWARD);
}
//==================================================
void turnAround()
{
motorSet = "RIGHT";
motor1.run(FORWARD);
motor2.run(BACKWARD);
delay(800);
motorSet = "FORWARD";
motor1.run(FORWARD);
motor2.run(FORWARD);
}
The original code that I copied had a speed formula that I changed because it wasn't working right and I had no idea how to make sense of it.
for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2) // slowly bring the speed up to avoid loading down the batteries too quickly
{
motor1.setSpeed(speedSet);
motor2.setSpeed(speedSet+MAX_SPEED_OFFSET);
delay(5);
This was on forward and back. Wouldn't the wheels need to be the same speed for forward and back?
Originally when I used this person's code only one motor would work and my bot just went in a circle.
EDIT
Multimeter is showing about 8.4V right now
Dunno if that would help figure out the problem or not.
