MachineDirectionController.cpp

00001 #include "Controller/MachineDirectionController.h"
00002 
00003 MachineDirectionController::MachineDirectionController(PwmOut *pwmOut, Queue<float, 2> *machine_direction_queue, Queue<float, 2> *imu_queue_velocity) {
00004     this->pwmOut = pwmOut;
00005     this->machine_direction_queue = machine_direction_queue;
00006     this->imu_queue_velocity = imu_queue_velocity;
00007     init();
00008 }
00009 
00010 void MachineDirectionController::init() {
00011     timer_velocity_sampling_time=0.01;
00012     
00013     l_Ki=0.1*timer_velocity_sampling_time;
00014 }
00015 
00016 void MachineDirectionController::check_queues() {
00017     velocity_set_event = machine_direction_queue->get(0);
00018     if (velocity_set_event.status == osEventMessage) {
00019         velocity_set = *(float*)velocity_set_event.value.p;
00020     }
00021     
00022     velocity_current_event = imu_queue_velocity->get(0);
00023     if (velocity_current_event.status == osEventMessage) {
00024         velocity_current = *(float *)velocity_current_event.value.p;
00025     }
00026 }
00027 
00028 void MachineDirectionController::cylic_control() {
00029     check_queues();
00030     
00031     Vorsteuerung=88.316*velocity_set+175.17;
00032     l_e = velocity_set-velocity_current;
00033     l_esum = l_esum + l_e;
00034 
00035     PI_Regler =l_Kp*l_e+l_Ki * l_esum;
00036 
00037     l_output=Vorsteuerung+PI_Regler;
00038 
00039     l_PWM = 1500+l_output;
00040 
00041     if(l_PWM<1500) {
00042         l_PWM = 1500;
00043         l_esum = l_esum-2*l_e;
00044     } else if(l_PWM>2000) {
00045         l_PWM = 2000;
00046         l_esum = l_esum-2*l_e;
00047     } else if(velocity_set < 0.1) {
00048         l_PWM = 1500;
00049     }
00050     
00051     pwmOut->pulsewidth_us(1700);
00052     //pwmOut.pulsewidth_us(l_PWM);
00053 }