 }
 
 void turnToAngle(double targetAngle, double speed) {
-	resetEncoders();
+  resetEncoders();
+  
+  hDrive(0.0, speed, 0.0);
+  
   while (abs(getAngle()) < abs(targetAngle)) {
-    hDrive(0.0, speed, 0.0);
-  }
+    Serial.println(getAngle());
+  }
+  
   hDrive(0.0, 0.0, 0.0);
 }
 

		hDrive(-swagSpeed, 0.0, 0.0);
		delay(50);
		// hDrive(0.0, swagSpeed , 0.0);
		turnToAngle(90, swagSpeed);
	} else if (rightTriggered && frontTriggered) {
		hDrive(-swagSpeed, 0.0, 0.0);
		delay(50);
		// hDrive(0.0, -swagSpeed , 0.0);
		turnToAngle(-90, -swagSpeed);
	} else if (leftTriggered && !frontTriggered) {
		hDrive(swagSpeed - 0.2, swagSpeed, 0.0);
	} else if (rightTriggered && !frontTriggered) {
		hDrive(swagSpeed - 0.2, -swagSpeed , 0.0);
	} else if (frontTriggered && !rightTriggered & !leftTriggered){
		hDrive(-swagSpeed, 0.0, 0.0);
		delay(50);
		turnToAngle(45, swagSpeed);
	} else if (backTriggered) {
		hDrive(-swagSpeed, -swagSpeed, 0.0);
	} else {
		hDrive(swagSpeed, swagSpeed , 0.0);
	}
	
	delay(50);

}

double getAngle() {
  return (180 / PI) * (getDistanceLeft() - getDistanceRight()) / 11.5;
}

double getDistanceLeft() {
