Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Diff: Controller/MachineDirectionController.cpp
- Revision:
- 13:34f7f783ad24
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Controller/MachineDirectionController.cpp Fri Feb 05 16:06:44 2016 +0000 @@ -0,0 +1,53 @@ +#include "Controller/MachineDirectionController.h" + +MachineDirectionController::MachineDirectionController(PwmOut *pwmOut, Queue<float, 2> *machine_direction_queue, Queue<float, 2> *imu_queue_velocity) { + this->pwmOut = pwmOut; + this->machine_direction_queue = machine_direction_queue; + this->imu_queue_velocity = imu_queue_velocity; + init(); +} + +void MachineDirectionController::init() { + timer_velocity_sampling_time=0.01; + + l_Ki=0.1*timer_velocity_sampling_time; +} + +void MachineDirectionController::check_queues() { + velocity_set_event = machine_direction_queue->get(0); + if (velocity_set_event.status == osEventMessage) { + velocity_set = *(float*)velocity_set_event.value.p; + } + + velocity_current_event = imu_queue_velocity->get(0); + if (velocity_current_event.status == osEventMessage) { + velocity_current = *(float *)velocity_current_event.value.p; + } +} + +void MachineDirectionController::cylic_control() { + check_queues(); + + Vorsteuerung=88.316*velocity_set+175.17; + l_e = velocity_set-velocity_current; + l_esum = l_esum + l_e; + + PI_Regler =l_Kp*l_e+l_Ki * l_esum; + + l_output=Vorsteuerung+PI_Regler; + + l_PWM = 1500+l_output; + + if(l_PWM<1500) { + l_PWM = 1500; + l_esum = l_esum-2*l_e; + } else if(l_PWM>2000) { + l_PWM = 2000; + l_esum = l_esum-2*l_e; + } else if(velocity_set < 0.1) { + l_PWM = 1500; + } + + pwmOut->pulsewidth_us(1700); + //pwmOut.pulsewidth_us(l_PWM); +} \ No newline at end of file