V1 of the ParallaxRobotShield library
ParallaxRobotShield.cpp@0:2ab9f4a3187a, 2015-08-20 (annotated)
- Committer:
- omarrasheedk
- Date:
- Thu Aug 20 21:51:12 2015 +0000
- Revision:
- 0:2ab9f4a3187a
V1 of ParallaxRobotShield
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
omarrasheedk | 0:2ab9f4a3187a | 1 | #include "ParallaxRobotShield.h" |
omarrasheedk | 0:2ab9f4a3187a | 2 | #include "mbed.h" |
omarrasheedk | 0:2ab9f4a3187a | 3 | |
omarrasheedk | 0:2ab9f4a3187a | 4 | ParallaxRobotShield::ParallaxRobotShield(PinName leftPin, PinName rightPin) : leftServo(leftPin) , rightServo(rightPin) |
omarrasheedk | 0:2ab9f4a3187a | 5 | { |
omarrasheedk | 0:2ab9f4a3187a | 6 | leftPosition = 1450; |
omarrasheedk | 0:2ab9f4a3187a | 7 | rightPosition = 1550; |
omarrasheedk | 0:2ab9f4a3187a | 8 | } |
omarrasheedk | 0:2ab9f4a3187a | 9 | |
omarrasheedk | 0:2ab9f4a3187a | 10 | void ParallaxRobotShield::left_servo(int speed) |
omarrasheedk | 0:2ab9f4a3187a | 11 | { |
omarrasheedk | 0:2ab9f4a3187a | 12 | speed = 1496 - (speed * 2); |
omarrasheedk | 0:2ab9f4a3187a | 13 | leftPosition = speed; |
omarrasheedk | 0:2ab9f4a3187a | 14 | } |
omarrasheedk | 0:2ab9f4a3187a | 15 | |
omarrasheedk | 0:2ab9f4a3187a | 16 | |
omarrasheedk | 0:2ab9f4a3187a | 17 | void ParallaxRobotShield::right_servo(int speed) |
omarrasheedk | 0:2ab9f4a3187a | 18 | { |
omarrasheedk | 0:2ab9f4a3187a | 19 | speed = 1490 + (speed * 2); |
omarrasheedk | 0:2ab9f4a3187a | 20 | rightPosition = speed; |
omarrasheedk | 0:2ab9f4a3187a | 21 | } |
omarrasheedk | 0:2ab9f4a3187a | 22 | |
omarrasheedk | 0:2ab9f4a3187a | 23 | //void ParallaxRobotShield::StartPulse() |
omarrasheedk | 0:2ab9f4a3187a | 24 | //{ |
omarrasheedk | 0:2ab9f4a3187a | 25 | // ServoPin = 1; |
omarrasheedk | 0:2ab9f4a3187a | 26 | // PulseStop.attach_us(this, &ParallaxRobotShield::EndPulse, Position); |
omarrasheedk | 0:2ab9f4a3187a | 27 | //} |
omarrasheedk | 0:2ab9f4a3187a | 28 | |
omarrasheedk | 0:2ab9f4a3187a | 29 | void ParallaxRobotShield::disable_left_motor() // Just for internal functions |
omarrasheedk | 0:2ab9f4a3187a | 30 | { |
omarrasheedk | 0:2ab9f4a3187a | 31 | leftServo = 0; |
omarrasheedk | 0:2ab9f4a3187a | 32 | } |
omarrasheedk | 0:2ab9f4a3187a | 33 | //void ParallaxRobotShield::EndPulse() |
omarrasheedk | 0:2ab9f4a3187a | 34 | //{ |
omarrasheedk | 0:2ab9f4a3187a | 35 | // ServoPin = 0; |
omarrasheedk | 0:2ab9f4a3187a | 36 | //} |
omarrasheedk | 0:2ab9f4a3187a | 37 | void ParallaxRobotShield::disable_right_motor() // Just for internal functions |
omarrasheedk | 0:2ab9f4a3187a | 38 | { |
omarrasheedk | 0:2ab9f4a3187a | 39 | rightServo = 0; |
omarrasheedk | 0:2ab9f4a3187a | 40 | } |
omarrasheedk | 0:2ab9f4a3187a | 41 | |
omarrasheedk | 0:2ab9f4a3187a | 42 | |
omarrasheedk | 0:2ab9f4a3187a | 43 | void ParallaxRobotShield::enable_left_motor() |
omarrasheedk | 0:2ab9f4a3187a | 44 | { |
omarrasheedk | 0:2ab9f4a3187a | 45 | leftServo = 1; |
omarrasheedk | 0:2ab9f4a3187a | 46 | leftPulse.attach_us(this, &ParallaxRobotShield::enable_left_motor, 20000); // Period is set to 20000 us by default which is equal to 50 Hz |
omarrasheedk | 0:2ab9f4a3187a | 47 | leftPulseStop.attach_us(this, &ParallaxRobotShield::disable_left_motor, leftPosition); |
omarrasheedk | 0:2ab9f4a3187a | 48 | } |
omarrasheedk | 0:2ab9f4a3187a | 49 | |
omarrasheedk | 0:2ab9f4a3187a | 50 | |
omarrasheedk | 0:2ab9f4a3187a | 51 | void ParallaxRobotShield::enable_right_motor() |
omarrasheedk | 0:2ab9f4a3187a | 52 | { |
omarrasheedk | 0:2ab9f4a3187a | 53 | rightServo = 1; |
omarrasheedk | 0:2ab9f4a3187a | 54 | rightPulse.attach_us(this, &ParallaxRobotShield::enable_right_motor, 20000); // Period is set to 20000 us by default which is equal to 50 Hz |
omarrasheedk | 0:2ab9f4a3187a | 55 | rightPulseStop.attach_us(this, &ParallaxRobotShield::disable_right_motor, rightPosition); |
omarrasheedk | 0:2ab9f4a3187a | 56 | } |
omarrasheedk | 0:2ab9f4a3187a | 57 | |
omarrasheedk | 0:2ab9f4a3187a | 58 | |
omarrasheedk | 0:2ab9f4a3187a | 59 | void ParallaxRobotShield::turn_left(int speed) |
omarrasheedk | 0:2ab9f4a3187a | 60 | { |
omarrasheedk | 0:2ab9f4a3187a | 61 | stopRight(); |
omarrasheedk | 0:2ab9f4a3187a | 62 | int i; |
omarrasheedk | 0:2ab9f4a3187a | 63 | for(i = 0; i < 100; i++) |
omarrasheedk | 0:2ab9f4a3187a | 64 | { |
omarrasheedk | 0:2ab9f4a3187a | 65 | left_servo(speed); |
omarrasheedk | 0:2ab9f4a3187a | 66 | wait_ms(10); |
omarrasheedk | 0:2ab9f4a3187a | 67 | } |
omarrasheedk | 0:2ab9f4a3187a | 68 | enable_right_motor(); |
omarrasheedk | 0:2ab9f4a3187a | 69 | } |
omarrasheedk | 0:2ab9f4a3187a | 70 | |
omarrasheedk | 0:2ab9f4a3187a | 71 | |
omarrasheedk | 0:2ab9f4a3187a | 72 | void ParallaxRobotShield::turn_right(int speed) |
omarrasheedk | 0:2ab9f4a3187a | 73 | { |
omarrasheedk | 0:2ab9f4a3187a | 74 | stopLeft(); |
omarrasheedk | 0:2ab9f4a3187a | 75 | int i; |
omarrasheedk | 0:2ab9f4a3187a | 76 | for(i = 0; i < 100; i++) |
omarrasheedk | 0:2ab9f4a3187a | 77 | { |
omarrasheedk | 0:2ab9f4a3187a | 78 | right_servo(speed); |
omarrasheedk | 0:2ab9f4a3187a | 79 | wait_ms(10); |
omarrasheedk | 0:2ab9f4a3187a | 80 | } |
omarrasheedk | 0:2ab9f4a3187a | 81 | enable_left_motor(); |
omarrasheedk | 0:2ab9f4a3187a | 82 | } |
omarrasheedk | 0:2ab9f4a3187a | 83 | |
omarrasheedk | 0:2ab9f4a3187a | 84 | |
omarrasheedk | 0:2ab9f4a3187a | 85 | void ParallaxRobotShield::forward(int speed) |
omarrasheedk | 0:2ab9f4a3187a | 86 | { |
omarrasheedk | 0:2ab9f4a3187a | 87 | int i; |
omarrasheedk | 0:2ab9f4a3187a | 88 | for(i = 0; i < 100; i++) |
omarrasheedk | 0:2ab9f4a3187a | 89 | { |
omarrasheedk | 0:2ab9f4a3187a | 90 | left_servo(speed); |
omarrasheedk | 0:2ab9f4a3187a | 91 | right_servo(speed); |
omarrasheedk | 0:2ab9f4a3187a | 92 | wait_ms(20); |
omarrasheedk | 0:2ab9f4a3187a | 93 | } |
omarrasheedk | 0:2ab9f4a3187a | 94 | } |
omarrasheedk | 0:2ab9f4a3187a | 95 | |
omarrasheedk | 0:2ab9f4a3187a | 96 | |
omarrasheedk | 0:2ab9f4a3187a | 97 | void ParallaxRobotShield::backward(int speed) |
omarrasheedk | 0:2ab9f4a3187a | 98 | { |
omarrasheedk | 0:2ab9f4a3187a | 99 | forward(-speed); |
omarrasheedk | 0:2ab9f4a3187a | 100 | } |
omarrasheedk | 0:2ab9f4a3187a | 101 | |
omarrasheedk | 0:2ab9f4a3187a | 102 | |
omarrasheedk | 0:2ab9f4a3187a | 103 | void ParallaxRobotShield::left(int speed) |
omarrasheedk | 0:2ab9f4a3187a | 104 | { |
omarrasheedk | 0:2ab9f4a3187a | 105 | int i; |
omarrasheedk | 0:2ab9f4a3187a | 106 | for(i = 0; i < 100; i++) |
omarrasheedk | 0:2ab9f4a3187a | 107 | { |
omarrasheedk | 0:2ab9f4a3187a | 108 | left_servo(speed); |
omarrasheedk | 0:2ab9f4a3187a | 109 | right_servo(-speed); |
omarrasheedk | 0:2ab9f4a3187a | 110 | wait_ms(20); |
omarrasheedk | 0:2ab9f4a3187a | 111 | } |
omarrasheedk | 0:2ab9f4a3187a | 112 | } |
omarrasheedk | 0:2ab9f4a3187a | 113 | |
omarrasheedk | 0:2ab9f4a3187a | 114 | |
omarrasheedk | 0:2ab9f4a3187a | 115 | void ParallaxRobotShield::right(int speed) |
omarrasheedk | 0:2ab9f4a3187a | 116 | { |
omarrasheedk | 0:2ab9f4a3187a | 117 | left(-speed); |
omarrasheedk | 0:2ab9f4a3187a | 118 | } |
omarrasheedk | 0:2ab9f4a3187a | 119 | |
omarrasheedk | 0:2ab9f4a3187a | 120 | |
omarrasheedk | 0:2ab9f4a3187a | 121 | void ParallaxRobotShield::stopLeft() |
omarrasheedk | 0:2ab9f4a3187a | 122 | { |
omarrasheedk | 0:2ab9f4a3187a | 123 | leftPulse.detach(); |
omarrasheedk | 0:2ab9f4a3187a | 124 | } |
omarrasheedk | 0:2ab9f4a3187a | 125 | |
omarrasheedk | 0:2ab9f4a3187a | 126 | void ParallaxRobotShield::stopRight() |
omarrasheedk | 0:2ab9f4a3187a | 127 | { |
omarrasheedk | 0:2ab9f4a3187a | 128 | rightPulse.detach(); |
omarrasheedk | 0:2ab9f4a3187a | 129 | } |
omarrasheedk | 0:2ab9f4a3187a | 130 | |
omarrasheedk | 0:2ab9f4a3187a | 131 | |
omarrasheedk | 0:2ab9f4a3187a | 132 | void ParallaxRobotShield::stop(int motor) |
omarrasheedk | 0:2ab9f4a3187a | 133 | { |
omarrasheedk | 0:2ab9f4a3187a | 134 | if(motor == 1) |
omarrasheedk | 0:2ab9f4a3187a | 135 | stopRight(); |
omarrasheedk | 0:2ab9f4a3187a | 136 | else if(motor == 2) |
omarrasheedk | 0:2ab9f4a3187a | 137 | stopLeft(); |
omarrasheedk | 0:2ab9f4a3187a | 138 | } |
omarrasheedk | 0:2ab9f4a3187a | 139 | |
omarrasheedk | 0:2ab9f4a3187a | 140 | |
omarrasheedk | 0:2ab9f4a3187a | 141 | void ParallaxRobotShield::stopAll() |
omarrasheedk | 0:2ab9f4a3187a | 142 | { |
omarrasheedk | 0:2ab9f4a3187a | 143 | stopRight(); |
omarrasheedk | 0:2ab9f4a3187a | 144 | stopLeft(); |
omarrasheedk | 0:2ab9f4a3187a | 145 | } |
omarrasheedk | 0:2ab9f4a3187a | 146 | |
omarrasheedk | 0:2ab9f4a3187a | 147 | //void ParallaxRobotShield::Enable(int StartPos, int Period) |
omarrasheedk | 0:2ab9f4a3187a | 148 | //{ |
omarrasheedk | 0:2ab9f4a3187a | 149 | // leftPosition = StartPos; |
omarrasheedk | 0:2ab9f4a3187a | 150 | // leftPulse.attach_us(this, &ParallaxRobotShield::enable_left_motor, Period); |
omarrasheedk | 0:2ab9f4a3187a | 151 | //} |