diff --git a/Core/Src/PWM_control.c b/Core/Src/PWM_control.c
index d0be4df..ecd54d2 100644
--- a/Core/Src/PWM_control.c
+++ b/Core/Src/PWM_control.c
@@ -61,5 +61,10 @@ void PWM_powerground_control(uint8_t percent){
   __HAL_TIM_SET_COMPARE(powerground, TIM_CHANNEL_4, ccr);
 }
 
+void PWM_powerground_softcontrol(){
+  int ccr = 2000 + (20 * current_powerground_status);
+  __HAL_TIM_SET_COMPARE(powerground, TIM_CHANNEL_3, ccr);
+  __HAL_TIM_SET_COMPARE(powerground, TIM_CHANNEL_4, ccr);
+}
 void PWM_battery_cooling_control(uint8_t percent){}
 void PWM_esc_cooling(uint8_t percent){}
diff --git a/Core/Src/state_machine.c b/Core/Src/state_machine.c
index b36c840..539a3d2 100644
--- a/Core/Src/state_machine.c
+++ b/Core/Src/state_machine.c
@@ -5,6 +5,7 @@
  *      Author: Hamza
  */
  
+#include "state_machine.h"
 #include "PWM_control.h"
 
 // Time to wait after reaching 95% of battery voltage before exiting precharge
@@ -56,7 +57,7 @@ void sm_update(){
   //if (CAN_timer < HAL_GetTick())
   //  state.current_state = state.target_state = STATE_ERROR;
 
-  if (state.current_state == STATE_INACTIVE){
+  if (state.current_state == STATE_INACTIVE || state.current_state == STATE_ERROR){
     CURRENT_MEASUREMENT = (module.auxVoltages[0] > 2494) ? (module.auxVoltages[0] - (2494.0)) * (300.0) : 0;
   } else {
     CURRENT_MEASUREMENT = (module.auxVoltages[0] > 2477) ? (module.auxVoltages[0] - (2477.0)) * (4600.0) : 0;
@@ -117,12 +118,12 @@ void sm_handle_ams_in(const uint8_t *data){
       break;
     case 0x02:
       if (state.current_state == STATE_READY || state.current_state == STATE_ACTIVE){
+        target_powerground_status = data[1];
         state.target_state = STATE_ACTIVE;        // READY -> ACTIVE
       }
       break;
     case 0xF0:
       if (state.current_state == STATE_INACTIVE){
-        target_powerground_status = data[1];
         state.target_state = STATE_CHARGING_PRECHARGE;
       }
       break;
@@ -162,7 +163,9 @@ void sm_precharge_discharge_manager(){
 }
 
 void sm_powerground_manager(){
-  if (target_powerground_status > 100){ //something went wrong
+  if (current_powerground_status == target_powerground_status)
+    return;
+  if ( current_powerground_status > 100 || target_powerground_status > 100){ //something went wrong
     PWM_powerground_control(255);
     current_powerground_status = target_powerground_status= 0;
     return;
@@ -170,10 +173,12 @@ void sm_powerground_manager(){
 
   if (powerground_softstart_timer < HAL_GetTick()){
     if (current_powerground_status < target_powerground_status ){
-      PWM_powerground_control(current_powerground_status + 1);
+      current_powerground_status++;
+      PWM_powerground_softcontrol();
       powerground_softstart_timer = HAL_GetTick() + 10;
     } else if (current_powerground_status > target_powerground_status) {
-      PWM_powerground_control(current_powerground_status - 1);
+      current_powerground_status--;
+      PWM_powerground_softcontrol();
       powerground_softstart_timer = HAL_GetTick() + 10;
     }
   }