 
 	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;

#pragma region "OldCode"

// TODO nicht ntig, weil nur einmal verwendet und der Speicherbedarf klein bleiben soll
//const int speedA = 3;  //speed control for motor outputs 1 and 2 is on digital pin 10  (Right motor)
//const int speedB = 11; //speed control for motor outputs 3 and 4 is on digital pin 11  (Left motor)
//const int dirA = 12;  //direction control for motor outputs 1 and 2 is on digital pin 12  (Right motor)
//const int dirB = 13;  //direction control for motor outputs 3 and 4 is on digital pin 13  (Left motor)

// TODO nie benutzt
//int turnSpeed = 200;  // tune value motors will run while turning (0-255) NOT TESTED
//int turnSpeedSlow = 125;  // tune value motors will run as they slow down from turning cycle to avoid overrun (0-255) NOT TESTED

#pragma endregion

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


void moveMotorOnSide(Direction side, Direction orientation, int speed)
{
	speed = max(min(speed, 180), 0);
	
	digitalWrite(side == left ? 13 : 12, orientation == forward ? HIGH : LOW);
	analogWrite(side == left ? 3 : 11, speed);
}

void moveBothMotors(int speedLeft, int speedRight)
