Matthew Else / SeeedStudioShieldBot

Library for the Seeed Studio Shield Bot

Dependents:   ShieldBotExample

SeeedStudioShieldBot.cpp@6:9479362be27f, 2013-07-15 (annotated)

Committer:
melse
Date:
Mon Jul 15 14:03:41 2013 +0000
Revision:
6:9479362be27f
Parent:
2:118efce95f3c 
added notes about 5v vin

Who changed what in which revision?

UserRevisionLine numberNew contents of line
melse 0:227158f56a11 1 #include "SeeedStudioShieldBot.h"
melse 0:227158f56a11 2 #include "mbed.h"
melse 0:227158f56a11 3
melse 0:227158f56a11 4
melse 2:118efce95f3c 5 SeeedStudioShieldBot::SeeedStudioShieldBot() : rightSensor(PTB0),
melse 0:227158f56a11 6 inRightSensor(PTB1),
melse 0:227158f56a11 7 centreSensor(PTB2),
melse 0:227158f56a11 8 inLeftSensor(PTB3),
melse 2:118efce95f3c 9 leftSensor(PTA4),
melse 2:118efce95f3c 10 motor1A(PTA5),
melse 2:118efce95f3c 11 motor1B(PTC9),
melse 2:118efce95f3c 12 motor1En(PTC8),
melse 2:118efce95f3c 13 motor2A(PTA12),
melse 2:118efce95f3c 14 motor2B(PTD0),
melse 2:118efce95f3c 15 motor2En(PTD5) {
melse 0:227158f56a11 16 // Something should go here...
melse 0:227158f56a11 17 }
melse 0:227158f56a11 18
melse 0:227158f56a11 19 void SeeedStudioShieldBot::right_motor(float speed) {
melse 0:227158f56a11 20 // The bit that's actually needed...
melse 0:227158f56a11 21 if (speed >= 0) {
melse 0:227158f56a11 22 motor1A = speed;
melse 0:227158f56a11 23 motor1B = 0;
melse 0:227158f56a11 24 }
melse 0:227158f56a11 25 else {
melse 0:227158f56a11 26 motor1A = 1 + speed;
melse 0:227158f56a11 27 motor1B = 1;
melse 0:227158f56a11 28 }
melse 0:227158f56a11 29 }
melse 0:227158f56a11 30
melse 0:227158f56a11 31 void SeeedStudioShieldBot::left_motor(float speed) {
melse 0:227158f56a11 32 // Useful bit
melse 0:227158f56a11 33 if (speed >= 0) {
melse 0:227158f56a11 34 motor2A = speed;
melse 0:227158f56a11 35 motor2B = 0;
melse 0:227158f56a11 36 }
melse 0:227158f56a11 37 else {
melse 0:227158f56a11 38 motor2A = 1 + speed;
melse 0:227158f56a11 39 motor2B = 1;
melse 0:227158f56a11 40 }
melse 0:227158f56a11 41 }
melse 0:227158f56a11 42
melse 0:227158f56a11 43 // The following two functions turn the robot on the spot...
melse 0:227158f56a11 44
melse 0:227158f56a11 45 void SeeedStudioShieldBot::left(float speed) {
melse 0:227158f56a11 46 left_motor(-speed);
melse 0:227158f56a11 47 right_motor(speed);
melse 0:227158f56a11 48 }
melse 0:227158f56a11 49
melse 0:227158f56a11 50 void SeeedStudioShieldBot::right(float speed) {
melse 0:227158f56a11 51 left_motor(speed);
melse 0:227158f56a11 52 right_motor(-speed);
melse 0:227158f56a11 53 }
melse 0:227158f56a11 54
melse 0:227158f56a11 55 // 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...
melse 0:227158f56a11 56 // This is the case for both forwards and backwards...
melse 0:227158f56a11 57
melse 0:227158f56a11 58 // 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.
melse 0:227158f56a11 59 // It also means that the 'lid' can't go on top.
melse 0:227158f56a11 60
melse 0:227158f56a11 61 void SeeedStudioShieldBot::forward(float speed) {
melse 0:227158f56a11 62 if (speed == 0) {
melse 0:227158f56a11 63 left_motor(0);
melse 0:227158f56a11 64 right_motor(0);
melse 0:227158f56a11 65 }
melse 0:227158f56a11 66 else {
melse 0:227158f56a11 67 left_motor(speed);
melse 0:227158f56a11 68 right_motor(speed);
melse 0:227158f56a11 69 }
melse 0:227158f56a11 70 }
melse 0:227158f56a11 71
melse 0:227158f56a11 72 void SeeedStudioShieldBot::backward(float speed) {
melse 0:227158f56a11 73 if (speed == 0) {
melse 0:227158f56a11 74 left_motor(0);
melse 0:227158f56a11 75 right_motor(0);
melse 0:227158f56a11 76 }
melse 0:227158f56a11 77 else {
melse 0:227158f56a11 78 left_motor(-speed);
melse 0:227158f56a11 79 right_motor(-speed);
melse 0:227158f56a11 80 }
melse 0:227158f56a11 81 }
melse 0:227158f56a11 82
melse 2:118efce95f3c 83 void SeeedStudioShieldBot::enable_right_motor() {
melse 0:227158f56a11 84 motor1En = 1;
melse 0:227158f56a11 85 }
melse 0:227158f56a11 86
melse 2:118efce95f3c 87 void SeeedStudioShieldBot::enable_left_motor() {
melse 0:227158f56a11 88 motor2En = 1;
melse 0:227158f56a11 89 }
melse 0:227158f56a11 90
melse 2:118efce95f3c 91 void SeeedStudioShieldBot::disable_right_motor() {
melse 0:227158f56a11 92 motor1En = 0;
melse 0:227158f56a11 93 }
melse 0:227158f56a11 94
melse 2:118efce95f3c 95 void SeeedStudioShieldBot::disable_left_motor() {
melse 0:227158f56a11 96 motor2En = 0;
melse 0:227158f56a11 97 }
melse 0:227158f56a11 98
melse 0:227158f56a11 99 // Give a value representative of the overall data received by the sensors...
melse 0:227158f56a11 100 // Output between +1 and -1.
melse 0:227158f56a11 101 // Positive is right, negative left...
melse 1:5c40f2a5e1ac 102 float SeeedStudioShieldBot::line_position() {
melse 0:227158f56a11 103 float output = 0;
melse 0:227158f56a11 104 if (rightSensor == 1) {
melse 0:227158f56a11 105 output += 0.5;
melse 0:227158f56a11 106 }
melse 0:227158f56a11 107 if (inRightSensor == 1) {
melse 1:5c40f2a5e1ac 108 output += 0.5;
melse 0:227158f56a11 109 }
melse 0:227158f56a11 110 if (leftSensor == 1) {
melse 0:227158f56a11 111 output -= 0.5;
melse 0:227158f56a11 112 }
melse 0:227158f56a11 113 if (inLeftSensor == 1) {
melse 1:5c40f2a5e1ac 114 output -= 0.5;
melse 0:227158f56a11 115 }
melse 0:227158f56a11 116
melse 0:227158f56a11 117 return output;
melse 0:227158f56a11 118 }
melse 0:227158f56a11 119
melse 0:227158f56a11 120 void SeeedStudioShieldBot::stopAll() {
melse 0:227158f56a11 121 motor1A = 0;
melse 0:227158f56a11 122 motor1B = 0;
melse 0:227158f56a11 123 motor2A = 0;
melse 0:227158f56a11 124 motor2B = 0;
melse 2:118efce95f3c 125 }
melse 2:118efce95f3c 126
melse 2:118efce95f3c 127 void SeeedStudioShieldBot::stop(int motor) {
melse 2:118efce95f3c 128 if (motor == 1) {
melse 2:118efce95f3c 129 // Stop motor 0...
melse 2:118efce95f3c 130 motor1A = 0;
melse 2:118efce95f3c 131 motor1B = 0;
melse 2:118efce95f3c 132 }
melse 2:118efce95f3c 133 else if (motor == 2) {
melse 2:118efce95f3c 134 motor2A = 0;
melse 2:118efce95f3c 135 motor2B = 0;
melse 2:118efce95f3c 136 }
melse 0:227158f56a11 137 }