 void moveMotorOnSide(Direction side, Direction orientation, int speed)
 {
 	speed = max(min(speed, 180), 0);
-
-	if (side == right)
-	{
-		if (orientation == forward)
-		{
-			digitalWrite(12, HIGH);
-			analogWrite(3, speed);
-		}
-		else
-		{
-			digitalWrite(12, LOW);
-			analogWrite(3, speed);
-		}
-
-	}
-	else
-	{
-		if (orientation == forward)
-		{
-			digitalWrite(13, HIGH);
-			analogWrite(11, speed);
-		}
-		else
-		{
-			digitalWrite(13, LOW);
-			analogWrite(11, speed);
-		}
-
-	}
-
+	
+	digitalWrite(side == left ? 13 : 12, orientation == forward ? HIGH : LOW);
+	analogWrite(side == left ? 3 : 11, speed);
 }
 
 void moveBothMotors(int speedLeft, int speedRight)


	Serial.println((int)position - 2500);
}

#pragma region "OldCode"

//// Turns to the sent variable of
//// 'L' (left), 'R' (right), 'S' (straight), or 'B' (back)
//// TODO Tune 'turnSpeed'
//void turn(char dir)
//{
//	switch (dir)
//	{
//		// Turn left 90deg
//	case 'L':
//		moveBothMotors(forward, back, turnSpeed);
//
//
//		position = qtrra.readLine(sensorValues);
//
//
//		while (sensorValues[5] < treshold)  // wait for outer most sensor to find the line
//		{
//			position = qtrra.readLine(sensorValues);
//		}
//
//		// slow down speed
//		moveBothMotors(forward, back, turnSpeedSlow);
//
//		// find center
//		while (position > 3000)  // tune - wait for line position to find near center
//		{
//			position = qtrra.readLine(sensorValues);
//		}
//
//		// stop both motors
//		moveBothMotors(forward, back, 0);
//		break;
//
//		// Turn right 90deg
//	case 'R':
//		moveBothMotors(back, forward, turnSpeed);
//
//		position = qtrra.readLine(sensorValues);
//
//		while (sensorValues[1] < treshold)  // wait for outer most sensor to find the line
//		{
//			position = qtrra.readLine(sensorValues);
//		}
//
//		// slow down speed
//		moveBothMotors(back, forward, turnSpeedSlow);
//
//		// find center
//		while (position < 3000)  // tune - wait for line position to find near center
//		{
//			position = qtrra.readLine(sensorValues);
//		}
//
//		// stop both motors
//		moveBothMotors(back, forward, 0);
//		break;
//
//		// Turn right 180deg to go back
//	case 'B':
//		moveBothMotors(back, forward, turnSpeed);
//
//		position = qtrra.readLine(sensorValues);
//
//		while (sensorValues[1] < treshold)  // wait for outer most sensor to find the line
//		{
//			position = qtrra.readLine(sensorValues);
//		}
//
//		// slow down speed
//		moveBothMotors(back, forward, turnSpeedSlow);
//
//		// find center
//		while (position < 300)  // tune - wait for line position to find near center
//		{
//			position = qtrra.readLine(sensorValues);
//		}
//
//		// stop both motors
//		moveBothMotors(back, forward, 0);
//		break;
//
//		// Straight ahead
//	case 'S':
//		// do nothing
//		break;
//	}
//} // end turn

//void MazeSolve()
//{
//	while (1)
//	{
//
//
//
//		// Drive straight a bit.
//		moveBothMotors(forward, forward, 200);
//		delay(25);
//
//		// These variables record whether the robot has seen a line to the
//		// left, straight ahead, and right, while examining the current
//		// intersection.
//		unsigned char found_left = 0; //TODO make this boolean
//		unsigned char found_straight = 0;
//		unsigned char found_right = 0;
//
//		// Now read the sensors and check the intersection type.
//		position = qtrra.readLine(sensorValues);
//
//		// Check for left and right exits.
//		if (sensorValues[0] > 400)
//			found_right = 1;
//		if (sensorValues[5] > 400)
//			found_left = 1;
//
//		// Drive straight a bit more 
//		moveBothMotors(forward, forward, 200);
//		delay(drivePastDelay);
//
//		position = qtrra.readLine(sensorValues);
//		// if(sensorValues[1] > 200 || sensorValues[2] > 200 || sensorValues[3] > 200 || sensorValues[4] > 200) //To Do: Which sensor values indicate a straight
//		// found_straight = 1;
//
//		// unsigned char dir = select_turn(found_left, found_straight, found_right); //TODO this method
//
//		 // Make the turn indicated by the path.
//		turn(dir);
//		//TODO End of the maze
//	}

//char selectTurn(unsigned char found_left, unsigned char found_straight, unsigned char found_right) //TODO make this boolean
//{
//	// Make a decision about how to turn.  The following code
//	// implements a left-hand-on-the-wall strategy, where we always
//	// turn as far to the left as possible.
//	if (found_left)
//		return 'L';
//	else if (found_straight)
//		return 'S';
//	else if (found_right)
//		return 'R';
//	else
//		return 'B';
//} // end select_turn

#pragma endregion

const float derivateConst = 1.0f;

const int standardMotorSpeed = 180;

int drivePastDelay = 300; // tune value in mseconds motors will run past intersection to align wheels for turn NOT TESTED

