control mode

Author: gbr1

src/motor_control.h

@@ -28,16 +28,30 @@
 
 #define CONTROL_LIMIT 4095
 #define MEM_SIZE 5
+#define CONTROL_MODE_NORMAL 0
+#define CONTROL_MODE_LINEAR 1
 
 class MotorControl{
     private:
+
+        uint8_t control_mode;
+
         float kp;
         float ki;
         float kd;
 
         float travel;
         float reference;
 
+        float trip;
+        float iterations;
+        float start_value;
+        float end_value;
+        float step_size;
+        float step;
+        int step_index;
+        float interpolation;
+
         float controller_period;
         float conversion_factor;
         float measure;
@@ -56,10 +70,15 @@ class MotorControl{
 
         PidController * vel_pid;
 
-        MotorControl(DCmotor * _motor, Encoder * _encoder, const float _kp, const float _ki, const float _kd, const float _controller_period){
+        MotorControl(DCmotor * _motor, Encoder * _encoder, const float _kp, const float _ki, const float _kd,
+                                       const float _controller_period,
+                                       const uint8_t _control_mode = CONTROL_MODE_LINEAR, const float _step_size=5.0){
             motor = _motor;
             encoder = _encoder;
 
+            control_mode = _control_mode;
+
+
             kp = _kp;
             ki = _ki;
             kd = _kd;
@@ -69,6 +88,15 @@ class MotorControl{
             travel=0.0;
             reference = 0.0;
 
+            trip=0.0;
+            iterations=0.0;
+            start_value=0.0;
+            end_value=0.0;
+            step_size=_step_size;
+            step=0.0;
+            step_index=0;
+            interpolation=0.0;
+
             measure = 0.0;
             last_measure = 0.0;
 
@@ -86,20 +114,16 @@ class MotorControl{
             vel_pid->reset();
             clearMemory();
         }
-
+/*
         bool setRPM(const float ref){
             if ((ref<MOTOR_LIMIT)&&(ref>-MOTOR_LIMIT)){
                 reference = ref;
                 vel_pid->setReference(reference);
-                /*
-                if ((reference)<0.1&&(reference>-0.1)){
-                    vel_pid->reset();
-                }
-                */
                 return true;
             }
             return false;
         }
+*/
 
         float checkLimits(float value){
             if (value>CONTROL_LIMIT){
@@ -191,6 +215,26 @@ class MotorControl{
 
        */
 
+
+        bool setRPM(const float ref){
+            if ((ref<MOTOR_LIMIT)&&(ref>-MOTOR_LIMIT)){
+                reference = ref;
+                if (control_mode==CONTROL_MODE_LINEAR){
+                    start_value=interpolation;
+                    end_value=reference;
+                    trip=0.0;
+                    iterations=abs(end_value-start_value)/step_size;
+                    step=1.0/iterations;
+                    step_index=0;
+                }
+                else if(control_mode==CONTROL_MODE_NORMAL){
+                    vel_pid->setReference(reference);
+                }
+                return true;
+            }
+            return false;
+        }
+
         void update(){
 
             measure = encoder->getCount();
@@ -224,7 +268,25 @@ class MotorControl{
             measure = measure*conversion_factor;
             */
 
-            
+
+            //vel_pid->update(measure);
+            //motor->setSpeed(vel_pid->getControlOutput());
+
+
+            if (control_mode==CONTROL_MODE_LINEAR){
+                if (step_index<iterations){
+                    step_index++;
+                    trip+=step;
+                    interpolation=trip*(end_value-start_value)+start_value;
+                    if (abs(interpolation)>abs(reference)){
+                        interpolation=reference;
+                    }
+                }
+                vel_pid->setReference(interpolation);
+            }            
+            else if(control_mode==CONTROL_MODE_NORMAL){
+                vel_pid->setReference(reference);
+            }
 
 
             vel_pid->update(measure);
@@ -247,6 +309,22 @@ class MotorControl{
             return vel_pid->getError();
         }
 
+        void brake(){
+            reference=0.0;
+            trip=0.0;
+            iterations=0.0;
+            start_value=0.0;
+            end_value=0.0;
+            step=0.0;
+            step_index=0;
+            interpolation=0.0;
+            clearMemory();
+
+            motor->setSpeed(0);
+            vel_pid->setReference(0.0);
+            vel_pid->reset();
+        }
+
 
 };
 

src/robot_definitions.h

@@ -29,9 +29,12 @@
 
 const float MOTOR_RATIO = MOTOR_CPR*MOTOR_GEAR_RATIO;
 
-#define MOTOR_KP_RIGHT 120.0
-#define MOTOR_KI_RIGHT 300.0
-#define MOTOR_KD_RIGHT 1.0
+#define MOTOR_KP_RIGHT 32.0
+#define MOTOR_KI_RIGHT 450.0
+#define MOTOR_KD_RIGHT 0.0
+
+//30, 450, 0.0
+//120,300,1,0
 #define MOTOR_CONTROL_PERIOD 0.02