diff --git a/Makefile b/Makefile
index 7a5b2b5..8c3b612 100644
--- a/Makefile
+++ b/Makefile
@@ -42,7 +42,7 @@ CFLAGS+=-I$(OPENCM3_DIR)/include
 LDFLAGS+=-L$(OPENCM3_DIR)/lib -lopencm3_stm32f0
 
 # Flags for fxplib
-CFLAGS+=-Ifxplib/include -DPOINTPOS=14
+CFLAGS+=-Ifxplib/include -DPOINTPOS=16
 LDFLAGS+=-Lfxplib/lib/$(BUILD) -lfxp_stm32f0
 
 # generic linking
diff --git a/src/main.c b/src/main.c
index 149488a..ddcf52f 100644
--- a/src/main.c
+++ b/src/main.c
@@ -528,16 +528,16 @@ int main(void)
 	uint32_t sleep_time = 10;
 	uint64_t force_display_update_time = 1000;
 
-	fxp_t PGAIN_CV = fxp_from_float( 50.000f);
-	fxp_t IGAIN_CV = fxp_from_float(  0.300f);
-	fxp_t IERR_LIMIT = fxp_from_int(4000);
+	fxp_t PGAIN_CV = fxp_from_float( 1000.000f);
+	fxp_t IGAIN_CV = fxp_from_float(    0.500f);
+	fxp_t IERR_LIMIT = fxp_from_int(5000);
 
 	fxp_t PGAIN_CC = fxp_from_float( 50.000f);
 	fxp_t IGAIN_CC = fxp_from_float(  0.300f);
 
 	fxp_t CURRENT_THRESHOLD = fxp_from_float(0.001f);
 
-	fxp_t AVG_FACT = fxp_from_float(0.10f);
+	fxp_t AVG_FACT = fxp_from_float(0.05f);
 	fxp_t AVG_FACT_INV = fxp_sub(fxp_from_int(1), AVG_FACT);
 
 	fxp_t MAX_VOLTAGE = fxp_from_float(14.400f);
@@ -565,9 +565,9 @@ int main(void)
 	//fxp_t CURRENT_SCALE = fxp_from_float(9.7f / 4095.0f);
 
 	// Calibrated from measurements
-	fxp_t VIN_SCALE = fxp_from_float(12.11f / 1600.0f);
-	fxp_t VOUT_SCALE = fxp_from_float(12.6f / 1620.0f);
-	fxp_t CURRENT_SCALE = fxp_from_float(9.01f / 4095.0f);
+	fxp_t VIN_SCALE = fxp_from_float(12.23f / 1600.0f);
+	fxp_t VOUT_SCALE = fxp_from_float(12.76f / 1620.0f);
+	fxp_t CURRENT_SCALE = fxp_from_float(8.95f / 4095.0f);
 
 	fxp_t CURRENT_OFFSET = fxp_from_float(0.045);
 
@@ -685,7 +685,7 @@ int main(void)
 					timer_set_oc_value(TIM1, TIM_CH_BOOTSTRAP, 24);
 
 					if(time_in_state >= 5) { // bootstrap duration in ms
-						iErr = fxp_div(IERR_LIMIT, fxp_from_int(2));
+						//iErr = 0;
 						operState = nextState;
 					}
 					break;
@@ -717,6 +717,21 @@ int main(void)
 						timer_set_oc_value(TIM1, TIM_CH_CONV, 0);
 					}
 
+#ifdef DEBUG
+					if((time_in_state % 100) == 0) {
+						debug_send_string("pErr: ");
+						fxp_format(pErr, msg, 3);
+						debug_send_string(msg);
+						debug_send_string(" iErr: ");
+						fxp_format(iErr, msg, 3);
+						debug_send_string(msg);
+						debug_send_string(" controlAction: ");
+						fxp_format(controlAction, msg, 3);
+						debug_send_string(msg);
+						sentSomething = 1;
+					}
+#endif
+
 					if(time_in_state > 5000 && pwm > CONV_PWM_MAX && power_state.current_avg < CURRENT_THRESHOLD) {
 						operState = Bootstrap;
 						nextState = ConvConstVoltage;