Chrissybot3000

I added the ping sensor into this demo, and also got tired of typing motordriver.goForward and ON an ON an ON.

So I turned the commands in modules, now I just need to finish the other direction modules and then add the second HC-SR04 (Which will require another head), and decide how to use the go backward command:  maybe if both sensor show as being in a corner (backward, left, forward)?

--------------------------------------------------------------------------------------------

// Demo function:The application method to drive the DC motor.

// Author:Frankie.Chu

// Date:20 November, 2012

#include "MotorDriver.h" //SeeedStudio Motor Library

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

#include "Servo.h" //Standard Servo Library

Servo HeadServo;

#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))

int uS; //value of SR04 ultrasonic sensor

int Distance; //distance in cm of ultrasonic sensor

int Too_Close = 50;

//int pos = 90; //Center the Servo

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

//int servoDirection = 0; //Sweep Left or Right

int lastRobotDirection; //last direction of the robot

int distanceCenter;

//int distanceLeft;

//int distanceRight;

//int servoDelay = 2; //Sweep speed

void setup()

{

/*Configure the motor A to control the wheel at the left side.*/

/*Configure the motor B to control the wheel at the right side.*/

motordriver.init();

motordriver.setSpeed(75,MOTORB);

motordriver.setSpeed(90,MOTORA);

Serial.begin(9600);

}

void loop()

{

// sweepServo();

getDistance();

if (Distance < Too_Close)

{

Forward();

//delay(2000);

}

else if (Distance > Too_Close)

{

Left();

}

else

{

Stop();

}

}

void Forward()

{

// sweepServo();

motordriver.goForward();

Serial.print("Going Forward");

}

void Backward()

{

// sweepServo();

motordriver.goBackward();

}

void Left()

{

// sweepServo();

motordriver.goLeft();

Serial.print("Going Left");

}

void Right()

{

// sweepServo();

motordriver.goRight();

}

void Stop()

{

// sweepServo();

motordriver.stop();

}

/* 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 Distance: "); //to check distance on the serial monitor

Serial.print(Distance);

Serial.println("cm");

}

}

/*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 */

I started fresh and after going back over Jeremy Blum's book again for some ideas, and following Daniel toll's advice, I started with parts that do work.

And I also switched some power wires.  D'Oh!

I narrowed the problem down to the servo sweep interfering with the drive motors, so for now, I am eliminating that from the sketch.

I will be adding a second HC-SR04 to give this a stereo-like avoidance combo.

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 ()

{

}

*/

logs can be found for now at #chrissybot3000 at google+ while I transition