added pid_controller class

Author: gbr1

examples/motor/motor.ino

@@ -14,7 +14,7 @@ void setup(){
   t=millis();
   t_change=millis();
   robot.setRpmRight(reference);
-  robot.setKPidRight(150, 0.1, 0);
+  robot.setKPidRight(30.0, 0.1, 0.4);
   Serial.print("reference");
   Serial.print(" ");
   Serial.println("measure");

src/motor_control.h

@@ -5,6 +5,7 @@
 #include "dcmotor.h"
 #include "encoder.h"
 #include "robot_definitions.h"
+#include "pid_controller.h"
 
 #define CONTROL_LIMIT 4095
 #define MEM_SIZE 5
@@ -41,6 +42,7 @@ class MotorControl{
 
         DCmotor * motor;
         Encoder * encoder;
+        PidController * vel_pid;
     public:
         MotorControl(DCmotor * _motor, Encoder * _encoder, const float _kp, const float _ki, const float _kd, const float _controller_period){
             motor = _motor;
@@ -72,6 +74,7 @@ class MotorControl{
             mean=0.0;
 
             conversion_factor = (1/MOTOR_RATIO)/(controller_period);
+            vel_pid = new PidController(kp,ki,kd,controller_period,CONTROL_LIMIT);
         }
 
         void begin(){
@@ -84,6 +87,7 @@ class MotorControl{
         bool setRPM(const float ref){
             if ((ref<MOTOR_LIMIT)&&(ref>-MOTOR_LIMIT)){
                 reference = ref;
+                vel_pid->setReference(reference);
                 return true;
             }
             return false;
@@ -159,7 +163,9 @@ class MotorControl{
         }
 
         */
+    
 
+    /*
        void update(){
             measure = encoder->getCount();
             encoder->reset();
@@ -175,6 +181,16 @@ class MotorControl{
             motor->setSpeed(-actuation);
        }
 
+       */
+
+        void update(){
+            measure = encoder->getCount();
+            encoder->reset();
+            measure = measure*conversion_factor;
+            vel_pid->update(measure);
+            motor->setSpeed(-vel_pid->getControlOutput());
+        }
+
         void setKP(const float _kp){
             kp=_kp;
         }

src/pid_controller.h

@@ -0,0 +1,108 @@
+#ifndef __PID_CONTROLLER_H__
+#define __PID_CONTROLLER_H__
+
+#include "Arduino.h"
+
+class PidController{
+    private:
+        float kp;
+        float ki;
+        float kd;
+
+        float reference;
+        float error;
+        float error_rate;
+        float previous_error;
+        float error_sum;
+        float last_measure;
+
+        float ctrl_p;
+        float ctrl_i;
+        float ctrl_d;
+        float ctrl_output;
+
+        float ctrl_period;
+        float ctrl_limit;
+
+
+    public:
+        PidController(const float _kp, const float _ki, const float _kd, const float _ctrl_period, const int _ctrl_limit = 4095){
+            kp=_kp;
+            ki=_ki;
+            kd=_kd;
+            ctrl_period=_ctrl_period;
+            ctrl_limit=_ctrl_limit;
+            
+            reference=0.0;
+            error=0.0;
+            error_rate=0.0;
+            previous_error=0.0;
+            error_sum=0.0;
+            last_measure=0.0;
+
+            ctrl_p=0.0;
+            ctrl_i=0.0;
+            ctrl_d=0.0;
+            ctrl_output=0.0;
+        }
+
+        void setReference(const float _reference){
+            reference=_reference;
+        }
+
+        float getControlOutput(){
+            return ctrl_output;
+        }
+
+        void setKPid(const float _kp, const float _ki, const float _kd){
+            kp=_kp;
+            ki=_ki;
+            kd=_kd;
+        }
+
+        void setKp(const float _k){
+            kp=_k;
+        }
+
+        void setKi(const float _k){
+            ki=_k;
+        }
+
+        void setKd(const float _k){
+            kd=_k;
+        }
+
+        void update(const float measure){
+            error = reference - measure;
+            error_sum += error*ctrl_period;
+            error_rate = (measure-last_measure)/ctrl_period;
+            last_measure=measure;
+            ctrl_p = error*kp;
+            ctrl_i = checkLimits(error_sum*ki);
+            ctrl_d = error_rate*kd;
+            ctrl_output = checkLimits(ctrl_p+ctrl_i-ctrl_d);
+        }
+
+        float checkLimits(float value){
+            if (value>ctrl_limit){
+                return ctrl_limit;
+            }
+            if (value<-ctrl_limit){
+                return -ctrl_limit;
+            }
+            return value;
+        }
+
+        void reset(){
+            ctrl_output=0.0;
+            error=0.0;
+            error_sum=0.0;
+            previous_error=0.0;
+            reference=0.0;
+            last_measure=0.0;
+        }
+};
+
+
+
+#endif

src/sensor_tof_matrix.h

@@ -17,7 +17,7 @@ class SensorTofMatrix{
         }
 
         int begin(){
-            wire->begin();
+            //wire->begin();
             sensor->begin();
             sensor->init_sensor();
             sensor->vl53l7cx_set_resolution(VL53L7CX_RESOLUTION_4X4);