Close

Code for Chrissybot3000

A project log for Chrissybot3000

My first robotics project. A bit ambitious.

txdo.msktxdo.msk 08/02/2014 at 01:360 Comments

I pulled the code from a project by Bajdi as it was the closest to what I thought was workable with my setup.

Still some wheel problems.  It might be as simple as not hooking the motorshield upp correctly (doubtful as Seeed's demo works) or adapting this from a bare chip has screwed up some timings.  All I know is, the sensor sweeps and reports and the drive variables and commands show up in the serial monitor debugger.

/*

http://www.bajdi.com (your number one source for buggy Arduino sketches)

Simple obstacle avoiding robot made from parts mostly sourced from Ebay

Micro controller = ATmega328P-PU with Arduino bootloader (3.5$ @ Eb.. noooo Tayda Electronics)

Using a L293D (bought from my favourite Ebay seller Polida) to drive 2 yellow "Ebay motors with yellow wheels"

Detecting obstacles with an SR04 ultrasonic sensor (uber cheap on Ebay)

SR04 sensor mounted on SG90 servo (you can get these servo very cheap on ... oh yes you guessed it: EBay)

Adapted by Ross Potts 7-29-2014 for Seeed Motor Shield V2 BUT I'M STILL HAVING WHEEL ISSUES

*/

#include "NewPing.h" //library for the SR04 ultrasonic sensor

#include "Servo.h" //servo library, SR04 is mounted on this servo

#include "MotorDriver.h"

Servo HeadServo;

//Servo LarmServo;

//Servo RarmServo;

#define TRIGGER_PIN 2 //using analog pins as digital for SR04 sensor

#define ECHO_PIN 3 //because I just happened to plug them in here

#define MAX_DISTANCE 200 //the sensor can't measure much further then this distance

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

#define runEvery(t) for (static typeof(t) _lasttime;(typeof(t))((typeof(t))millis() - _lasttime) > (t);_lasttime += (t))

//const int EN1 = 8; //enable motor 1 = pin 1 of L293D

//const int direction1 = 11; //direcion motor 1 = pin 2 of L293D

//const int EN2 = 12; //enable motor 2 = pin 9 of L293D

//const int direction2 = 13; //direction motor 2 = pin 15 of L293D

// leds are very handy for testing

//const int redLed = 10; // this led will lit up when the robot drives forward

//const int greenLed = 11; // this led will lit up when the robot drives backward

//const int yellowLed = 12; // this led will lit up when the robot turns left

//const int whiteLed = 13; // this led will lit up when the robot turns right

int uS; //value of SR04 ultrasonic sensor

int distance; //distance in cm of ultrasonic sensor

int pos = 90; //start position of servo = 90

int servoDirection = 0; // sweeping left or right

int robotDirection = 0; //0 = forward, 1 = backward, 2 = left, 3 = right

int lastRobotDirection; //last direction of the robot

int distanceCenter;

int distanceLeft;

int distanceRight;

int servoDelay = 20; //with this parameter you can change the sweep speed of the servo

long previousMillis = 0;

const int interval = 650; //interval to switch between the robotDirection, this value will determine

//how long the robot will turn left/right when it detects an obstacle

void setup() {

motordriver.init();

motordriver.setSpeed(200,MOTORB);

motordriver.setSpeed(200,MOTORA);

Serial.begin(9600); //to use the serial monitor

HeadServo.attach(4); //Head servo on pin 8

HeadServo.write(pos); //center Head servo

// LarmServo.attach(5); //Left Arm servo on pin 5

// LarmServo.write(pos); //center Left Arm servo

// RarmServo.attach(6); //Right Arm servo on pin 6

// RarmServo.write(pos); //center Right Arm servo

delay(1500); // delay so we have some time to put the robot on the floor

}

void loop()

{

sweepServo(); //function to sweep the servo

getDistance(); //function to get the distance from the ultrasonic sensor

if (pos >= 15 && pos <= 45)

{

distanceRight = distance; //servo is to the right so measured distance = distanceRight

}

if (pos >= 135 && pos <= 165)

{

distanceLeft = distance; //servo is to the left so measured distance = distanceLeft

}

if (pos > 70 && pos < 110)

{

distanceCenter = distance; //servo is centred so measured distance = distanceCenter

}

if (distanceCenter >= 25) // coast is clear, full power forward

{

robotDirection = 0; //move forward

}

else //obstacle detected, turn left or right?

{

if (distanceLeft > distanceRight)

{

robotDirection = 2; //turn left

}

if (distanceLeft < distanceRight)

{

robotDirection = 3; //turn right

}

if (distanceLeft <= 5 && distanceCenter <= 5 || distanceRight <= 5 && distanceCenter <= 5)

{

robotDirection = 1; // we are stuck <img src="http://www.bajdi.com/wp-includes/images/smilies/icon_sad.gif" alt=":("> lets try and backup

}

}

unsigned long currentMillis = millis(); //set a timer

if(robotDirection == 0 && robotDirection == lastRobotDirection)

{

forward();

lastRobotDirection = robotDirection;

debugMotor();

}

if(robotDirection == 0 && robotDirection != lastRobotDirection && currentMillis - previousMillis > interval )

{

forward();

lastRobotDirection = robotDirection;

previousMillis = currentMillis;

debugMotor();

}

if(robotDirection == 1 && robotDirection == lastRobotDirection)

{

backward();

lastRobotDirection = robotDirection;

debugMotor();

}

if(robotDirection == 1 && robotDirection != lastRobotDirection && currentMillis - previousMillis > interval )

{

backward();

lastRobotDirection = robotDirection;

previousMillis = currentMillis;

debugMotor();

}

if(robotDirection == 2 && robotDirection == lastRobotDirection)

{

left();

lastRobotDirection = robotDirection;

debugMotor();

}

if(robotDirection == 2 && robotDirection != lastRobotDirection && currentMillis - previousMillis > interval )

{

left();

lastRobotDirection = robotDirection;

debugMotor();

previousMillis = currentMillis;

}

if(robotDirection == 3 && robotDirection == lastRobotDirection)

{

right();

lastRobotDirection = robotDirection;

debugMotor();

}

if(robotDirection == 3 && robotDirection != lastRobotDirection && currentMillis - previousMillis > interval )

{

right();

lastRobotDirection = robotDirection;

previousMillis = currentMillis;

debugMotor();

}

}

void forward()

{

motordriver.goForward();

Serial.println("Going Forward");

delay(2000);

}

void stop()

{

motordriver.stop();

Serial.println("Going Nowhere fast");

delay(1000);

}

void backward()

{

motordriver.goBackward();

Serial.println("Going Backward");

delay(2000);

}

void left()

{

motordriver.goLeft();

Serial.println("Going Left");

delay(2000);

}

void right()

{

motordriver.goRight();

Serial.println("Going Right");

delay(2000);

}

/* SERVO AND SENSOR COMMANDS - BEGIN */

void getDistance()

{

runEvery(40) //loop for ultrasonic measurement

{

uS = sonar.ping();

distance = uS / US_ROUNDTRIP_CM;

if (uS == NO_ECHO) // if the sensor did not get a ping

{

distance = MAX_DISTANCE; //so the distance must be bigger then the max vaulue of the sensor

}

Serial.print("Ping: "); //to check distance on the serial monitor

Serial.print(distance);

Serial.println("cm");

}

}

void debugMotor()

{

Serial.print("RobotDirection: "); //to check current direction variable on the serial monitor

Serial.println(robotDirection);

}

void sweepServo()

{

runEvery(servoDelay) //this loop determines the servo position

{

if(pos < 165 && servoDirection == 0) // 165 = servo to the left

{

pos = pos + 5; // +1 was to slow

}

if(pos > 15 && servoDirection == 1) // 15 = servo to the right

{

pos = pos - 5;

}

}

if (pos == 165 )

{

servoDirection = 1; //changes the direction

}

if (pos == 15 )

{

servoDirection = 0; //changes the direction

}

HeadServo.write(pos); //move that servo!

}

/* SERVO AND SENSOR COMMANDS - END */

/*

void WalkArms ()

{

}

void ScaredArms ()

{

}

*/

Discussions