SYRT WS21/22 version (developed during lecture on 2022-01-18)

Core/Src/main.c

@@ -70,6 +70,7 @@ iir1_filter_t voltage_iir1;
 float speed_raw;
 float speed_filt;
 static uint16_t datarec_divcnt = 0;
+pictrl_t speed_pi;
 
 /* USER CODE END PV */
 
@@ -680,6 +681,10 @@ void hw_init(void){
 	filter_iir1_init(&speed_iir1, 20.0f, 0.0625f);
 	filter_iir1_init(&voltage_iir1, 10.0f, 0.0625f);
 
+	control_pictrl_init(&speed_pi, 0.61, 84, 62.5e-3);
+	speed_pi.max = 20.0f;
+	speed_pi.min = -20.0f;
+
 	//Encoder
 	LL_TIM_EnableCounter(TIM3);
 
@@ -753,7 +758,7 @@ void control_sm(void){
 	}
 }
 
-//Call by 16kHz (every 2nd PWM counter update due to repetition counter)
+//Call by 16kHz (every 4th PWM counter update due to repetition counter)
 void main_pwm_ctrl(void){
 	LL_GPIO_SetOutputPin(DebugPin0_GPIO_Port, DebugPin0_Pin);
 
@@ -780,7 +785,21 @@ void main_pwm_ctrl(void){
 	speed_filt = filter_iir1(&speed_iir1, speed_raw);
 
 	// *** Control ***
-	uref = 0.0f;
+	//uref = val_para_uref[0];
+	//if(val_para_mode == 1){
+	//	uref = val_para_uref[1];
+	//}
+
+	float speed_ref;
+	speed_ref = val_para_nref[0];
+	if(val_para_mode == 1){
+		speed_ref = val_para_nref[1];
+	}
+
+	uref = control_pictrl2(&speed_pi, speed_ref, speed_filt);
+
+
+	// *** End of Control ***
 
 
 	//*** Voltage output / PWM ***