Diff: main.cpp

Revision:

0:99dd5499928a

Child:

1:b9485bb1ca69

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Nov 19 03:01:07 2015 +0000
@@ -0,0 +1,113 @@
+#include "mbed.h"
+#include "QEI.h"
+#include "AVEncoder.h"
+
+DigitalOut myled(LED1);
+DigitalIn mybutton(USER_BUTTON);
+
+PwmOut LMotorForward(PA_7);
+PwmOut LMotorReverse(PB_6);
+
+PwmOut RMotorForward(PB_10);
+PwmOut RMotorReverse(PB_4);
+
+QEI LeftEncoder(PA_15, PB_3, NC, 12);
+QEI RightEncoder(PA_1, PA_2, NC, 12);
+
+Serial pc(USBTX, USBRX);
+
+void pid();
+double P_controller(int error);
+double D_controller(int error);
+double I_controller(int error);
+void setRightPwm(double speed); 
+void setLeftPwm(double speed);
+void stop();
+
+double speed = 0.5;
+Ticker interrupt;
+int STOP = 0;
+
+int main() 
+{
+    myled = 1;
+    interrupt.attach_us(&pid, 1000);
+    while(mybutton == 1);
+    
+    speed = 0.5;
+    
+    while(true)
+    {
+        setLeftPwm(speed);
+        setRightPwm(speed);
+    }
+}
+
+void pid()
+{
+    speed += P_controller(0) + D_controller(0) + I_controller(0);
+    
+    if(speed < 0)
+    {
+        speed = 0;
+    }
+    else if(speed > 1)
+    {
+        speed = 1;
+    }
+}
+
+double P_controller(int error)
+{
+    return -1;
+}
+
+double D_controller(int error)
+{
+    return -1;
+}
+
+double I_controller(int error)
+{
+    return -1;
+}
+
+void setLeftPwm(double speed) 
+{
+    if(speed == 0)
+    {
+        LMotorForward = 1.0;
+        LMotorReverse = 1.0;
+    }
+    if(speed > 0)
+    {
+        LMotorForward = speed;
+        LMotorReverse = 0;
+    }
+    if(speed < 0)
+    {
+        LMotorForward = 0;
+        LMotorReverse = -speed;
+    }
+}
+
+//todo: make this have ramping
+void setRightPwm(double speed) 
+{
+    if(speed > 0)
+    {
+        RMotorForward = speed;
+        RMotorReverse = STOP;
+    }
+    if(speed < 0)
+    {
+        RMotorForward = STOP;
+        RMotorReverse = -speed;
+    }
+}
+
+void stop()
+{
+    setLeftPwm(0);
+    setRightPwm(0);
+}