Matti Borchers
/
mbed_amf_controlsystem_iO_copy
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.
Fork of
mbed_amf_controlsystem_iO_copy
by 
Oliver Wenzel
Controller/QuadratureController.cpp@4:f0be27a5a83a, 2016-02-04 (annotated)
- Committer:
- mborchers
- Date:
- Thu Feb 04 09:33:42 2016 +0000
- Revision:
- 4:f0be27a5a83a
- Parent:
- 3:391c4639bc7d
- Child:
- 6:aa27bc8c58f5
Fehler behoben (s. Funktion "redirect_quadrature_controller")
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| mborchers | 3:391c4639bc7d | 1 | #include "Controller/QuadratureController.h" |
| mborchers | 3:391c4639bc7d | 2 | |
| mborchers | 3:391c4639bc7d | 3 | QuadratureController::QuadratureController(PwmOut *pwmOut) { |
| mborchers | 3:391c4639bc7d | 4 | this->pwmOut = pwmOut; |
| mborchers | 3:391c4639bc7d | 5 | this->quadrature_queue = quadrature_queue; |
| mborchers | 3:391c4639bc7d | 6 | this->imu_queue_steering_angle = imu_queue_steering_angle; |
| mborchers | 3:391c4639bc7d | 7 | init(); |
| mborchers | 3:391c4639bc7d | 8 | } |
| mborchers | 3:391c4639bc7d | 9 | |
| mborchers | 3:391c4639bc7d | 10 | void QuadratureController::init() { |
| mborchers | 3:391c4639bc7d | 11 | timer_steering_angle_sampling_time = 0.01; |
| mborchers | 3:391c4639bc7d | 12 | |
| mborchers | 3:391c4639bc7d | 13 | q_Kp = 8.166343211; |
| mborchers | 3:391c4639bc7d | 14 | q_Ki = 18.6661236; |
| mborchers | 3:391c4639bc7d | 15 | feed_forward_control_factor = 13.37091452; |
| mborchers | 3:391c4639bc7d | 16 | q_esum = 0; |
| mborchers | 3:391c4639bc7d | 17 | feed_forward = 0; |
| mborchers | 3:391c4639bc7d | 18 | q_Ki_sampling_time = q_Ki * timer_steering_angle_sampling_time; |
| mborchers | 3:391c4639bc7d | 19 | } |
| mborchers | 3:391c4639bc7d | 20 | |
| mborchers | 3:391c4639bc7d | 21 | void QuadratureController::check_queues() { |
| mborchers | 3:391c4639bc7d | 22 | steering_angle_set_event = quadrature_queue->get(0); |
| mborchers | 3:391c4639bc7d | 23 | if (steering_angle_set_event.status == osEventMessage) { |
| mborchers | 3:391c4639bc7d | 24 | steering_angle_set = *(float *)steering_angle_set_event.value.p; |
| mborchers | 3:391c4639bc7d | 25 | } |
| mborchers | 3:391c4639bc7d | 26 | |
| mborchers | 3:391c4639bc7d | 27 | steering_angle_current_event = imu_queue_steering_angle->get(0); |
| mborchers | 3:391c4639bc7d | 28 | if (steering_angle_current_event.status == osEventMessage) { |
| mborchers | 3:391c4639bc7d | 29 | steering_angle_current = *(float *)steering_angle_current_event.value.p; |
| mborchers | 3:391c4639bc7d | 30 | } |
| mborchers | 3:391c4639bc7d | 31 | } |
| mborchers | 3:391c4639bc7d | 32 | |
| mborchers | 4:f0be27a5a83a | 33 | void QuadratureController::cylic_control() { |
| mborchers | 3:391c4639bc7d | 34 | check_queues(); |
| mborchers | 3:391c4639bc7d | 35 | |
| mborchers | 3:391c4639bc7d | 36 | q_e = steering_angle_set - steering_angle_current; |
| mborchers | 3:391c4639bc7d | 37 | q_esum = q_esum + q_e; |
| mborchers | 3:391c4639bc7d | 38 | |
| mborchers | 3:391c4639bc7d | 39 | feed_forward = steering_angle_set * feed_forward_control_factor; |
| mborchers | 3:391c4639bc7d | 40 | q_PI_controller = q_Kp*q_e + q_Ki_sampling_time * q_esum; |
| mborchers | 3:391c4639bc7d | 41 | |
| mborchers | 3:391c4639bc7d | 42 | q_output = feed_forward + q_PI_controller; |
| mborchers | 3:391c4639bc7d | 43 | |
| mborchers | 3:391c4639bc7d | 44 | if(q_output > 500){q_output = 500;} // evtl Begrenzung schon auf z.b. 300/ -300 stellen (wegen Linearität) |
| mborchers | 4:f0be27a5a83a | 45 | else if(q_output < -500){q_output = - 500;} |
| mborchers | 3:391c4639bc7d | 46 | |
| mborchers | 3:391c4639bc7d | 47 | q_PWM = 1500 + q_output; |
| mborchers | 3:391c4639bc7d | 48 | |
| mborchers | 3:391c4639bc7d | 49 | pwmOut->pulsewidth_us(q_PWM); |
| mborchers | 3:391c4639bc7d | 50 | } |