diff --git a/Inc/stepper.h b/Inc/stepper.h
index c65683b..364cc59 100644
--- a/Inc/stepper.h
+++ b/Inc/stepper.h
@@ -17,7 +17,9 @@ typedef struct {
     uint32_t next_step;
 } stepper_T;
   
-#define STEPPERS_MAX_SPEED 300
+#define STEPPER_MAX_SPEED 600
+
+#define STEPPER_STEPS_PER_ROTATION 3200
 
 void Timer1_Init(void);
 void stepper_gpio_init();
diff --git a/Makefile b/Makefile
index a487317..4270395 100644
--- a/Makefile
+++ b/Makefile
@@ -22,7 +22,7 @@ TARGET = Parafraktor
 # debug build?
 DEBUG = 1
 # optimization
-OPT = -O0
+OPT = -Og
 
 
 #######################################
diff --git a/Src/stepper.c b/Src/stepper.c
index 637dd4d..b5eedc2 100644
--- a/Src/stepper.c
+++ b/Src/stepper.c
@@ -89,10 +89,10 @@ void do_steps(){
                steppers[i].speed--;
 
             /* limit max speed */
-            if(steppers[i].speed >= STEPPERS_MAX_SPEED)
-               steppers[i].speed=STEPPERS_MAX_SPEED;
-            if(steppers[i].speed <= -STEPPERS_MAX_SPEED)
-               steppers[i].speed=-STEPPERS_MAX_SPEED;
+            if(steppers[i].speed >= STEPPER_MAX_SPEED)
+               steppers[i].speed=STEPPER_MAX_SPEED;
+            if(steppers[i].speed <= -STEPPER_MAX_SPEED)
+               steppers[i].speed=-STEPPER_MAX_SPEED;
 
             /* get direction from sign */
             if(steppers[i].speed>0)
@@ -122,7 +122,7 @@ void do_steps(){
                 steppers[i].next_step = timer + 1;
             }
 
-            printf("motor[%d]: pos %ld\tspeed %d\n", i, steppers[i].pos, steppers[i].speed);
+            printf("motor[%d]: pos %ld\tspeed %ld\n", i, steppers[i].pos, steppers[i].speed);
         }
 
         if(steppers[i].homed){
@@ -130,7 +130,8 @@ void do_steps(){
             old_ramp_to_speed = steppers[i].ramp_to_speed;
 
             int32_t swapped_val = rxBuffer[2*i+1]<<8 | (rxBuffer[2*i+2]&0xFF);
-            steppers[i].ramp_to_speed = swapped_val - steppers[i].pos;
+            int32_t scaled_steps = swapped_val * STEPPER_STEPS_PER_ROTATION / UINT16_MAX;
+            steppers[i].ramp_to_speed = scaled_steps - 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 9edb108..92d6f97 100644
--- a/compile_flags.txt
+++ b/compile_flags.txt
@@ -12,7 +12,7 @@
 -IDrivers/SystemView/Config
 -IDrivers/SystemView/SEGGER
 -IDrivers/SystemView/SYSVIEW
--O0
+-Og
 -Wall
 -fdata-sections
 -ffunction-sections