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

Committer:
mborchers
Date:
2016-02-04
Revision:
4:f0be27a5a83a
Parent:
3:391c4639bc7d
Child:
6:aa27bc8c58f5

File content as of revision 4:f0be27a5a83a:

#include "Controller/QuadratureController.h"

QuadratureController::QuadratureController(PwmOut *pwmOut) {
    this->pwmOut = pwmOut;
    this->quadrature_queue = quadrature_queue;
    this->imu_queue_steering_angle = imu_queue_steering_angle;
    init();
}

void QuadratureController::init() {
    timer_steering_angle_sampling_time = 0.01;
    
    q_Kp = 8.166343211;
    q_Ki = 18.6661236;
    feed_forward_control_factor = 13.37091452;
    q_esum = 0;
    feed_forward = 0;
    q_Ki_sampling_time = q_Ki * timer_steering_angle_sampling_time;
}

void QuadratureController::check_queues() {
    steering_angle_set_event = quadrature_queue->get(0);
    if (steering_angle_set_event.status == osEventMessage) {
        steering_angle_set = *(float *)steering_angle_set_event.value.p;
    }
    
    steering_angle_current_event = imu_queue_steering_angle->get(0);
    if (steering_angle_current_event.status == osEventMessage) {
        steering_angle_current = *(float *)steering_angle_current_event.value.p;
    }
}

void QuadratureController::cylic_control() {
    check_queues();
    
    q_e = steering_angle_set - steering_angle_current;
    q_esum = q_esum + q_e;
    
    feed_forward = steering_angle_set * feed_forward_control_factor;
    q_PI_controller = q_Kp*q_e + q_Ki_sampling_time * q_esum;

    q_output = feed_forward + q_PI_controller;

    if(q_output > 500){q_output = 500;}           // evtl Begrenzung schon auf z.b. 300/ -300 stellen (wegen Linearität)
    else if(q_output < -500){q_output = - 500;}

    q_PWM = 1500 + q_output;
    
    pwmOut->pulsewidth_us(q_PWM);
}