Diff: main.cpp

Revision:

0:bc9c90c7b357

Child:

1:4200f28040d3

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Feb 08 13:55:01 2017 +0000
@@ -0,0 +1,62 @@
+#include "mbed.h"
+#include "encoderKRAI.h"
+#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 p,i,d;
+double last_error = 0, current_error, sum_error = 0;
+double rpm, target_rpm = 1500.0;
+
+encoderKRAI encoder( PB_14, PB_13, 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) {
+        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;
+            encoder.reset();
+            pc.printf("%.04lf\n",rpm);
+            
+        }
+        //last_speed = speed;
+        last_error = current_error;
+        
+    }
+}