             if(rangeRight > rangeLeft && rangeRight > rangeCenter )
             {
                 // drive straight
-                SetMotor(motorPin1_1, motorPin1_2, motorSpeed);
-                SetMotor(motorPin2_1, motorPin2_2, motorSpeed/2);
+                SetMotor(1, motorSpeed);
+                SetMotor(2, motorSpeed/2);
             }
 
             if(rangeLeft > rangeRight && rangeLeft > rangeCenter )
             {
                 // drive straight
-                SetMotor(motorPin1_1, motorPin1_2, motorSpeed/2);
-                SetMotor(motorPin2_1, motorPin2_2, motorSpeed);
+                SetMotor(1, motorSpeed/2);
+                SetMotor(2, motorSpeed);
             }
         }
 
         // drive straight
-        SetMotor(motorPin1_1, motorPin1_2, motorSpeed);
-        SetMotor(motorPin2_1, motorPin2_2, motorSpeed);
+        SetMotor(1, motorSpeed);
+        SetMotor(2, motorSpeed);
     }
     else
     {


        if(servoRangeDirection == LEFT)
        {
            SetMotor(motorPin1_1, motorPin1_2, -motorSpeed);
            SetMotor(motorPin2_1, motorPin2_2, motorSpeed);
        }
        else
        {
            SetMotor(motorPin1_1, motorPin1_2, motorSpeed);
            SetMotor(motorPin2_1, motorPin2_2, -motorSpeed);
        }

        wheelSpinCompleted = false;

{
    ColorSet(LEDStrip.Color(255, 255, 255));

    SetMotor(1, 0);
    SetMotor(2, 0);
}

void SetLightErrorState()
{
    ColorSet(LEDStrip.Color(0, 0, 255));
}

void SetServo(uint32_t uptime, uint8_t servoPos)
