Hi all.
I have started making a small robot the RMOAR https://www.robotshop.com/letsmakerobots/node/20422
but im having some problems with the code i tried to made in order to get this thing working.
I dont know if that code will work and how can i improve it .
Here is the code:
#include <AFMotor.h>
#include <Servo.h>
#define BUMPSENSOR 3
#define BUMPSENSOR2 2
#define TOPSPEED 200
Servo scanservo;
int pos = 0;
int servoStep = 15;
int bumped = digitalRead(BUMPSENSOR);
int bumped2 = digitalRead(BUMPSENSOR2);
AF_DCMotor rightMotor(9, MOTOR12_8KHZ);
AF_DCMotor leftMotor(10, MOTOR12_8KHZ);
void setup()
{
Serial.begin(9600);
pinMode(7, OUTPUT);
scanservo.attach(7);
delay(2000);
rightMotor.setSpeed(TOPSPEED);
leftMotor.setSpeed(TOPSPEED);
pinMode(BUMPSENSOR, INPUT);
pinMode(BUMPSENSOR2, INPUT);
moveForward();
delay(500);
}
void loop()
{
delay(500);
moveForward();
delay(500);
for(pos = 0; pos < 180; pos+= servoStep)
{
scanservo.write(pos);
delay(15);
checkforbump();
}
for(pos = 180; pos > 1; pos-= servoStep)
{
scanservo.write(pos);
delay(15);
checkforbump();
}
}
void checkforbump() {
if(bumped == HIGH || bumped2 == HIGH && pos < 90){
turnRight();
}
if(bumped == HIGH || bumped2 == HIGH && pos > 90){
turnLeft();
}
if(bumped == HIGH || bumped2 == HIGH && pos == 90){
stop();
moveBackward();
}
}
void moveForward(){
Serial.println("Move Forward");
rightMotor.run(FORWARD);
leftMotor.run(FORWARD);
}
void turnLeft(){
Serial.println("Turn Left");
rightMotor.run(BACKWARD);
leftMotor.run(FORWARD);
}
void turnRight(){
Serial.println("Turn Right");
rightMotor.run(FORWARD);
leftMotor.run(BACKWARD);
}
void stop(){
Serial.println("Stop");
rightMotor.run(RELEASE);
leftMotor.run(RELEASE);
delay(500);
}
void moveBackward(){
Serial.println("Move Backward");
rightMotor.run(RELEASE);
leftMotor.run(RELEASE);
rightMotor.run(BACKWARD);
leftMotor.run(BACKWARD);
}
}