Diff: MDPIDSpeed/MDPIDSpeed.cpp

Revision:

0:e83b840a5f86

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MDPIDSpeed/MDPIDSpeed.cpp	Tue Sep 18 03:11:01 2018 +0000
@@ -0,0 +1,70 @@
+/*
+ * MDPIDSpeed.cpp
+ *
+ *  Created on: 2018/08/31
+ *      Author: komazawataiyou
+ */
+
+#include "MDPIDSpeed.h"
+
+MD_PID_Speed::MD_PID_Speed(MD *md, QEI *encorder, double kp, double ki, double kd)
+: Timer_PID(kp, ki, kd), duty_(0.00), target_rpm_(0.00)
+{
+	this->md = md;
+	this->encorder = encorder;
+
+}
+
+MD_PID_Speed::~MD_PID_Speed() {
+}
+
+void MD_PID_Speed::drive(double rpm)
+{
+	double current_rpm = this->get_current_rpm();
+	target_rpm_ = rpm;
+
+	duty_ += Timer_PID::PID(current_rpm, rpm);
+
+	for(int i = 1; i < (signed)(sizeof(rpm_log_) / sizeof(double)); i++)
+		rpm_log_[i - 1] = rpm_log_[i];
+	rpm_log_[sizeof(rpm_log_) / sizeof(double) - 1] = current_rpm;
+
+	md->drive(duty_);
+}
+
+void MD_PID_Speed::brake(double rpm)
+{
+
+}
+
+void MD_PID_Speed::free()
+{
+
+}
+
+bool MD_PID_Speed::target_complete(float allowable_error)
+{
+	allowable_error = (fabs(allowable_error) < 1.0) ? fabs(allowable_error) : 1.00;
+	double rpm_mean = 0.00;
+	for(int i = 0; i < (signed)(sizeof(rpm_log_) / sizeof(rpm_log_[0])); i++)
+		rpm_mean += rpm_log_[i];
+	rpm_mean /= (sizeof(rpm_log_) / sizeof(double));
+
+	return (rpm_mean <= target_rpm_ + allowable_error && rpm_mean >= target_rpm_ - allowable_error);
+}
+
+double MD_PID_Speed::get_duty()
+{
+	return duty_;
+}
+
+double MD_PID_Speed::get_current_rpm()
+{
+	return encorder->speed() * 60;
+}
+
+void MD_PID_Speed::reset()
+{
+	duty_ = 0;
+	Timer_PID::reset();
+}