SeeedStudioShieldBot.cpp

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