Diff: Controller/QuadratureController.cpp

Revision:

18:0e8ad413a840

Parent:

17:76636aaf80de

--- a/Controller/QuadratureController.cpp	Mon Feb 08 14:56:34 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,50 +0,0 @@
-#include "Controller/QuadratureController.h"
-
-QuadratureController::QuadratureController(PwmOut *pwmOut, Queue<float, 2> *quadrature_queue, Queue<float, 2> *imu_queue_steering_angle) {
-    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 = 0;
-    q_Ki = 18.6661236;
-    feed_forward_control_factor = 11.2921;
-    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() {
-    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)
-    else if(q_output < -500){q_output = - 500;}
-
-    q_PWM = 1500 + q_output;
-    
-    pwmOut->pulsewidth_us(q_PWM);
-}
\ No newline at end of file