diff --git a/src/main.c b/src/main.c
index 8bc2e6a1..f133df44 100644
--- a/src/main.c
+++ b/src/main.c
@@ -41,7 +41,7 @@ void link_ac_sync_res(){
 	link_hal_pins("pderiv0.out", "pid0.vel_fb");
 	set_hal_pin("pderiv0.lp", 0.8);
 	link_hal_pins("enc0.pos", "pderiv1.in");
-	link_hal_pins("pderiv1.out", "pid0.vel_ext_cmd");
+	link_hal_pins("pderiv1.out", "pid0.vel_ext_c");
 	set_hal_pin("pderiv1.lp", 0.8);
 	link_hal_pins("res0.pos", "plus1.in0");
 	set_hal_pin("plus1.in1", -0.64);
@@ -64,8 +64,48 @@ void link_ac_sync_res(){
 	set_hal_pin("p2uvw0.volt", 130.0);
 	set_hal_pin("p2uvw0.pwm_max", 0.9);
 	set_hal_pin("pid0.volt", 130.0);
+	set_hal_pin("p2uvw0.poles", 1.0);		
+}
+
+void link_ac_sync_enc(){
+	link_hal_pins("enc0.pos", "pos_minus0.in1");
+	link_hal_pins("res0.pos", "pos_minus0.in0");
+	link_hal_pins("pos_minus0.out", "pid0.pos_error");
+	link_hal_pins("pid0.pwm_cmd", "p2uvw0.pwm");
+	link_hal_pins("enc0.pos", "pderiv0.in");
+	link_hal_pins("pderiv0.out", "pid0.vel_fb");
+	set_hal_pin("pderiv0.lp", 0.8);
+	link_hal_pins("res0.pos", "pderiv1.in");
+	link_hal_pins("pderiv1.out", "pid0.vel_ext_c");
+	set_hal_pin("pderiv1.lp", 0.8);
+	link_hal_pins("enc0.pos", "plus1.in0");
+	set_hal_pin("plus1.in1", -0.64);
+	link_hal_pins("plus1.out", "mul0.in0");
+	set_hal_pin("mul0.in1", 3.0);
+	link_hal_pins("mul0.out", "mod0.in");
+	link_hal_pins("mod0.out", "plus0.in0");
+	set_hal_pin("plus0.in1", M_PI / 2.0);
+	link_hal_pins("plus0.out", "p2uvw0.magpos");
+	link_hal_pins("p2uvw0.u", "pwmout0.u");
+	link_hal_pins("p2uvw0.v", "pwmout0.v");
+	link_hal_pins("p2uvw0.w", "pwmout0.w");
+	link_hal_pins("res0.pos", "term0.wave0");
+	link_hal_pins("enc0.pos", "term0.wave1");
+	set_hal_pin("res0.enable", 1.0);
+	set_hal_pin("pid0.enable", 1.0);
+	set_hal_pin("pwmout0.enable", 0.9);
+	set_hal_pin("pwmout0.volt", 130.0);
+	set_hal_pin("pwmout0.pwm_max", 0.9);
+	set_hal_pin("p2uvw0.volt", 130.0);
+	set_hal_pin("p2uvw0.pwm_max", 0.9);
+	set_hal_pin("pid0.volt", 130.0);
+	set_hal_pin("pid0.pos_p", 80.0);
+	set_hal_pin("pid0.pos_lp", 1.0);
+	set_hal_pin("pid0.vel_lp", 0.6);
+	set_hal_pin("pid0.vel_max", 200.0);
+	set_hal_pin("pid0.acc_max", 3000.0);
 	set_hal_pin("p2uvw0.poles", 1.0);
-		
+	set_hal_pin("enc0.res", 4096.0);		
 }
 
 void DMA2_Stream2_IRQHandler(void){
@@ -96,7 +136,7 @@ void ADC_IRQHandler(void) // 20khz
 void TIM5_IRQHandler(void){ //1KHz
 	TIM_ClearITPendingBit(TIM5, TIM_IT_Update);
 	float s = 0,c = 0;
-	int freq = 1000;
+	int freq = 5000;
 	float period = 1.0 / freq;
 
 	for(int i = 0;i<10;i++){
diff --git a/src/pid.comp b/src/pid.comp
index e26bef9a..874f4b43 100644
--- a/src/pid.comp
+++ b/src/pid.comp
@@ -28,14 +28,14 @@ HAL_PIN(enable) = 0.0;
 
 HAL_PIN(pos_en) = 1.0;
 HAL_PIN(pos_p) = 100.0;
-HAL_PIN(pos_lp) = 1.0;
+HAL_PIN(pos_lp) = 0.8;
 
 HAL_PIN(vel_en) = 1.0;
 HAL_PIN(vel_p) = 1.0;
-HAL_PIN(vel_i) = 40.0;
+HAL_PIN(vel_i) = 80.0;
 //HAL_PIN(vel_d) = 0.0;
 HAL_PIN(vel_ff) = 1.0;
-HAL_PIN(vel_lp) = 0.9;
+HAL_PIN(vel_lp) = 0.3;
 
 HAL_PIN(acc_p) = 0.1;
 HAL_PIN(acc_ff) = 0.0;
@@ -53,7 +53,7 @@ HAL_PIN(volt) = 130;
 
 
 HAL_PIN(vel_max) = 62.9;
-HAL_PIN(vel_e_max) = 2.5;
+HAL_PIN(vel_e_max) = 1.25;
 HAL_PIN(acc_max) = 1200.0;
 HAL_PIN(force_max) = 100.0;
 HAL_PIN(cur_max) = 6.0;
diff --git a/src/setup.c b/src/setup.c
index 3052cea4..50d29f28 100644
--- a/src/setup.c
+++ b/src/setup.c
@@ -80,7 +80,7 @@ void setup_pid_timer(){
     /* pid int set up, TIM5*/
     TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
     TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
-    TIM_TimeBaseStructure.TIM_Period = 8400;//1kHz
+    TIM_TimeBaseStructure.TIM_Period = 1680;//5kHz
     TIM_TimeBaseStructure.TIM_Prescaler = 9;
     //TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
     TIM_TimeBaseInit(TIM5, &TIM_TimeBaseStructure);