December 12 - Code Used To Run Roboplow

This is our final code we are using to run Roboplow. Our Robot is alive!!!!!! A final video soon to come.

	// Use this code to test your Obstacle Avoiding vehicle with the Arduino board:
	// this constant won't change. It's the pin number
	// of the sensor's output:
	const int pingPin = 7;
	// Motors
	int motor_left[] = {6,8};
	int motor_right[] = {3,5};
	int ledPin = 13; // LED connected to digital pin 13
	// Setup
	void setup() {
	Serial.begin(9600);
	// Setup motors
	int i;
	for(i = 0; i < 3; i++){
	pinMode(motor_left[i], OUTPUT);
	pinMode(motor_right[i], OUTPUT);
	pinMode(ledPin, OUTPUT);
	}
	digitalWrite(motor_left[2], HIGH);
	digitalWrite(motor_right[2], HIGH);
	}
	// Loop
	void loop() {
	long duration, inches, cm;
	// The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
	// Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
	pinMode(pingPin, OUTPUT);
	digitalWrite(pingPin, LOW);
	delayMicroseconds(2);
	digitalWrite(pingPin, HIGH);
	delayMicroseconds(5);
	digitalWrite(pingPin, LOW);
	// The same pin is used to read the signal from the PING))): a HIGH
	// pulse whose duration is the time (in microseconds) from the sending
	// of the ping to the reception of its echo off of an object.
	pinMode(pingPin, INPUT);
	duration = pulseIn(pingPin, HIGH);
	// convert the time into a distance
	inches = microsecondsToInches(duration);
	cm = microsecondsToCentimeters(duration);
	Serial.print(duration); Serial.println();
	Serial.print(inches); Serial.print("in, ");
	Serial.print(cm); Serial.print("cm");
	Serial.println();
	if (cm>170)
	{
	Serial.println("Moving Forward");
	drive_straight();
	drive_forward();
	delay(500);
	}
	{
	if (cm<100)
	{
	Serial.println("Obstacle Very Close");
	Serial.println("Evasive Manuever");
	drive_straight();
	drive_backward();
	delay(1000);
	}
	if (cm<=170&&cm>100)
	{
	motor_stop();
	Serial.println("Turning Left");
	turn_left();
	Serial.println("Reverse");
	drive_backward();
	delay(3000);
	Serial.println("Turning Right");
	turn_right();
	Serial.println("Moving Forward");
	drive_forward();
	delay(1000);
	drive_straight();
	drive_forward();
	delay(1000);
	}
	}
	motor_active();
	motor_inactive();
	delay(1);
	}
	// Drive
	void motor_active(){
	digitalWrite(ledPin, HIGH); // set the LED on
	delay(500); // wait for a second
	}
	void motor_inactive(){
	digitalWrite(ledPin, LOW); // set the LED off
	delay(500); // wait for a second
	}
	void motor_stop(){
	digitalWrite(motor_left[0], LOW);
	digitalWrite(motor_left[1], LOW);
	delay(25);
	}
	void drive_straight(){
	digitalWrite(motor_right[0], LOW);
	digitalWrite(motor_right[1], LOW);
	}
	void drive_forward(){
	analogWrite(motor_left[0],200);
	digitalWrite(motor_left[1], LOW);
	}
	void drive_backward(){
	digitalWrite(motor_left[0], LOW);
	digitalWrite(motor_left[1], HIGH);
	}
	void turn_left(){
	digitalWrite(motor_right[0], HIGH);
	digitalWrite(motor_right[1], LOW);
	}
	void turn_right(){
	digitalWrite(motor_right[0], LOW);
	digitalWrite(motor_right[1], HIGH);
	}
	long microsecondsToInches(long microseconds)
	{
	// there are
	// 73.746 microseconds per inch (i.e. sound travels at 1130 feet per
	// second). This gives the distance travelled by the ping, outbound
	// and return, so we divide by 2 to get the distance of the obstacle.
	return microseconds / 148;
	}
	long microsecondsToCentimeters(long microseconds)
	{
	// The speed of sound is 340 m/s or 29 microseconds per centimeter.
	// The ping travels out and back, so to find the distance of the
	// object we take half of the distance travelled.
	return microseconds / 58;
	}

view raw Roboplow Code hosted with ❤ by GitHub