CODE

//Code written by Prabhjyot and Ben

const int pingPin = 6; // Trigger Pin of Ultrasonic Sensor
const int echoPin = 7; // Echo Pin of Ultrasonic Sensor

​

//libraries used for code
#include <Servo.h> //core library used ot move an serov motors
#include <Oscillator.h>
#include <US.h>
#include <Otto.h> //library built by same individual who modeled the 3d parts
Otto Otto;  

  #define PIN_YL 2 //servo[2]
  #define PIN_YR 3 //servo[3]
  #define PIN_RL 4 //servo[4]
  #define PIN_RR 5 //servo[5]

Servo arm; //creates servo arm object

​

void setup(){

  Otto.init(PIN_YL,PIN_YR,PIN_RL,PIN_RR,true); //attach the servo pins to the library code
  Otto.home();

  Serial.begin(9600); // Starting Serial Terminal

  arm.attach(8); //attach the servo arm motor to pin 8

  delay(50);
}

void loop() {

//this block of code gets the values from the ultrasonic sensor, and converts them to inches (it also prints these values to the Serial monitor for convienence)
  long duration, inches, cm;

   pinMode(pingPin, OUTPUT);
   digitalWrite(pingPin, LOW);
   delayMicroseconds(2);
   digitalWrite(pingPin, HIGH);
   delayMicroseconds(10);
   digitalWrite(pingPin, LOW);
   pinMode(echoPin, INPUT);
   duration = pulseIn(echoPin, HIGH);
   inches = microsecondsToInches(duration);
   Serial.print(inches);
   Serial.print("in, ");
   Serial.println();
   delay(100);

//if the robot senses an object less than 7 inches away, the bot will swings its arm, and turn (thus moving away from object)
   if (inches < 7){
    MoveArmFoward(arm, 8);
    Otto.turn(6, 600, -1);
    
    delay(1000);
   
    }
//otherwise, it will contine forward
    else if (inches > 7){
     walkNorth();
     
    }
    Otto.home();
}

//functions

//walks 2 steps forward when called
void walkNorth()
{
  Otto.walk(2, 600, 1);
}

//walks 2 steps backwards when called
void WalkSouth()
{
   Otto.walk(2, 600, -1); 
}

//uses a loop to continusley change the servo motors position, thus swinging the arm attached to the motor
void MoveArmFoward(Servo arm, int pin)
{
  for(angle = 0; angle < 180; angle += 1)    // command to move from 0 degrees to 180 degrees 
  {                                  
    arm.write(angle);                 //command to rotate the servo to the specified angle
    delay(15);                       
  }  
}

//conversion tool used to get inches for ultrasonic values
long microsecondsToInches(long microseconds) {
   return microseconds / 74 / 2;
}