Matti Borchers / Mbed 2 deprecated 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.

Dependencies:   mbed-rtos mbed

Fork of mbed_amf_controlsystem_iO_copy by Oliver Wenzel

Controller/QuadratureController.cpp@6:aa27bc8c58f5, 2016-02-04 (annotated)

Committer:
torstenwylegala
Date:
Thu Feb 04 17:46:19 2016 +0000
Revision:
6:aa27bc8c58f5
Parent:
4:f0be27a5a83a
Child:
12:221c9e02ea96
foo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mborchers 3:391c4639bc7d 1 #include "Controller/QuadratureController.h"
mborchers 3:391c4639bc7d 2
torstenwylegala 6:aa27bc8c58f5 3 QuadratureController::QuadratureController(PwmOut *pwmOut, Queue<float, 2> *quadrature_queue, Queue<float, 2> *imu_queue_steering_angle) {
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 }