Diff: Controller/QuadratureController.cpp

Revision:

3:391c4639bc7d

Child:

4:f0be27a5a83a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controller/QuadratureController.cpp	Thu Feb 04 08:54:06 2016 +0000
@@ -0,0 +1,50 @@
+#include "Controller/QuadratureController.h"
+
+QuadratureController::QuadratureController(PwmOut *pwmOut) {
+    this->pwmOut = pwmOut;
+    this->quadrature_queue = quadrature_queue;
+    this->imu_queue_steering_angle = imu_queue_steering_angle;
+    init();
+}
+
+void QuadratureController::init() {
+    timer_steering_angle_sampling_time = 0.01;
+    
+    q_Kp = 8.166343211;
+    q_Ki = 18.6661236;
+    feed_forward_control_factor = 13.37091452;
+    q_esum = 0;
+    feed_forward = 0;
+    q_Ki_sampling_time = q_Ki * timer_steering_angle_sampling_time;
+}
+
+void QuadratureController::check_queues() {
+    steering_angle_set_event = quadrature_queue->get(0);
+    if (steering_angle_set_event.status == osEventMessage) {
+        steering_angle_set = *(float *)steering_angle_set_event.value.p;
+    }
+    
+    steering_angle_current_event = imu_queue_steering_angle->get(0);
+    if (steering_angle_current_event.status == osEventMessage) {
+        steering_angle_current = *(float *)steering_angle_current_event.value.p;
+    }
+}
+
+void QuadratureController::cylic_control(void const *args) {
+    check_queues();
+    
+    q_e = steering_angle_set - steering_angle_current;
+    q_esum = q_esum + q_e;
+    
+    feed_forward = steering_angle_set * feed_forward_control_factor;
+    q_PI_controller = q_Kp*q_e + q_Ki_sampling_time * q_esum;
+
+    q_output = feed_forward + q_PI_controller;
+
+    if(q_output > 500){q_output = 500;}           // evtl Begrenzung schon auf z.b. 300/ -300 stellen (wegen Linearität)
+    if(q_output < -500){q_output = - 500;}
+
+    q_PWM = 1500 + q_output;
+    
+    pwmOut->pulsewidth_us(q_PWM);
+}
\ No newline at end of file