Diff: functions.cpp

Revision:

0:15664f71b21f

Child:

1:bd88d4062c97

diff -r 000000000000 -r 15664f71b21f functions.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/functions.cpp	Sat May 23 14:13:09 2015 +0000
@@ -0,0 +1,92 @@
+// The following file implements all the functions necessary 
+// for the robot.
+// Please don't modify any of these functions!
+
+#include "mbed.h"
+#include "Motor.h"
+#include "hcsr04.h"
+
+// Returns value from the QRE1113
+// lower numbers mean more refleacive
+// more than 3000 means nothing was reflected.
+int read_line1()
+{
+    // Time to record how long input is active
+    Timer t;
+    
+    // Activate the line sensor and then read
+    line1.output();
+    line1 = 1;
+    wait_us(15);
+    line1.input();
+    
+    // Start timer
+    t.start();
+
+    // Time how long the input is HIGH, but quit
+    // after 1ms as nothing happens after that
+    while (line1 == 1 && t.read_us() < 1000);
+    return t.read_us();
+}
+
+int read_line2()
+{
+    // Time to record how long input is active
+    Timer t;
+    
+    // Activate the line sensor and then read
+    line2.output();
+    line2 = 1;
+    wait_us(15);
+    line2.input();
+    
+    // Start timer
+    t.start();
+
+    // Time how long the input is HIGH, but quit
+    // after 1ms as nothing happens after that
+    while (line2 == 1 && t.read_us() < 1000);
+    return t.read_us();
+}
+
+int detect_line()
+{
+    int line1val = read_line1();
+    int line2val = read_line2();
+    
+    if ((line1val-line2val) > 50) {
+        printf("Line detected from front");
+        return 1;
+    } else if ((line1val-line2val) < -50) {
+        printf("Line detected from back");
+        return -1;
+    } else {
+        printf("Line not detected");
+        return 0;
+    }
+}
+
+void reverse()
+{
+    printf("Reverse\n");
+    MotorLeft.speed((chargespeed));
+    MotorRight.speed(-(chargespeed));
+    wait_ms(1000);
+    return;
+}
+
+void reverseandturn()
+{
+    printf("Reverse and turn\n");
+    MotorLeft.speed((chargespeed-0.2));
+    MotorRight.speed(-(chargespeed-0.1));
+    wait_ms(2000);
+    return;
+}
+
+void charge()
+{
+    MotorLeft.speed(chargespeed);
+    MotorRight.speed(chargespeed);
+    return;
+}
\ No newline at end of file