Diff: main.cpp

Revision:

0:fae4f75668ab

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Feb 10 10:30:55 2017 +0000
@@ -0,0 +1,67 @@
+#include "mbed.h"
+#include "encoderKRAI.h"
+#include "Motor.h"
+#include "millis.h"
+ 
+double speed, maxSpeed = 0.8, minSpeed = -0.8;
+double kp=0.6757, kd=0.6757, ki=0.00006757;
+double p,i,d;
+double last_error = 0, current_error, sum_error = 0;
+double rpm, target_rpm = 9.0;
+
+encoderKRAI encoder( PB_13, PB_14, 14, encoderKRAI::X2_ENCODING);
+Serial pc(USBTX,USBRX);
+Motor motor(PA_8,PC_1,PC_2);
+
+unsigned long int previousMillis = 0;
+unsigned long int currentMillis;
+
+void init_speed (){
+    if (speed>maxSpeed){
+        speed = maxSpeed;
+    }
+    
+    if (speed<minSpeed){
+        speed = minSpeed;
+    }
+}
+
+
+
+
+int main() {
+
+startMillis();    
+
+    
+    while(1) {
+
+        currentMillis = millis();
+        
+        
+        if (currentMillis-previousMillis>=10)
+        {
+            rpm = (double)encoder.getPulses();
+            
+            current_error = target_rpm - rpm;
+            sum_error = sum_error + current_error;
+            p = current_error*kp;
+            d = (current_error-last_error)*kd/10.0;
+            i = sum_error*ki*10.0;
+        
+            speed = p + d + i;
+            init_speed();
+            motor.speed(speed);
+            last_error = current_error;
+        
+            encoder.reset();
+            pc.printf("%.04lf\n",rpm);
+            
+            previousMillis = currentMillis;
+            
+        }
+
+        
+        
+    }
+}