Diff: SeeedStudioShieldBot.cpp

Revision:

0:227158f56a11

Child:

1:5c40f2a5e1ac

diff -r 000000000000 -r 227158f56a11 SeeedStudioShieldBot.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SeeedStudioShieldBot.cpp	Mon Jul 15 13:16:09 2013 +0000
@@ -0,0 +1,125 @@
+#include "SeeedStudioShieldBot.h"
+#include "mbed.h"
+
+
+SeeedStudioShieldBot::SeeedStudioShieldBot() :  motor1A(PTA5),
+    motor1B(PTC9),
+    motor2A(PTA12),
+    motor2B(PTD0),
+    motor1En(PTC8),
+    motor2En(PTD5),
+    rightSensor(PTB0),
+    inRightSensor(PTB1),
+    centreSensor(PTB2),
+    inLeftSensor(PTB3),
+    leftSensor(PTA4) {
+    // Something should go here...
+}
+
+void SeeedStudioShieldBot::right_motor(float speed) { 
+    // The bit that's actually needed...
+    if (speed >= 0) {
+        motor1A = speed;
+        motor1B = 0;
+    }
+    else {
+        motor1A = 1 + speed;
+        motor1B = 1;
+    }
+}
+
+void SeeedStudioShieldBot::left_motor(float speed) {   
+    // Useful bit
+    if (speed >= 0) {
+        motor2A = speed;
+        motor2B = 0;
+    }
+    else {
+        motor2A = 1 + speed;
+        motor2B = 1;
+    }
+}
+
+// The following two functions turn the robot on the spot...
+
+void SeeedStudioShieldBot::left(float speed) {
+    left_motor(-speed);
+    right_motor(speed);
+}
+
+void SeeedStudioShieldBot::right(float speed) {
+    left_motor(speed);
+    right_motor(-speed);
+}
+
+// Again, until we get PWM on the pin w/o PWM, there's no way we can really have this any other way than 1...
+// This is the case for both forwards and backwards...
+
+// FIXED, using jumper between pins 8 and 3 on top of the robot board... unfortunately it wastes an io pin, but is easier than using software IO.
+// It also means that the 'lid' can't go on top.
+
+void SeeedStudioShieldBot::forward(float speed) {
+    if (speed == 0) {
+        left_motor(0);
+        right_motor(0);
+    }
+    else {
+        left_motor(speed);
+        right_motor(speed);
+    }
+}
+
+void SeeedStudioShieldBot::backward(float speed) {
+    if (speed == 0) {
+        left_motor(0);
+        right_motor(0);
+    }
+    else {
+        left_motor(-speed);
+        right_motor(-speed);
+    }
+}
+
+void SeeedStudioShieldBot::enable_motor_a() {
+    motor1En = 1;
+}
+
+void SeeedStudioShieldBot::enable_motor_b() {
+    motor2En = 1;
+}
+
+void SeeedStudioShieldBot::disable_motor_a() {
+    motor1En = 0;
+}
+
+void SeeedStudioShieldBot::disable_motor_b() {
+    motor2En = 0;
+}
+
+// Give a value representative of the overall data received by the sensors...
+// Output between +1 and -1.
+// Positive is right, negative left...
+float SeeedStudioShieldBot::get_sensors_overall_value() {
+    float output = 0; 
+    if (rightSensor == 1) {
+        output += 0.5;
+    }
+    if (inRightSensor == 1) {
+        output += 0.25;
+    }
+    if (leftSensor == 1) {
+        output -= 0.5;
+    }
+    if (inLeftSensor == 1) {
+        output -= 0.25;
+    }
+    
+    return output;
+}
+
+void SeeedStudioShieldBot::stopAll() {
+    motor1A = 0;
+    motor1B = 0;
+    motor2A = 0;
+    motor2B = 0;
+}
\ No newline at end of file