SeeedStudioShieldBot.cpp

00001 #include "SeeedStudioShieldBot.h"
00002 #include "mbed.h"
00003 
00004 
00005 SeeedStudioShieldBot::SeeedStudioShieldBot() :  rightSensor(PTB0),
00006     inRightSensor(PTB1),
00007     centreSensor(PTB2),
00008     inLeftSensor(PTB3),
00009     leftSensor(PTA4),
00010     motor1A(PTA5),
00011     motor1B(PTC9),
00012     motor1En(PTC8),
00013     motor2A(PTA12),
00014     motor2B(PTD0),
00015     motor2En(PTD5) {
00016     // Something should go here...
00017 }
00018 
00019 void SeeedStudioShieldBot::right_motor(float speed) { 
00020     // The bit that's actually needed...
00021     if (speed >= 0) {
00022         motor1A = speed;
00023         motor1B = 0;
00024     }
00025     else {
00026         motor1A = 1 + speed;
00027         motor1B = 1;
00028     }
00029 }
00030 
00031 void SeeedStudioShieldBot::left_motor(float speed) {   
00032     // Useful bit
00033     if (speed >= 0) {
00034         motor2A = speed;
00035         motor2B = 0;
00036     }
00037     else {
00038         motor2A = 1 + speed;
00039         motor2B = 1;
00040     }
00041 }
00042 
00043 // The following two functions turn the robot on the spot...
00044 
00045 void SeeedStudioShieldBot::left(float speed) {
00046     left_motor(-speed);
00047     right_motor(speed);
00048 }
00049 
00050 void SeeedStudioShieldBot::right(float speed) {
00051     left_motor(speed);
00052     right_motor(-speed);
00053 }
00054 
00055 // 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...
00056 // This is the case for both forwards and backwards...
00057 
00058 // 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.
00059 // It also means that the 'lid' can't go on top.
00060 
00061 void SeeedStudioShieldBot::forward(float speed) {
00062     if (speed == 0) {
00063         left_motor(0);
00064         right_motor(0);
00065     }
00066     else {
00067         left_motor(speed);
00068         right_motor(speed);
00069     }
00070 }
00071 
00072 void SeeedStudioShieldBot::backward(float speed) {
00073     if (speed == 0) {
00074         left_motor(0);
00075         right_motor(0);
00076     }
00077     else {
00078         left_motor(-speed);
00079         right_motor(-speed);
00080     }
00081 }
00082 
00083 void SeeedStudioShieldBot::enable_right_motor() {
00084     motor1En = 1;
00085 }
00086 
00087 void SeeedStudioShieldBot::enable_left_motor() {
00088     motor2En = 1;
00089 }
00090 
00091 void SeeedStudioShieldBot::disable_right_motor() {
00092     motor1En = 0;
00093 }
00094 
00095 void SeeedStudioShieldBot::disable_left_motor() {
00096     motor2En = 0;
00097 }
00098 
00099 // Give a value representative of the overall data received by the sensors...
00100 // Output between +1 and -1.
00101 // Positive is right, negative left...
00102 float SeeedStudioShieldBot::line_position() {
00103     float output = 0; 
00104     if (rightSensor == 1) {
00105         output += 0.5;
00106     }
00107     if (inRightSensor == 1) {
00108         output += 0.5;
00109     }
00110     if (leftSensor == 1) {
00111         output -= 0.5;
00112     }
00113     if (inLeftSensor == 1) {
00114         output -= 0.5;
00115     }
00116     
00117     return output;
00118 }
00119 
00120 void SeeedStudioShieldBot::stopAll() {
00121     motor1A = 0;
00122     motor1B = 0;
00123     motor2A = 0;
00124     motor2B = 0;
00125 }
00126 
00127 void SeeedStudioShieldBot::stop(int motor) {
00128     if (motor == 1) {
00129         // Stop motor 0...
00130         motor1A = 0;
00131         motor1B = 0;
00132     }
00133     else if (motor == 2) {
00134         motor2A = 0;
00135         motor2B = 0;
00136     }
00137 }