diff --git a/extras/D11-Firmware/D11-Firmware.ino b/extras/D11-Firmware/D11-Firmware.ino
index 20aad0e..ecb4cd1 100644
--- a/extras/D11-Firmware/D11-Firmware.ino
+++ b/extras/D11-Firmware/D11-Firmware.ino
@@ -64,7 +64,8 @@ void setup() {
   Wire.onReceive(receiveEvent);
   pinMode(LED_BUILTIN, OUTPUT);
   pinMode(IRQ_PIN, OUTPUT);
-  analogWriteResolution(8);
+  //analogWriteResolution(8);
+  analogWriteResolution(24); //new servo resolution
 }
 
 volatile uint8_t command = 0;
diff --git a/extras/D11-Firmware/DCMotor.cpp b/extras/D11-Firmware/DCMotor.cpp
index 0aa6d50..969b570 100644
--- a/extras/D11-Firmware/DCMotor.cpp
+++ b/extras/D11-Firmware/DCMotor.cpp
@@ -9,7 +9,8 @@ void DCMotor::setDuty(int duty) {
   }
 
   // scale duty to period
-  duty = duty * 255 / 100;
+  //duty = duty * 255 / 100;
+  duty = duty * 16777215/ 100; //due to new servo resolution
 
   if (duty > 0) {
     analogWrite(in1, 0);
diff --git a/extras/D11-Firmware/PID.cpp b/extras/D11-Firmware/PID.cpp
index bc5d37a..f6ef0d8 100644
--- a/extras/D11-Firmware/PID.cpp
+++ b/extras/D11-Firmware/PID.cpp
@@ -18,13 +18,13 @@ void calculatePID_wrapper(void* arg) {
         //save curvelocmd for next iteration
         prevvelocmd[i] = curvelocmd;
         int dutyout = (int16_t)curvelocmd ;
-        obj[i]->motor->setDuty(dutyout); //dutyout should be a value between -100 and 100.
+        obj[i]->motor->setDuty(-dutyout); //dutyout should be a value between -100 and 100.
       }
     } else {
       //CL_VELOCITY
       if (obj[i]->pid_velo->Compute()) {
         int dutyout = int32_t(obj[i]->actualDuty);
-        obj[i]->motor->setDuty(dutyout);
+        obj[i]->motor->setDuty(-dutyout);
       }
     }
   }
diff --git a/src/ServoMotor.cpp b/src/ServoMotor.cpp
index e570bb5..67cb391 100644
--- a/src/ServoMotor.cpp
+++ b/src/ServoMotor.cpp
@@ -25,7 +25,8 @@ mc::ServoMotor::ServoMotor() {
 };
 
 void mc::ServoMotor::setAngle(int angle) {
-  setData(SET_PWM_DUTY_CYCLE_SERVO, instance, map(angle,0,180,7,28));
+  //setData(SET_PWM_DUTY_CYCLE_SERVO, instance, map(angle,0,180,7,28));
+  setData(SET_PWM_DUTY_CYCLE_SERVO, instance, map(angle,0,180,600000,1740000));//new servo resolution
 }
 
 void mc::ServoMotor::detach() {