na
Fork of SeeedShieldBot by
SeeedStudioShieldBot.cpp
- Committer:
- melse
- Date:
- 2013-07-15
- Revision:
- 2:118efce95f3c
- Parent:
- 1:5c40f2a5e1ac
- Child:
- 7:887c729fd7c1
File content as of revision 2:118efce95f3c:
#include "SeeedStudioShieldBot.h" #include "mbed.h" SeeedStudioShieldBot::SeeedStudioShieldBot() : rightSensor(PTB0), inRightSensor(PTB1), centreSensor(PTB2), inLeftSensor(PTB3), leftSensor(PTA4), motor1A(PTA5), motor1B(PTC9), motor1En(PTC8), motor2A(PTA12), motor2B(PTD0), motor2En(PTD5) { // 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_right_motor() { motor1En = 1; } void SeeedStudioShieldBot::enable_left_motor() { motor2En = 1; } void SeeedStudioShieldBot::disable_right_motor() { motor1En = 0; } void SeeedStudioShieldBot::disable_left_motor() { 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::line_position() { float output = 0; if (rightSensor == 1) { output += 0.5; } if (inRightSensor == 1) { output += 0.5; } if (leftSensor == 1) { output -= 0.5; } if (inLeftSensor == 1) { output -= 0.5; } return output; } void SeeedStudioShieldBot::stopAll() { motor1A = 0; motor1B = 0; motor2A = 0; motor2B = 0; } void SeeedStudioShieldBot::stop(int motor) { if (motor == 1) { // Stop motor 0... motor1A = 0; motor1B = 0; } else if (motor == 2) { motor2A = 0; motor2B = 0; } }