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/MachineDirectionController.cpp

Committer:
mborchers
Date:
2016-02-07
Revision:
15:141de2b5646d
Parent:
14:48cdd880ca1a
Child:
17:76636aaf80de

File content as of revision 15:141de2b5646d:

#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;
    max_velocity = 2.0;
    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();
    if( velocity_set < 0.1) {
        velocity_set = velocity_set_alt;
    } else if(velocity_set > max_velocity){
        velocity_set = max_velocity;
    }
    
    velocity_set = 1.2;
    
    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;
    l_PWM=1500+Vorsteuerung;
    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;
    }
    velocity_set_alt=velocity_set;
    
    //pwmOut->pulsewidth_us(1700);
    pwmOut->pulsewidth_us(l_PWM);
}