na
Fork of SeeedShieldBot by
SeeedStudioShieldBot.cpp@1:5c40f2a5e1ac, 2013-07-15 (annotated)
- Committer:
- melse
- Date:
- Mon Jul 15 13:38:07 2013 +0000
- Revision:
- 1:5c40f2a5e1ac
- Parent:
- 0:227158f56a11
- Child:
- 2:118efce95f3c
Made the library more compatible with the m3pi lib
Who changed what in which revision?
User | Revision | Line number | New 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 | 0:227158f56a11 | 5 | SeeedStudioShieldBot::SeeedStudioShieldBot() : motor1A(PTA5), |
melse | 0:227158f56a11 | 6 | motor1B(PTC9), |
melse | 0:227158f56a11 | 7 | motor2A(PTA12), |
melse | 0:227158f56a11 | 8 | motor2B(PTD0), |
melse | 0:227158f56a11 | 9 | motor1En(PTC8), |
melse | 0:227158f56a11 | 10 | motor2En(PTD5), |
melse | 0:227158f56a11 | 11 | rightSensor(PTB0), |
melse | 0:227158f56a11 | 12 | inRightSensor(PTB1), |
melse | 0:227158f56a11 | 13 | centreSensor(PTB2), |
melse | 0:227158f56a11 | 14 | inLeftSensor(PTB3), |
melse | 0:227158f56a11 | 15 | leftSensor(PTA4) { |
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 | 0:227158f56a11 | 83 | void SeeedStudioShieldBot::enable_motor_a() { |
melse | 0:227158f56a11 | 84 | motor1En = 1; |
melse | 0:227158f56a11 | 85 | } |
melse | 0:227158f56a11 | 86 | |
melse | 0:227158f56a11 | 87 | void SeeedStudioShieldBot::enable_motor_b() { |
melse | 0:227158f56a11 | 88 | motor2En = 1; |
melse | 0:227158f56a11 | 89 | } |
melse | 0:227158f56a11 | 90 | |
melse | 0:227158f56a11 | 91 | void SeeedStudioShieldBot::disable_motor_a() { |
melse | 0:227158f56a11 | 92 | motor1En = 0; |
melse | 0:227158f56a11 | 93 | } |
melse | 0:227158f56a11 | 94 | |
melse | 0:227158f56a11 | 95 | void SeeedStudioShieldBot::disable_motor_b() { |
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 | 0:227158f56a11 | 125 | } |