Diff: motorcar/motorcar.cpp

Revision:

2:1dcd81fdef9e

Child:

3:4be8f486a120

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motorcar/motorcar.cpp	Tue Jul 31 08:09:05 2018 +0000
@@ -0,0 +1,108 @@
+#include "mbed.h"
+
+#include "motorcar.h"
+//#include "Ping.h"
+
+// output used for controlling motors
+AnalogOut M1_enable(D6);
+AnalogOut M2_enable(D5);
+DigitalOut M1_in1(D3); // connected to right motor (IN1)
+DigitalOut M1_in2(D2); // connected to right motor (IN2)
+DigitalOut M2_in3(D1); // connected to left motor (IN3)
+DigitalOut M2_in4(D0); // connected to left motor (IN4)
+
+// input used for IR sensors
+AnalogIn leftIR(A1); // left sensor
+AnalogIn middleIR(A3); // middle sensor
+AnalogIn rightIR(A5); // right sensor
+
+// used for storing returned IR sensors' value
+int left, middle, right;
+
+// PID factors
+int interror = 0;
+int olderror = 0;
+int values = 0;
+float Kp = 1, Ki = 0, Kd = 0;
+/* Though STM32 only has 12-bit DAC,
+ * mbed still cam map the value by
+ * using write_u16()
+ */
+int limit = 65535; // 2^16 - 1
+
+// used for output message via serial for ease of debugging
+Serial pc(SERIAL_TX, SERIAL_RX);
+
+void readSensorValue() {
+    left = leftIR.read_u16();
+    middle = middleIR.read_u16();
+    right = rightIR.read_u16();
+    
+    values = left * (-1) + middle * 0 + right * 1;
+
+    //pc.printf("left middle right: %d %d %d\r\n", left, middle, right);
+    //pc.printf("IR values: %d\r\n", ret);
+}
+
+void runPID() {
+    int error = values;
+    interror += error;
+    int lasterror = error - olderror;
+    olderror = error;
+    int power = error * Kp + interror * Ki + lasterror * Kd;
+    
+    // limit the output of PID value
+    if(power > limit) {
+        power = limit;
+    } 
+    if(power < -limit) {
+        power = -limit;
+    }
+    
+    // PID works here
+    if(power > 0) {
+        speed(limit, limit - power);
+    } else {
+        speed(limit + power, limit);
+    }
+}
+
+void init() {
+    speed(0, 0);
+}
+
+void speed(int left, int right) {
+    // control right wheel
+    if(right < 0) { 
+        // go backward
+        right = -right;
+        M1_in1 = 0;
+        M1_in2 = 1;
+    } else if(right == 0) {
+        // stop
+        M1_in1 = 0;
+        M1_in2 = 0;
+    } else {
+        // go forward
+        M1_in1 = 1;
+        M1_in2 = 0;
+    }        
+    M1_enable.write_u16(right);
+    
+    // control left wheel
+    if(left < 0) {
+        // go backward
+        left = -left;
+        M2_in3 = 0;
+        M2_in4 = 1;
+    } else if(left == 0) {
+        // stop
+        M2_in3 = 0;
+        M2_in4 = 0;
+    } else {
+        // go forward
+        M2_in3 = 1;
+        M2_in4 = 0;
+    }
+    M2_enable.write_u16(left);
+}
\ No newline at end of file