Diff: actions.h

Revision:

0:8adde5eaea5a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/actions.h	Wed Nov 08 20:30:42 2017 +0000
@@ -0,0 +1,75 @@
+#ifndef ACTIONS_H
+#define ACTIONS_H
+
+PwmOut mypwm(PWM_OUT);
+DigitalOut myled(LED1);
+//Left Motor
+PwmOut m1F(PC_7);
+PwmOut m1B(PB_10);
+//Right Motor
+PwmOut m2F(PA_7);
+PwmOut m2B(PB_6);
+//Left Encoder
+AnalogIn lFront(PB_3);
+AnalogIn lBack(PA_15);
+//Right Encoder
+AnalogIn rFront(PA_1);
+AnalogIn rBack(PC_14);
+
+Timer 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.08f;
+        prev = 0.0f;
+        kp = 1.0f; //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 d = (error - e->prev)/DT;
+  e->integral += error * DT;
+  e->prev = error;
+}
+
+
+void turnleft_loop(float lencoder, float rencoder){ //4096 per turn of wheel
+    while(lencoder < 12861.0f){
+        float error = lencoder - 12861.0f;
+        float delta_motor = getFix(lturn, error);
+        m1F.period(0.01f);
+        m1F = m1basespeed + delta_motor;
+    }
+    while(rencoder < 12861f){
+        float error = rencoder - 12861f;
+        float delta_motor = getFix(rturn, error);
+        m2B.period(0.01f);
+        m2B = m2basespeed + delta_motor;
+    }
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+#endif
\ No newline at end of file