Takeuchi Issei / Mbed 2 deprecated 4Omunisleeve3

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 Eigen

shared/MD_PID/MD_PID.cpp@0:ee7e9405e1c7, 2021-04-14 (annotated)

Committer:
e2011220
Date:
Wed Apr 14 07:26:19 2021 +0000
Revision:
0:ee7e9405e1c7
first

Who changed what in which revision?

UserRevisionLine numberNew contents of line
e2011220 0:ee7e9405e1c7 1 #include "MD_PID.h"
e2011220 0:ee7e9405e1c7 2
e2011220 0:ee7e9405e1c7 3 MD_PID::MD_PID( MD *md, QEI *qei,
e2011220 0:ee7e9405e1c7 4 double k, double ti, double td,
e2011220 0:ee7e9405e1c7 5 double max_speed)
e2011220 0:ee7e9405e1c7 6 : _md(md), _qei(qei), _pid(new PI_D(k, ti, td))
e2011220 0:ee7e9405e1c7 7 {
e2011220 0:ee7e9405e1c7 8 _duty = 0.0;
e2011220 0:ee7e9405e1c7 9 set_max_speed(max_speed);
e2011220 0:ee7e9405e1c7 10 }
e2011220 0:ee7e9405e1c7 11
e2011220 0:ee7e9405e1c7 12 void MD_PID::drive(double target_vel, double interval)
e2011220 0:ee7e9405e1c7 13 {
e2011220 0:ee7e9405e1c7 14 double present_vel = read_vel();
e2011220 0:ee7e9405e1c7 15
e2011220 0:ee7e9405e1c7 16 if(fabs(target_vel) > _max_speed){
e2011220 0:ee7e9405e1c7 17 if(target_vel >= 0)
e2011220 0:ee7e9405e1c7 18 target_vel = _max_speed;
e2011220 0:ee7e9405e1c7 19 else
e2011220 0:ee7e9405e1c7 20 target_vel = -_max_speed;
e2011220 0:ee7e9405e1c7 21 }
e2011220 0:ee7e9405e1c7 22 //PID_Control::PID(present_vel, target_vel, interval);
e2011220 0:ee7e9405e1c7 23 //PID_Control::VPID(present_vel, target_vel, interval);
e2011220 0:ee7e9405e1c7 24 //PID::PI_D(present_vel, target_vel, interval);
e2011220 0:ee7e9405e1c7 25 _pid->set_state(present_vel, target_vel);
e2011220 0:ee7e9405e1c7 26 _duty = -1 * _pid->get_control();
e2011220 0:ee7e9405e1c7 27
e2011220 0:ee7e9405e1c7 28 if( fabs(_duty) > 1 )
e2011220 0:ee7e9405e1c7 29 _duty /= fabs(_duty);
e2011220 0:ee7e9405e1c7 30
e2011220 0:ee7e9405e1c7 31 _md->drive(_duty);
e2011220 0:ee7e9405e1c7 32 //printf("presnet_vel:\t%lf\tduty:\t%lf\r\n", present_vel, _duty);
e2011220 0:ee7e9405e1c7 33 }
e2011220 0:ee7e9405e1c7 34
e2011220 0:ee7e9405e1c7 35 double MD_PID::get_duty(){
e2011220 0:ee7e9405e1c7 36 return _duty;
e2011220 0:ee7e9405e1c7 37 }
e2011220 0:ee7e9405e1c7 38
e2011220 0:ee7e9405e1c7 39 void MD_PID::brake(double target)
e2011220 0:ee7e9405e1c7 40 {
e2011220 0:ee7e9405e1c7 41
e2011220 0:ee7e9405e1c7 42 }
e2011220 0:ee7e9405e1c7 43
e2011220 0:ee7e9405e1c7 44 void MD_PID::free()
e2011220 0:ee7e9405e1c7 45 {
e2011220 0:ee7e9405e1c7 46
e2011220 0:ee7e9405e1c7 47 }
e2011220 0:ee7e9405e1c7 48
e2011220 0:ee7e9405e1c7 49 void MD_PID::set_max_speed(double max_speed)
e2011220 0:ee7e9405e1c7 50 {
e2011220 0:ee7e9405e1c7 51 _max_speed = max_speed;
e2011220 0:ee7e9405e1c7 52 }
e2011220 0:ee7e9405e1c7 53
e2011220 0:ee7e9405e1c7 54 double MD_PID::read_vel()
e2011220 0:ee7e9405e1c7 55 {
e2011220 0:ee7e9405e1c7 56 return _qei->get_ang_vel();
e2011220 0:ee7e9405e1c7 57 }