Revision 0:40cfee72b03a, committed 2019-07-01

Comitter:

fuji_

Date:

Mon Jul 01 06:44:11 2019 +0000

Commit message:

20190701backdrive

Changed in this revision

diff -r 000000000000 -r 40cfee72b03a pid.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pid.cpp	Mon Jul 01 06:44:11 2019 +0000
@@ -0,0 +1,101 @@
+#include "pid.h"
+
+//position
+void PositionPid::period(float control_cycle){
+        dt = control_cycle;
+}
+
+void PositionPid::gain(float kp_, float ki_, float kd_) {
+    kp = kp_;
+    ki = ki_;
+    kd = kd_;
+}
+
+void PositionPid::reset(){
+    for(int i=NOW;i<=OLD;i++){
+        distance[i] = 0;
+        difference[i] = 0;
+    }
+    
+    for(int i=P; i<=D; i++){
+        result[i] = 0;
+    }
+}
+
+void PositionPid::cal(float current,float target){
+    distance[NOW] = current;
+    
+    difference[OLD] = difference[NOW];
+    difference[NOW] = target - distance[NOW];
+
+    result[P]  = difference[NOW];
+    result[I] += (difference[NOW]+difference[OLD])/2 * dt;
+    result[D]  = (distance[NOW] - distance[OLD])/dt;
+    
+    result[OUTPUT] = kp*result[P] + ki*result[I] + kd*result[D];
+    
+    if(result[OUTPUT] >  pid_constant::UPPER_LIMIT){
+         result[OUTPUT] =  pid_constant::UPPER_LIMIT;
+    } else if(result[OUTPUT] < pid_constant::LOWER_LIMIT){
+         result[OUTPUT] = pid_constant::LOWER_LIMIT;
+    }
+    
+    distance[OLD] = distance[NOW];
+}
+
+float PositionPid::getState(int channel){
+    float output;
+    switch(channel){
+        case P:
+            output = result[P];
+        case I:
+            output = result[I];
+        case D:
+            output = result[D];
+        case OUTPUT:
+            output = result[OUTPUT];
+    }
+    return output;
+}
+
+
+//speed
+void SpeedPid::cal(float target_speed, float now_speed){
+        diffelence[TWO_OLD] = diffelence[OLD];
+        diffelence[OLD]     = diffelence[NOW];
+        diffelence[NOW]     = target_speed - now_speed;
+            
+        result[P]  = diffelence[NOW] - diffelence[OLD];
+        result[I]  = diffelence[NOW] * dt;
+        result[D]  = diffelence[NOW] - diffelence[OLD]*2 + diffelence[TWO_OLD];
+            
+        result[OUTPUT] += (result[P]*gain[P] + result[I]*gain[I] + result[D]*gain[D]);
+            
+        if(result[OUTPUT] >  1.0f){ 
+                result[OUTPUT] =  1.0f;
+            } else if(result[OUTPUT] < -1.0f){
+                result[OUTPUT] = -1.0f;
+            }
+}
+            
+float SpeedPid::getState(int channel){
+        float output;
+        switch(channel){
+            case P:
+                output = result[P];
+            case I:
+                output = result[I];
+            case D:
+                output = result[D];
+            case OUTPUT:
+                output = result[OUTPUT];
+        }
+        return output;
+}
+        
+void SpeedPid::init(float kp_, float ki_, float kd_, float dt_){
+        gain[P] = kp_;
+        gain[I] = ki_;
+        gain[D] = kd_;
+        dt = dt_;
+}
\ No newline at end of file

diff -r 000000000000 -r 40cfee72b03a pid.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pid.h	Mon Jul 01 06:44:11 2019 +0000
@@ -0,0 +1,60 @@
+#ifndef PID_H
+#define PID_H
+#include "mbed.h"
+
+enum INDEX{
+	NOW,
+	OLD,
+	TWO_OLD,
+	INDEX_NUMBER,
+};
+
+enum RESULT{
+	P,
+	I,
+	D,
+	OUTPUT,
+	RESULT_NUMBER
+};
+
+namespace pid_constant{
+	const float UPPER_LIMIT =  1.0f;
+	const float LOWER_LIMIT = -1.0f;
+};
+
+class PositionPid{
+    public:
+        void period(float control_cycle);
+        void gain(float kp_, float ki_, float kd_);
+        void reset();
+        
+        void cal(float current,float target);
+        
+        float getState(int channel);
+        
+		
+	private:
+		float result[RESULT_NUMBER];	// = {P計算値, I計算値, D計算値, 出力値};
+		float difference[INDEX_NUMBER]; // = {現在の目標値と現在値の差分, 1つ前の目標値と現在地の差分};
+		float distance[INDEX_NUMBER];	// = {現在の位置, 1つ前のい位置};
+		float kp, ki, kd, dt,dt_reciprocal;
+			  
+};
+
+class SpeedPid{
+	public:				
+		void cal(float target_speed, float now_speed);
+			
+		float getState(int channel);
+		
+        void init(float kp_, float ki_, float kd_, float dt_);
+	private:
+		float 	dt,
+				gain[RESULT_NUMBER],
+				diffelence[RESULT_NUMBER],
+				result[RESULT_NUMBER];
+
+};
+
+
+#endif