Diff: main.cpp

Revision:

0:e997997dca8e

diff -r 000000000000 -r e997997dca8e main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Nov 09 01:08:18 2017 +0000
@@ -0,0 +1,115 @@
+#include "mbed.h"
+#include "QEI.h"
+ 
+Serial pc(SERIAL_TX, SERIAL_RX); 
+ 
+PwmOut lpwmf(PA_7);
+PwmOut lpwmb(PB_6);
+ 
+PwmOut rpwmf(PB_10);
+PwmOut rpwmb(PC_7);
+ 
+ 
+PinName rfront(PB_3);
+PinName rback(PA_15);
+//DigitalIn lfront(PB_3);
+//DigitalIn lback(PA_15);
+//Right Encoder
+PinName lfront(PA_1);
+PinName lback(PC_4);
+ 
+//QEI::Encoding encoding = QEI::Encoding[1];
+ 
+QEI LeftEncoder(lfront, lback, NC, 4096, QEI::X4_ENCODING);
+QEI RightEncoder(rfront, rback, NC, 4096, QEI::X4_ENCODING);       
+ 
+const float rbase = 0.135f;
+const float lbase = 0.12f;
+ 
+Timer t_time;
+ 
+struct pid { //Note that because we have two different types of distance sensors (Andrew's works a little differently than Jeffrey's we should have two different errors. To stay straight though we can just use one side right?)
+  pid(){
+        integral = 0.0f;
+        prev = 0.0f;
+        kp = .01f; //the ks should be negative to counteract error
+        ki = 0.0f;
+        kd = 0.8f;
+      }
+  float integral;
+  int prev;
+  float kp; //the ks should be negative to counteract error
+  float ki;
+  float kd;
+};
+ 
+void resetpid(struct pid *e) {
+  e->integral = 0.0f;
+  e->prev = 0.0f;
+}
+ 
+float getFix(struct pid *e, float error, float time) {
+  float d = (error - e->prev)/time;
+  e->integral += error * time;
+  e->prev = error;
+  float temp = (e->kp * error + e->ki * e->integral + e->kd * d);
+  return temp;
+
+}
+ 
+float constrain(float l_limit, float u_limit, float val){
+    if (val < l_limit){
+        return l_limit;
+        }
+    else if(val > u_limit){
+        return u_limit;
+        }    
+    return val;    
+    }
+ 
+int main() {
+    pid lman, rman;
+    lman.kp = .0040f;
+    lman.ki = .0002f;
+    lman.kd = .0015f;
+    rman.kp = .5f;
+    rman.ki = .1f;
+    rman.kd = .4f;
+    
+    
+    lpwmf.period(0.01f);
+    lpwmf = lbase;
+    rpwmf.period(0.01f);
+    rpwmf = rbase;
+    t_time.start();
+    
+    while(1){
+        float dt = t_time.read();
+        t_time.reset();
+        float error = (LeftEncoder.getPulses() - RightEncoder.getPulses())/16.0f; //Can be pos or neg, Range from 0 - 255 for one revolution
+
+//        pc.printf("no %d \n", RightEncoder.getPulses());
+//        pc.printf("yas %d \n", LeftEncoder.getPulses());
+
+
+        if(error == 0.0f){
+            resetpid(&lman);
+            resetpid(&rman);
+        }
+        float adjust_l = getFix(&lman, error, dt); 
+        pc.printf("%f \n", adjust_l);
+        
+        float lspeed = constrain(0.0f, 0.4f, lbase - adjust_l);
+        float rspeed = rbase;
+        
+        pc.printf("no %f \n", rspeed);
+        pc.printf("yas %f \n", lspeed);
+        
+        lpwmf = lspeed;
+        rpwmf = rspeed;
+        
+        wait(0.2f);
+        
+        }
+        t_time.stop();
+    }
\ No newline at end of file