Diff: main.cpp

Revision:

1:4200f28040d3

Parent:

0:bc9c90c7b357

--- a/main.cpp	Wed Feb 08 13:55:01 2017 +0000
+++ b/main.cpp	Thu Feb 09 13:46:48 2017 +0000
@@ -3,15 +3,15 @@
 #include "Motor.h"
 #include "millis.h"
  
-double speed, maxSpeed = 0.8, minSpeed = -0.8, last_speed = 0.0;
-double kp=0.0005, kd=0.005, ki=0.000005;
+double speed, maxSpeed = 0.8, minSpeed = -0.8;
+double kp=0.167, kd=0.167, ki=0.000167;
 double p,i,d;
 double last_error = 0, current_error, sum_error = 0;
-double rpm, target_rpm = 1500.0;
+double rpm, target_rpm = 2000.0;
 
 encoderKRAI encoder( PB_14, PB_13, 14, encoderKRAI::X2_ENCODING);
 Serial pc(USBTX,USBRX);
-Motor motor(PA_8,PC_1,PC_2);
+Motor motor(PA_8,PC_2,PC_1);
 
 unsigned long int previousMillis = 0;
 unsigned long int currentMillis;
@@ -35,28 +35,33 @@
 
     
     while(1) {
-        current_error = target_rpm - rpm;
-        sum_error += current_error;
-        p = current_error*kp;
-        d = (current_error-last_error)*kd;
-        i = sum_error*ki;
-        
-        speed = last_speed + p + d;
-        init_speed();
-        motor.speed(speed);
+
         currentMillis = millis();
         
         
         if (currentMillis-previousMillis>=50)
         {
             rpm = (double)encoder.getPulses()*60000/(double)(14.0*50.0);
-            previousMillis = currentMillis;
+            
+            current_error = target_rpm - rpm;
+            sum_error = sum_error + current_error;
+            p = current_error*kp;
+            d = (current_error-last_error)*kd/50.0;
+            i = sum_error*ki*50.0;
+        
+            speed = p + d + i;
+            init_speed();
+            motor.speed(speed);
+            last_error = current_error;
+        
             encoder.reset();
             pc.printf("%.04lf\n",rpm);
             
+            previousMillis = currentMillis;
+            
         }
-        //last_speed = speed;
-        last_error = current_error;
+
+        
         
     }
 }