QuadratureController.cpp

00001 #include "Controller/QuadratureController.h"
00002 
00003 QuadratureController::QuadratureController(PwmOut *pwmOut, Queue<float, 2> *quadrature_queue, Queue<float, 2> *imu_queue_steering_angle) {
00004     this->pwmOut = pwmOut;
00005     this->quadrature_queue = quadrature_queue;
00006     this->imu_queue_steering_angle = imu_queue_steering_angle;
00007     init();
00008 }
00009 
00010 void QuadratureController::init() {
00011     timer_steering_angle_sampling_time = 0.01;
00012     
00013     q_Kp = 0;
00014     q_Ki = 18.6661236;
00015     feed_forward_control_factor = 11.2921;
00016     q_esum = 0;
00017     feed_forward = 0;
00018     q_Ki_sampling_time = q_Ki * timer_steering_angle_sampling_time;
00019 }
00020 
00021 void QuadratureController::check_queues() {
00022     steering_angle_set_event = quadrature_queue->get(0);
00023     if (steering_angle_set_event.status == osEventMessage) {
00024         steering_angle_set = *(float *)steering_angle_set_event.value.p;
00025     }
00026     
00027     steering_angle_current_event = imu_queue_steering_angle->get(0);
00028     if (steering_angle_current_event.status == osEventMessage) {
00029         steering_angle_current = *(float *)steering_angle_current_event.value.p;
00030     }
00031 }
00032 
00033 void QuadratureController::cylic_control() {
00034     check_queues();
00035     
00036     q_e = steering_angle_set - steering_angle_current;
00037     q_esum = q_esum + q_e;
00038     
00039     feed_forward = steering_angle_set * feed_forward_control_factor;
00040     q_PI_controller = q_Kp*q_e + q_Ki_sampling_time * q_esum;
00041 
00042     q_output = feed_forward + q_PI_controller;
00043 
00044     if(q_output > 500){q_output = 500;}           // evtl Begrenzung schon auf z.b. 300/ -300 stellen (wegen Linearität)
00045     else if(q_output < -500){q_output = - 500;}
00046 
00047     q_PWM = 1500 + q_output;
00048     
00049     pwmOut->pulsewidth_us(q_PWM);
00050 }