Diff: main.cpp

Revision:

0:178159bf8a9c

Child:

1:f22840ee371d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Jun 28 22:20:25 2018 +0000
@@ -0,0 +1,96 @@
+#include "mbed.h"
+#include "TSISensor.h"
+
+#include "PID.h"
+#define RATE 0.1
+
+#include "MMA8451Q.h"
+
+#if   defined (TARGET_KL25Z) || defined (TARGET_KL46Z)
+  PinName const SDA = PTE25;
+  PinName const SCL = PTE24;
+#elif defined (TARGET_KL05Z)
+  PinName const SDA = PTB4;
+  PinName const SCL = PTB3;
+#elif defined (TARGET_K20D50M)
+  PinName const SDA = PTB1;
+  PinName const SCL = PTB0;
+#else
+  #error TARGET NOT DEFINED
+#endif
+
+#define MMA8451_I2C_ADDRESS (0x1d<<1)
+
+TSISensor tsi;
+MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
+
+//Kc, Ti, Td, interval
+PID controller(1.0, 0.0, 0.0, RATE);
+
+int fordward = 1;
+int backward = 0;
+int direction;
+int offSystem;
+
+PwmOut pwmWheelL(D8);
+PwmOut pwmWheelR(D5);
+
+DigitalOut directWheelL(D10);
+DigitalOut directWheelR(D12);
+
+int main()
+{
+    //Analog input from 0.0 to 3.3V
+    controller.setInputLimits(-0.40, 0.40);
+    //Pwm output from 0.0 to 1.0
+    controller.setOutputLimits(0.0, 1.0);
+    //If there's a bias.
+    controller.setBias(0.3);
+    controller.setMode(1);
+    //We want the process variable to be 1.7V
+    controller.setSetPoint(0.00);
+    
+   // directWheelL = fordward;
+//    directWheelR = fordward;
+    
+    pwmWheelL.period(0.001);
+    pwmWheelR.period(0.001);
+    
+    printf("MMA8451 ID: %d\n", acc.getWhoAmI());
+   
+    while (true) {
+        float x, y, z;
+        x = abs(acc.getAccX());
+        y = acc.getAccY();
+        z = abs(acc.getAccZ());
+        
+        //pwmWheelL = 1.0 - tsi.readPercentage();
+        //pwmWheelR = 1.0 - tsi.readPercentage();
+//        pwmWheelL = tsi.readPercentage();
+//        pwmWheelR = tsi.readPercentage();
+
+        if(acc.getAccY()<0){
+            direction = fordward;
+        }else{
+            direction = backward;
+        }
+        directWheelL = direction;
+        directWheelR = direction;
+        //Update the process variable.
+        controller.setProcessValue(y);
+        //Set the new output.
+        if(tsi.readPercentage()>0.00){
+            pwmWheelL = controller.compute();
+            pwmWheelR = pwmWheelL;
+        }else{
+            pwmWheelL = 0;
+            pwmWheelR = 0;
+        }
+//        pwmWheelL = controller.compute();
+//        pwmWheelR = pwmWheelL;
+        //Wait for another loop calculation.
+        wait(RATE);
+    
+        printf("X: %1.2f, Y: %1.2f, Z: %1.2f, Touch: %1.2f, PID: %1.2f\n", x, y, z, tsi.readPercentage(), controller.compute());
+    }
+}
\ No newline at end of file