 	} else if (frontTriggered && !rightTriggered & !leftTriggered){
 		hDrive(-swagSpeed, 0.0, 0.0);
 		delay(50);
-		turnToAngle(45, swagSpeed);
+		turnToAngle(90, -swagSpeed);
+		rightTurns++;
 	} else if (backTriggered) {
-		hDrive(-swagSpeed, swagSpeed, 0.0);
+		hDrive(swagSpeed, swagSpeed, 0.0);
 	} else {
-		hDrive(swagSpeed, -swagSpeed , 0.0);
-	}
+		hDrive(swagSpeed, 0.0, 0.0);
+	}
+	
+	if (direction) {
+		strafeMotor.set(0.3);
+	} else {
+		strafeMotor.set(-0.3);
+	}
+	
+	direction = !direction;
 	
 	delay(50);
+}
+
+void tankDrive(double leftSpeed, double rightSpeed, double strafeSpeed) {
+	leftMotor1.set(constrain(leftSpeed, -1.0, 1.0));
+	leftMotor2.set(constrain(leftSpeed, -1.0, 1.0));
+	rightMotor1.set(-constrain(rightSpeed, -1.0, 1.0));
+	rightMotor2.set(-constrain(rightSpeed, -1.0, 1.0));
+	strafeMotor.set(strafeSpeed);
+}
+
+void followWall(double targetDistance) {
+	if (rightUltrasonic.getDistance() < targetDistance) {
+		tankDrive(0.3, 0.6, 0.0);
+	} else if (rightUltrasonic.getDistance() > targetDistance) {
+		tankDrive(0.6, 0.3, 0.0);
+	} else {
+		tankDrive(0.6, 0.6, 0.0);
+	}
 }

void driveToWhite();
void driveThroughRoom();
void hDrive(double move, double rotate, double strafe);
void definedStartSearch();
double getAngle(double targetAngle);
double getPercentError(double actual, double target);

  // put your main code here, to run repeatedly:
  if (FreqCount.available()) {
    count = FreqCount.read();
	Serial.println(count);
  } else {
    count = 0;
  }
