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@3:391c4639bc7d, 2016-02-04 (annotated)

Committer:
mborchers
Date:
Thu Feb 04 08:54:06 2016 +0000
Revision:
3:391c4639bc7d
Child:
4:f0be27a5a83a
Habe versucht die Querregelung in eine Klasse auszulagern (Quadrature Control). Er wirft allerdings ein Fehler wenn ich die Funktion aus dem Klasse dem Timer ?bergeben will.

Who changed what in which revision?

UserRevisionLine numberNew 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 3:391c4639bc7d 33 void QuadratureController::cylic_control(void const *args) {
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 3:391c4639bc7d 45 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 }