Create a gist now

Instantly share code, notes, and snippets.

What would you like to do?
// e-Gizmo OBSTASONIC PBOT jr.
/*
This programs a sumobot with built in line
sensor to avoid lines and stay in a ring.
It also has an ultrasonic sensor that
tells the Pbot to ram the target but avoids
the ring if detected.
Codes by:
e-Gizmo Mechatronix Central
Taft, Manila, Philippines
http://www.e-gizmo.com
January 11, 2014
*/
#include <NewPing.h>
const int LINE_1 = 5; //line sensor 1
const int LINE_2 = 6; //line sensor 2
const int LINE_3 = 7; //line sensor 3
const int DIR_ML = 8; //Left Motor direction low=fwd
const int PWM_ML = 9; //Motor speed
const int PWM_MR = 10; //Motor speed
const int DIR_MR = 11; //Right motor direction low=fwd
const int trigger = 13;
const int echo = 4;
int maxdistance = 200;
NewPing sonar(trigger,echo,maxdistance); // NewPing setup of pins and maximum distance.
int speed;
int ramspeed = 255; // Ram speed
int safespeed = 150; // Neutral speed
int stucktimer = 0;
int timer = 0;
int liner = 0;
int randomizer = 0;
int z = 0;
int y = 0;
float distance; // Distance measured by the ultrasonic sensor
void setup() {
Serial.begin(9600);
pinMode(DIR_ML, OUTPUT); // Set all motor driver pins to output
pinMode(PWM_ML, OUTPUT);
pinMode(PWM_MR, OUTPUT);
pinMode(DIR_MR, OUTPUT);
pinMode(LINE_1, INPUT); // All line sensor pins should be set as input
pinMode(LINE_2, INPUT);
pinMode(LINE_3, INPUT);
}
void loop() {
ultraread();
if(distance > 15)
{
safeDrive();
}
if((z == 0 && distance <= 15) && y == 0)
{
z = 1;
}
if(z == 1 && timer <= 10)
{
LEFT_TURN();
timer++;
stucktimer++;
delay(40);
if(timer == 10)
{
timer = 20;
y = 1;
z = 2;
}
}
// If right is not the way, turn left
if(distance <= 15 && y == 1)
{
y = 2;
}
if(y == 2 && timer >= 20)
{
RIGHT_TURN();
timer++;
stucktimer++;
delay(40);
if(timer == 30)
{
timer = 0;
y = 0;
z = 0;
}
}
// If bot is stuck,
if(stucktimer >= 100)
{
stucktimer = 0;
ROTATE();
delay(800);
}
}
// PRESET FUNCTIONS:
void ultraread()
{
delay(10);
unsigned int uS = sonar.ping(); // Send ping, get ping time in microseconds (uS).
distance = sonar.convert_cm(uS);
Serial.println(distance);
}
void safeDrive()
{
digitalWrite(DIR_ML, LOW);
digitalWrite(DIR_MR, LOW);
analogWrite(PWM_ML, safespeed);
analogWrite(PWM_MR, safespeed);
}
void WIGGLE() // One can use this instead of the rotate
{
digitalWrite(DIR_ML, HIGH);
digitalWrite(PWM_ML, LOW);
digitalWrite(PWM_MR, LOW);
digitalWrite(DIR_MR, LOW);
delay(200);
digitalWrite(DIR_ML, LOW);
digitalWrite(PWM_ML, LOW);
digitalWrite(PWM_MR, LOW);
digitalWrite(DIR_MR, HIGH);
delay(200);
digitalWrite(DIR_ML, HIGH);
digitalWrite(PWM_ML, HIGH);
digitalWrite(PWM_MR, HIGH);
digitalWrite(DIR_MR, HIGH);
}
void ROTATE() // Default reaction after timer runs out
{
digitalWrite(DIR_ML, HIGH);
digitalWrite(PWM_ML, LOW);
digitalWrite(PWM_MR, LOW);
digitalWrite(DIR_MR, LOW);
}
void STOP()
{
digitalWrite(DIR_ML, LOW);
digitalWrite(DIR_MR, LOW);
analogWrite(PWM_ML,1);
analogWrite(PWM_MR,1);
delay(10);
}
void CHARGE()
{
int fulldrive = (ramspeed - 255);
digitalWrite(DIR_ML, LOW);
analogWrite(PWM_ML, fulldrive);
analogWrite(PWM_MR, fulldrive);
digitalWrite(DIR_MR, LOW);
}
void LEFT_TURN()
{
digitalWrite(DIR_ML, LOW);
digitalWrite(DIR_MR, LOW);
analogWrite(PWM_ML,10);
analogWrite(PWM_MR,250);
}
void RIGHT_TURN()
{
digitalWrite(DIR_ML, LOW);
digitalWrite(DIR_MR, LOW);
analogWrite(PWM_ML,250);
analogWrite(PWM_MR,10);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment