 
 /*---------------------------------------------------------------------------------------*/
 //circleRoutine
-void circleRoutine(unsigned long seconds, unsigned long &results) {
-  results = HIGH;
+void circleRoutine(unsigned long seconds, unsigned long &rcAuto) {
+  rcAuto = HIGH;
   
   steer(LEFT_TURN_OUT);
   delay(1000);
   seconds = seconds * 1000;
   unsigned long loopTime = millis();
   while (millis() < (loopTime + seconds)) {
-    moveVehicle(128);
-  }
-  results = LOW;
+    moveVehicle(112);
+  }
+  rcAuto = LOW;
+}
+/*---------------------------------------------------------------------------------------*/
+//squareRoutine
+void squareRoutine(unsigned long sides, unsigned long &rcAuto) {
+  Serial.println("Starting square routine...");
+  rcAuto = HIGH;
+  long straightSpeed = 2750;        //mmPs
+  long turnSpeed = 1800;            //mmPs
+  sides = sides * 1000;             //convert side length to mm
+  unsigned long sideSec = sides / straightSpeed;  //calculate seconds per side at set speed
+  sideSec = sideSec * 1000;         //convert to ms
+  unsigned long loopTime;           //start time of while loops for throttle
+  float turnDist = TURN_RADIUS_CM * PI / 2 * 10; //turnDist == 1/4 turn circumference in mm
+  unsigned long turnSec = (long)turnDist / turnSpeed * 1000; //turnSec == time for 90-degree turn at 1250 mmPs
+  for(int i = 0; i < 4; i++){
+    //steer(LEFT_TURN_OUT);
+    steer(STRAIGHT_TURN_OUT);
+    brake(MAX_BRAKE_OUT);
+    delay(1000);
+    brake(MIN_BRAKE_OUT);
+    
+    loopTime = millis();
+    while (millis() < (loopTime + sideSec)) {
+      moveVehicle(112);
+    }
+    
+    steer(LEFT_TURN_OUT);
+    brake(MAX_BRAKE_OUT);
+    delay(1000);
+
+    brake(MIN_BRAKE_OUT);
+    
+
+    loopTime = millis();
+    while (millis() < (loopTime + turnSec)) {
+      moveVehicle(96);
+    }
+  }
+  brake(MAX_BRAKE_OUT);
+  delay(1000);
+  brake(MIN_BRAKE_OUT);
+  rcAuto = LOW;
 }
 /*---------------------------------------------------------------------------------------*/
 void startCapturingRCState(){

    int going = convertThrottle(results[RC_RDR]);
    moveVehicle(going);
  }
  else if(doCircleRoutine(results[RC_RDR])){
    moveVehicle(MIN_ACC_OUT);
    circleRoutine(5, results[RC_AUTO]);
  }

/*---------------------------------------------------------------------------------------*/
void processHighLevel(SerialData * results)
{
  //results->update();
  //Steer
  int turn_signal = convertDeg(results->angle_deg);
  steer(turn_signal);
  //End Steer
  //Throttle
  Throttle_PID(10*results->speed_cmPs);
  //End Throttle
  writeSerial(&Serial3, &results);
}
