diff --git a/Inc/stepper.h b/Inc/stepper.h
index 73cfb16..2931c85 100644
--- a/Inc/stepper.h
+++ b/Inc/stepper.h
@@ -12,11 +12,13 @@ typedef struct {
     uint8_t homed:1;
     uint8_t trigger_step:1;
     int16_t pos;
-    int16_t speed;
-    int16_t ramp_to_speed;
+    int32_t speed;
+    int32_t ramp_to_speed;
     uint32_t next_step;
 } stepper_T;
-  
+
+#define STEPPER_SPEED_LIMIT 300
+
 void Timer1_Init(void);
 void stepper_gpio_init();
 void do_steps();
diff --git a/Makefile b/Makefile
index 4270395..a487317 100644
--- a/Makefile
+++ b/Makefile
@@ -22,7 +22,7 @@ TARGET = Parafraktor
 # debug build?
 DEBUG = 1
 # optimization
-OPT = -Og
+OPT = -O0
 
 
 #######################################
diff --git a/README.md b/README.md
index 6df032a..1ca4b74 100644
--- a/README.md
+++ b/README.md
@@ -12,7 +12,7 @@
 |         |              | 006 - 126 | Forward Rotation fast -> slow  |
 |         |              | 127 - 128 | Stop                           |
 |         |              | 129 - 255 | Backward Rotation slow -> fast |
-| 31      | Auto Mode    | 000 - 004 | No function                    |
+| 36      | Auto Mode    | 000 - 004 | No function                    |
 |         |              | 005 - 010 | Go to Reference                |
 |         |              | 015 - 020 | Classic Mode                   |
 
@@ -22,4 +22,4 @@
 - BIGTREETECH Manta M8P
     - https://github.com/bigtreetech/Manta-M8P/tree/master/V1.0_V1.1/Hardware
     - https://github.com/bigtreetech/Manta-M8P
-    - https://bttwiki.com/M8P.html#product-profile
\ No newline at end of file
+    - https://bttwiki.com/M8P.html#product-profile
diff --git a/Src/stepper.c b/Src/stepper.c
index c7dcbf7..0e2fa82 100644
--- a/Src/stepper.c
+++ b/Src/stepper.c
@@ -6,6 +6,7 @@
 
 #include "SEGGER_SYSVIEW.h"
 #include "uart_dmx.h"
+#include "mapping_table.h"
 
 stepper_T steppers[] = {
     {.en_port=GPIOC, .en_pin=11, .dir_port=GPIOB, .dir_pin=4, .step_port=GPIOE, .step_pin=2, .home_port=GPIOF, .home_pin=3},
@@ -43,6 +44,9 @@ void stepper_gpio_init(){
     for(uint8_t i=0; i<(sizeof(steppers)/sizeof(stepper_T)); i++){
         set_pins(&steppers[i]);
     }
+
+    steppers[0].homed = 1;
+    steppers[1].homed = 1;
 }
 
 void Timer1_Init(void) {
@@ -89,10 +93,10 @@ void do_steps(){
                steppers[i].speed--;
 
             /* limit max speed */
-            if(steppers[i].speed >= 100)
-               steppers[i].speed=100;
-            if(steppers[i].speed <= -100)
-               steppers[i].speed=-100;
+            if(steppers[i].speed >= STEPPER_SPEED_LIMIT)
+               steppers[i].speed=STEPPER_SPEED_LIMIT;
+            if(steppers[i].speed <= -STEPPER_SPEED_LIMIT)
+               steppers[i].speed=-STEPPER_SPEED_LIMIT;
 
             /* get direction from sign */
             if(steppers[i].speed>0)
@@ -128,7 +132,8 @@ void do_steps(){
         if(steppers[i].homed){
             static uint16_t old_ramp_to_speed;
             old_ramp_to_speed = steppers[i].ramp_to_speed;
-            steppers[i].ramp_to_speed = rxBuffer[i+1] - steppers[i].pos;
+            motor_mapping_T* tmp = &(((fixture_mapping_T*)&rxBuffer[1])->channels[i]);
+            steppers[i].ramp_to_speed = tmp->tilt - steppers[i].pos;
 
             if(old_ramp_to_speed == 0 && steppers[i].ramp_to_speed != 0)
                 steppers[i].trigger_step = 1;
diff --git a/compile_flags.txt b/compile_flags.txt
index 92d6f97..9edb108 100644
--- a/compile_flags.txt
+++ b/compile_flags.txt
@@ -12,7 +12,7 @@
 -IDrivers/SystemView/Config
 -IDrivers/SystemView/SEGGER
 -IDrivers/SystemView/SYSVIEW
--Og
+-O0
 -Wall
 -fdata-sections
 -ffunction-sections