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@15:141de2b5646d, 2016-02-07 (annotated)

Committer:
mborchers
Date:
Sun Feb 07 18:55:50 2016 +0000
Revision:
15:141de2b5646d
Parent:
14:48cdd880ca1a
Child:
17:76636aaf80de
xfd

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mborchers 13:34f7f783ad24 1 #include "Controller/MachineDirectionController.h"
mborchers 13:34f7f783ad24 2
mborchers 13:34f7f783ad24 3 MachineDirectionController::MachineDirectionController(PwmOut *pwmOut, Queue<float, 2> *machine_direction_queue, Queue<float, 2> *imu_queue_velocity) {
mborchers 13:34f7f783ad24 4 this->pwmOut = pwmOut;
mborchers 13:34f7f783ad24 5 this->machine_direction_queue = machine_direction_queue;
mborchers 13:34f7f783ad24 6 this->imu_queue_velocity = imu_queue_velocity;
mborchers 13:34f7f783ad24 7 init();
mborchers 13:34f7f783ad24 8 }
mborchers 13:34f7f783ad24 9
mborchers 13:34f7f783ad24 10 void MachineDirectionController::init() {
mborchers 13:34f7f783ad24 11 timer_velocity_sampling_time=0.01;
mborchers 15:141de2b5646d 12 max_velocity = 2.0;
mborchers 13:34f7f783ad24 13 l_Ki=0.1*timer_velocity_sampling_time;
mborchers 13:34f7f783ad24 14 }
mborchers 13:34f7f783ad24 15
mborchers 13:34f7f783ad24 16 void MachineDirectionController::check_queues() {
mborchers 13:34f7f783ad24 17 velocity_set_event = machine_direction_queue->get(0);
mborchers 13:34f7f783ad24 18 if (velocity_set_event.status == osEventMessage) {
mborchers 13:34f7f783ad24 19 velocity_set = *(float*)velocity_set_event.value.p;
mborchers 13:34f7f783ad24 20 }
mborchers 13:34f7f783ad24 21
mborchers 15:141de2b5646d 22 /* velocity_current_event = imu_queue_velocity->get(0);
mborchers 13:34f7f783ad24 23 if (velocity_current_event.status == osEventMessage) {
mborchers 13:34f7f783ad24 24 velocity_current = *(float *)velocity_current_event.value.p;
mborchers 15:141de2b5646d 25 }*/
mborchers 13:34f7f783ad24 26 }
mborchers 13:34f7f783ad24 27
mborchers 13:34f7f783ad24 28 void MachineDirectionController::cylic_control() {
mborchers 13:34f7f783ad24 29 check_queues();
mborchers 15:141de2b5646d 30 if( velocity_set < 0.1) {
mborchers 15:141de2b5646d 31 velocity_set = velocity_set_alt;
mborchers 15:141de2b5646d 32 } else if(velocity_set > max_velocity){
mborchers 15:141de2b5646d 33 velocity_set = max_velocity;
mborchers 15:141de2b5646d 34 }
mborchers 15:141de2b5646d 35
mborchers 15:141de2b5646d 36 velocity_set = 1.2;
mborchers 13:34f7f783ad24 37
mborchers 13:34f7f783ad24 38 Vorsteuerung=88.316*velocity_set+175.17;
OWenzel 14:48cdd880ca1a 39 //l_e = velocity_set-velocity_current;
mborchers 15:141de2b5646d 40 //l_esum = l_esum + l_e;
mborchers 13:34f7f783ad24 41
mborchers 15:141de2b5646d 42 //PI_Regler =l_Kp*l_e+l_Ki * l_esum;
mborchers 13:34f7f783ad24 43
mborchers 15:141de2b5646d 44 //l_output=Vorsteuerung+PI_Regler;
OWenzel 14:48cdd880ca1a 45
mborchers 13:34f7f783ad24 46
mborchers 15:141de2b5646d 47 //l_PWM = 1500+l_output;
OWenzel 14:48cdd880ca1a 48 l_PWM=1500+Vorsteuerung;
mborchers 13:34f7f783ad24 49 if(l_PWM<1500) {
mborchers 13:34f7f783ad24 50 l_PWM = 1500;
mborchers 13:34f7f783ad24 51 l_esum = l_esum-2*l_e;
mborchers 13:34f7f783ad24 52 } else if(l_PWM>2000) {
mborchers 13:34f7f783ad24 53 l_PWM = 2000;
mborchers 13:34f7f783ad24 54 l_esum = l_esum-2*l_e;
mborchers 13:34f7f783ad24 55 }
OWenzel 14:48cdd880ca1a 56 velocity_set_alt=velocity_set;
mborchers 13:34f7f783ad24 57
OWenzel 14:48cdd880ca1a 58 //pwmOut->pulsewidth_us(1700);
OWenzel 14:48cdd880ca1a 59 pwmOut->pulsewidth_us(l_PWM);
mborchers 13:34f7f783ad24 60 }