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.txt@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 kp, double ki, double kd,
e2011220 0:ee7e9405e1c7 5 double max_speed)
e2011220 0:ee7e9405e1c7 6 : _md(md), _qei(qei), PID_Control(kp, ki, kd)
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_Control::PI_D(present_vel, target_vel, interval);
e2011220 0:ee7e9405e1c7 25 _duty = PID_Control::get_control();
e2011220 0:ee7e9405e1c7 26
e2011220 0:ee7e9405e1c7 27 //printf("%lf\r\n", _duty);
e2011220 0:ee7e9405e1c7 28
e2011220 0:ee7e9405e1c7 29 if( fabs(_duty) > 1 )
e2011220 0:ee7e9405e1c7 30 _duty /= fabs(_duty);
e2011220 0:ee7e9405e1c7 31
e2011220 0:ee7e9405e1c7 32 _md->drive(_duty);
e2011220 0:ee7e9405e1c7 33 //printf("presnet_vel:\t%lf\tduty:\t%lf\r\n", present_vel, _duty);
e2011220 0:ee7e9405e1c7 34 }
e2011220 0:ee7e9405e1c7 35
e2011220 0:ee7e9405e1c7 36 double MD_PID::get_duty(){
e2011220 0:ee7e9405e1c7 37 return _duty;
e2011220 0:ee7e9405e1c7 38 }
e2011220 0:ee7e9405e1c7 39
e2011220 0:ee7e9405e1c7 40 void MD_PID::brake(double target)
e2011220 0:ee7e9405e1c7 41 {
e2011220 0:ee7e9405e1c7 42
e2011220 0:ee7e9405e1c7 43 }
e2011220 0:ee7e9405e1c7 44
e2011220 0:ee7e9405e1c7 45 void MD_PID::free()
e2011220 0:ee7e9405e1c7 46 {
e2011220 0:ee7e9405e1c7 47
e2011220 0:ee7e9405e1c7 48 }
e2011220 0:ee7e9405e1c7 49
e2011220 0:ee7e9405e1c7 50 void MD_PID::set_max_speed(double max_speed)
e2011220 0:ee7e9405e1c7 51 {
e2011220 0:ee7e9405e1c7 52 _max_speed = max_speed;
e2011220 0:ee7e9405e1c7 53 }
e2011220 0:ee7e9405e1c7 54
e2011220 0:ee7e9405e1c7 55 double MD_PID::read_vel()
e2011220 0:ee7e9405e1c7 56 {
e2011220 0:ee7e9405e1c7 57 return _qei->get_ang_vel();
e2011220 0:ee7e9405e1c7 58 }