taiyou komazawa / Mbed 2 deprecated Nucleo_fliping_arm

2018 HongoMechaTech A

Dependencies:   mbed

MDPIDSpeed/MDPIDSpeed.cpp@0:e83b840a5f86, 2018-09-18 (annotated)

Committer:
Komazawa_sun
Date:
Tue Sep 18 03:11:01 2018 +0000
Revision:
0:e83b840a5f86
????????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Komazawa_sun 0:e83b840a5f86 1 /*
Komazawa_sun 0:e83b840a5f86 2 * MDPIDSpeed.cpp
Komazawa_sun 0:e83b840a5f86 3 *
Komazawa_sun 0:e83b840a5f86 4 * Created on: 2018/08/31
Komazawa_sun 0:e83b840a5f86 5 * Author: komazawataiyou
Komazawa_sun 0:e83b840a5f86 6 */
Komazawa_sun 0:e83b840a5f86 7
Komazawa_sun 0:e83b840a5f86 8 #include "MDPIDSpeed.h"
Komazawa_sun 0:e83b840a5f86 9
Komazawa_sun 0:e83b840a5f86 10 MD_PID_Speed::MD_PID_Speed(MD *md, QEI *encorder, double kp, double ki, double kd)
Komazawa_sun 0:e83b840a5f86 11 : Timer_PID(kp, ki, kd), duty_(0.00), target_rpm_(0.00)
Komazawa_sun 0:e83b840a5f86 12 {
Komazawa_sun 0:e83b840a5f86 13 this->md = md;
Komazawa_sun 0:e83b840a5f86 14 this->encorder = encorder;
Komazawa_sun 0:e83b840a5f86 15
Komazawa_sun 0:e83b840a5f86 16 }
Komazawa_sun 0:e83b840a5f86 17
Komazawa_sun 0:e83b840a5f86 18 MD_PID_Speed::~MD_PID_Speed() {
Komazawa_sun 0:e83b840a5f86 19 }
Komazawa_sun 0:e83b840a5f86 20
Komazawa_sun 0:e83b840a5f86 21 void MD_PID_Speed::drive(double rpm)
Komazawa_sun 0:e83b840a5f86 22 {
Komazawa_sun 0:e83b840a5f86 23 double current_rpm = this->get_current_rpm();
Komazawa_sun 0:e83b840a5f86 24 target_rpm_ = rpm;
Komazawa_sun 0:e83b840a5f86 25
Komazawa_sun 0:e83b840a5f86 26 duty_ += Timer_PID::PID(current_rpm, rpm);
Komazawa_sun 0:e83b840a5f86 27
Komazawa_sun 0:e83b840a5f86 28 for(int i = 1; i < (signed)(sizeof(rpm_log_) / sizeof(double)); i++)
Komazawa_sun 0:e83b840a5f86 29 rpm_log_[i - 1] = rpm_log_[i];
Komazawa_sun 0:e83b840a5f86 30 rpm_log_[sizeof(rpm_log_) / sizeof(double) - 1] = current_rpm;
Komazawa_sun 0:e83b840a5f86 31
Komazawa_sun 0:e83b840a5f86 32 md->drive(duty_);
Komazawa_sun 0:e83b840a5f86 33 }
Komazawa_sun 0:e83b840a5f86 34
Komazawa_sun 0:e83b840a5f86 35 void MD_PID_Speed::brake(double rpm)
Komazawa_sun 0:e83b840a5f86 36 {
Komazawa_sun 0:e83b840a5f86 37
Komazawa_sun 0:e83b840a5f86 38 }
Komazawa_sun 0:e83b840a5f86 39
Komazawa_sun 0:e83b840a5f86 40 void MD_PID_Speed::free()
Komazawa_sun 0:e83b840a5f86 41 {
Komazawa_sun 0:e83b840a5f86 42
Komazawa_sun 0:e83b840a5f86 43 }
Komazawa_sun 0:e83b840a5f86 44
Komazawa_sun 0:e83b840a5f86 45 bool MD_PID_Speed::target_complete(float allowable_error)
Komazawa_sun 0:e83b840a5f86 46 {
Komazawa_sun 0:e83b840a5f86 47 allowable_error = (fabs(allowable_error) < 1.0) ? fabs(allowable_error) : 1.00;
Komazawa_sun 0:e83b840a5f86 48 double rpm_mean = 0.00;
Komazawa_sun 0:e83b840a5f86 49 for(int i = 0; i < (signed)(sizeof(rpm_log_) / sizeof(rpm_log_[0])); i++)
Komazawa_sun 0:e83b840a5f86 50 rpm_mean += rpm_log_[i];
Komazawa_sun 0:e83b840a5f86 51 rpm_mean /= (sizeof(rpm_log_) / sizeof(double));
Komazawa_sun 0:e83b840a5f86 52
Komazawa_sun 0:e83b840a5f86 53 return (rpm_mean <= target_rpm_ + allowable_error && rpm_mean >= target_rpm_ - allowable_error);
Komazawa_sun 0:e83b840a5f86 54 }
Komazawa_sun 0:e83b840a5f86 55
Komazawa_sun 0:e83b840a5f86 56 double MD_PID_Speed::get_duty()
Komazawa_sun 0:e83b840a5f86 57 {
Komazawa_sun 0:e83b840a5f86 58 return duty_;
Komazawa_sun 0:e83b840a5f86 59 }
Komazawa_sun 0:e83b840a5f86 60
Komazawa_sun 0:e83b840a5f86 61 double MD_PID_Speed::get_current_rpm()
Komazawa_sun 0:e83b840a5f86 62 {
Komazawa_sun 0:e83b840a5f86 63 return encorder->speed() * 60;
Komazawa_sun 0:e83b840a5f86 64 }
Komazawa_sun 0:e83b840a5f86 65
Komazawa_sun 0:e83b840a5f86 66 void MD_PID_Speed::reset()
Komazawa_sun 0:e83b840a5f86 67 {
Komazawa_sun 0:e83b840a5f86 68 duty_ = 0;
Komazawa_sun 0:e83b840a5f86 69 Timer_PID::reset();
Komazawa_sun 0:e83b840a5f86 70 }