diff --git a/firmware/jerk_control/src/executor.rs b/firmware/jerk_control/src/executor.rs
index 3be023b..bdd06cf 100644
--- a/firmware/jerk_control/src/executor.rs
+++ b/firmware/jerk_control/src/executor.rs
@@ -37,9 +37,16 @@ impl CommandQueue {
     // Note that step_late needs to be called as well to advance the segment.
     pub fn step_early(&mut self) -> bool {
         
-        let cur_segment = &mut self.segments[self.cur_segment as usize];
+        let cur_segment = unsafe { self.segments.get_unchecked_mut(self.cur_segment as usize) };
 
-        self.p.0 = (self.p + cur_segment.delta[0]).0.rem_euclid(self.step_size);
+
+
+        self.p.0 = (self.p + cur_segment.delta[0]).0;
+        if self.p.0 > self.step_size {
+            self.p.0 -= self.step_size;
+        } else if self.p.0 < 0 {
+            self.p.0 += self.step_size;
+        }
 
         cur_segment.delta[0] += cur_segment.delta[1];
         cur_segment.delta[1] += cur_segment.delta[2];