diff --git a/compLib/Encoder.py b/compLib/Encoder.py
index d6ef934..a2490d2 100644
--- a/compLib/Encoder.py
+++ b/compLib/Encoder.py
@@ -9,6 +9,7 @@ MOTOR_COUNT = 4
 
 
 encoder_start_values = [0] * (MOTOR_COUNT + 1)
+encoder_last_raw_value = [0] * (MOTOR_COUNT + 1)
 
 class Encoder(object):
     """Class used to read the encoders
@@ -24,15 +25,25 @@ class Encoder(object):
         """
         if port <= 0 or port > MOTOR_COUNT:
             raise IndexError("Invalid encoder port specified!")
+        
+        global encoder_last_raw_value
+        raw_value = 0
 
         if port == 1:
-            return Spi.read(Register.MOTOR_1_POS_B3, 4)
+            raw_value = Spi.read(Register.MOTOR_1_POS_B3, 4)
         elif port == 2:
-            return Spi.read(Register.MOTOR_2_POS_B3, 4)
+            raw_value = Spi.read(Register.MOTOR_2_POS_B3, 4)
         elif port == 3:
-            return Spi.read(Register.MOTOR_3_POS_B3, 4)
+            raw_value = Spi.read(Register.MOTOR_3_POS_B3, 4)
         elif port == 4:
-            return Spi.read(Register.MOTOR_4_POS_B3, 4)
+            raw_value = Spi.read(Register.MOTOR_4_POS_B3, 4)
+        
+        if abs(raw_value - encoder_last_raw_value[port]) > 1000:
+            encoder_last_raw_value[port] = raw_value
+            return Encoder.read_raw(port)
+        
+        encoder_last_raw_value[port] = raw_value
+        return raw_value
         
     @staticmethod
     def read(port: int) -> int:
@@ -48,6 +59,8 @@ class Encoder(object):
         diff = Encoder.read_raw(port) - encoder_start_values[port]
         if diff > 2 ** 31:
             diff -= 2 ** 32
+        elif diff < -2 ** 31:
+            diff += 2 ** 32
         
         MetricsLogging.put("Encoder", diff, port)
         return diff
diff --git a/compLib/Motor.py b/compLib/Motor.py
index 272e3ea..8d46f58 100644
--- a/compLib/Motor.py
+++ b/compLib/Motor.py
@@ -8,7 +8,7 @@ from compLib.Spi import Spi, Register
 MOTOR_COUNT = 4
 MAX_MOTOR_SPEED = 65535
 MOTOR_PERCENTAGE_MULT = MAX_MOTOR_SPEED / 100.0
-MOTOR_CURVE = [0.0, 0.0, 426.5, 692.0, 842.5, 953.5, 1032.5, 1090.5, 1135.5, 1171.0, 1203.5, 1230.0, 1249.5, 1268.0, 1283.0, 1298.5, 1308.0, 1320.0, 1332.0, 1339.5, 1352.5]
+MOTOR_CURVE = [0.0, 0.0, 0.5, 60.0, 199.83333333333334, 377.66666666666663, 990.3333333333333, 1860.6666666666665, 2587.0, 3091.6666666666665, 3489.0, 3860.5, 4197.333333333333, 4432.166666666667, 4647.166666666666, 4873.166666666666, 5054.333333333334, 5208.666666666667, 5353.0, 5466.5, 5604.0]
 
 SPEED_LOCK = False
 SPEED_MULT = 1.0
@@ -33,7 +33,7 @@ class Motor(object):
         :param mode: Motor mode. See enum MotorMode for more info
         :raises: IndexError
         """
-
+        
         if SPEED_LOCK:
             return
 
@@ -92,11 +92,9 @@ class Motor(object):
         :param percent: Percentage of max speed. between -100 and 100
         :raises: IndexError
         """
-        raw_power = 0
-        if percent > 0:
-            raw_power = Motor.__linearizePower(MOTOR_CURVE, percent)
-        elif percent < 0:
-            raw_power =  -Motor.__linearizePower(MOTOR_CURVE, -percent)
+        raw_power = raw_power = Motor.__linearizePower(MOTOR_CURVE, abs(percent))
+        if percent < 0:
+            raw_power *= -1
 
         MetricsLogging.put("Motor", float(percent), port)
         Motor.power_raw(port, raw_power, False)
@@ -135,7 +133,8 @@ class Motor(object):
         """
         if (len(curve) != 21):
             raise ValueError('The motor curve is invalid, check documentation for set_motor_curve()!')
-
+        
+        global MOTOR_CURVE
         MOTOR_CURVE = curve
 
     @staticmethod
@@ -152,6 +151,12 @@ class Motor(object):
         """
         Linear interpolation. Check https://www.arduino.cc/reference/en/language/functions/math/map/
         """
+        x = float(x)
+        in_min = float(in_min)
+        in_max = float(in_max)
+        out_min = float(out_min)
+        out_max = float(out_max)
+
         return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
 
     @staticmethod
@@ -160,10 +165,10 @@ class Motor(object):
         Interpolate a speed in the specified motor curve and return 
         the 'power percentage that is needed to reach that speed'
         """
-        if speed < curve[0] or speed > curve[20]:
+        if speed < min(curve) or speed > max(curve):
             raise ValueError(f'Speed out of range: {str(speed)} ticks/s')
 
-        for index in range(20):  # There are 20 speed ranges
+        for index in range(len(curve) - 2):
             if speed >= curve[index] and speed <= curve[index + 1] and curve[index] != curve[index + 1]:
                 return Motor.__map(speed, curve[index], curve[index + 1], index * 5, index * 5 + 5)
 
@@ -177,7 +182,7 @@ class Motor(object):
         if power < 0 or power > 100:
             raise ValueError(f'Power out of range: {str(power)}%')
 
-        requiredSpeed = Motor.__map(power, 0, 100, curve[0], curve[20])
+        requiredSpeed = Motor.__map(power, 0, 100, min(curve), max(curve))
         return Motor.__interpolateSpeed(curve, requiredSpeed)