robot unalmed
Shieldbot.cpp@0:fc1b473bfa7f, 2015-08-11 (annotated)
- Committer:
- jeserenom
- Date:
- Tue Aug 11 04:49:45 2015 +0000
- Revision:
- 0:fc1b473bfa7f
robots proyecto unalmed
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jeserenom | 0:fc1b473bfa7f | 1 | /* |
jeserenom | 0:fc1b473bfa7f | 2 | Created to support the release of the Sheildbot from SeeedStudios |
jeserenom | 0:fc1b473bfa7f | 3 | http://www.seeedstudio.com/wiki/Shield_Bot |
jeserenom | 0:fc1b473bfa7f | 4 | |
jeserenom | 0:fc1b473bfa7f | 5 | Created by Jacob Rosenthal and Colin Ho, December 30, 2012. |
jeserenom | 0:fc1b473bfa7f | 6 | Released into the public domain. |
jeserenom | 0:fc1b473bfa7f | 7 | */ |
jeserenom | 0:fc1b473bfa7f | 8 | |
jeserenom | 0:fc1b473bfa7f | 9 | // include core Wiring API |
jeserenom | 0:fc1b473bfa7f | 10 | #include "mbed.h" |
jeserenom | 0:fc1b473bfa7f | 11 | |
jeserenom | 0:fc1b473bfa7f | 12 | // include this library's description file |
jeserenom | 0:fc1b473bfa7f | 13 | #include "Shieldbot.h" |
jeserenom | 0:fc1b473bfa7f | 14 | |
jeserenom | 0:fc1b473bfa7f | 15 | #define SHIELDBOTDEBUG 0 |
jeserenom | 0:fc1b473bfa7f | 16 | // for shieldbot Version 0.9 and 1.0 |
jeserenom | 0:fc1b473bfa7f | 17 | |
jeserenom | 0:fc1b473bfa7f | 18 | /*#define right1 5 //define I1 interface |
jeserenom | 0:fc1b473bfa7f | 19 | #define speedPinRight 6 //enable right motor (bridge A) |
jeserenom | 0:fc1b473bfa7f | 20 | #define right2 7 //define I2 interface |
jeserenom | 0:fc1b473bfa7f | 21 | #define left1 8 //define I3 interface |
jeserenom | 0:fc1b473bfa7f | 22 | #define speedPinLeft 9 //enable motor B |
jeserenom | 0:fc1b473bfa7f | 23 | #define left2 10 //define I4 interface */ |
jeserenom | 0:fc1b473bfa7f | 24 | |
jeserenom | 0:fc1b473bfa7f | 25 | // for shieldbot Version 1.1, we changed to the driver pins , |
jeserenom | 0:fc1b473bfa7f | 26 | //if you shieldbot is ver1.0 or 0.9, please using the above pin mapping; |
jeserenom | 0:fc1b473bfa7f | 27 | #define right1 PTA0 //define I1 interface |
jeserenom | 0:fc1b473bfa7f | 28 | #define speedPinRight PTC4 //enable right motor (bridge A) |
jeserenom | 0:fc1b473bfa7f | 29 | #define right2 PTD2 //define I2 interface |
jeserenom | 0:fc1b473bfa7f | 30 | #define left1 PTD3 //define I3 interface |
jeserenom | 0:fc1b473bfa7f | 31 | #define speedPinLeft PTD0 //enable motor B |
jeserenom | 0:fc1b473bfa7f | 32 | #define left2 PTD1 //define I4 interface |
jeserenom | 0:fc1b473bfa7f | 33 | |
jeserenom | 0:fc1b473bfa7f | 34 | #define finder1 PTB2 //define line finder S1 |
jeserenom | 0:fc1b473bfa7f | 35 | #define finder2 PTB3 //define line finder S2 |
jeserenom | 0:fc1b473bfa7f | 36 | #define finder3 PTB10 //define line finder S3 |
jeserenom | 0:fc1b473bfa7f | 37 | #define finder4 PTB11 //define line finder S4 |
jeserenom | 0:fc1b473bfa7f | 38 | #define finder5 PTC11 //define line finder S5 |
jeserenom | 0:fc1b473bfa7f | 39 | |
jeserenom | 0:fc1b473bfa7f | 40 | int speedmotorA = 255; //define the speed of motorA |
jeserenom | 0:fc1b473bfa7f | 41 | int speedmotorB = 255; //define the speed of motorB |
jeserenom | 0:fc1b473bfa7f | 42 | |
jeserenom | 0:fc1b473bfa7f | 43 | |
jeserenom | 0:fc1b473bfa7f | 44 | |
jeserenom | 0:fc1b473bfa7f | 45 | Shieldbot::Shieldbot() |
jeserenom | 0:fc1b473bfa7f | 46 | { |
jeserenom | 0:fc1b473bfa7f | 47 | DigitalOut right1(PTA0); |
jeserenom | 0:fc1b473bfa7f | 48 | DigitalOut right2(PTD2); |
jeserenom | 0:fc1b473bfa7f | 49 | PwmOut speedPinRight(PTC4); |
jeserenom | 0:fc1b473bfa7f | 50 | speedPinRight.period(0.002f); |
jeserenom | 0:fc1b473bfa7f | 51 | DigitalOut left1(PTD3); |
jeserenom | 0:fc1b473bfa7f | 52 | DigitalOut left2(PTD1); |
jeserenom | 0:fc1b473bfa7f | 53 | PwmOut speedPinLeft(PTD0); |
jeserenom | 0:fc1b473bfa7f | 54 | speedPinLeft.period(0.002f); |
jeserenom | 0:fc1b473bfa7f | 55 | DigitalIn finder1(PTB2); |
jeserenom | 0:fc1b473bfa7f | 56 | DigitalIn finder2(PTB3); |
jeserenom | 0:fc1b473bfa7f | 57 | DigitalIn finder3(PTB10); |
jeserenom | 0:fc1b473bfa7f | 58 | DigitalIn finder4(PTB11); |
jeserenom | 0:fc1b473bfa7f | 59 | DigitalIn finder5(PTC11); |
jeserenom | 0:fc1b473bfa7f | 60 | } |
jeserenom | 0:fc1b473bfa7f | 61 | |
jeserenom | 0:fc1b473bfa7f | 62 | |
jeserenom | 0:fc1b473bfa7f | 63 | |
jeserenom | 0:fc1b473bfa7f | 64 | //sets same max speed to both motors |
jeserenom | 0:fc1b473bfa7f | 65 | void Shieldbot::setMaxSpeed(int both){ |
jeserenom | 0:fc1b473bfa7f | 66 | setMaxLeftSpeed(both); |
jeserenom | 0:fc1b473bfa7f | 67 | setMaxRightSpeed(both); |
jeserenom | 0:fc1b473bfa7f | 68 | } |
jeserenom | 0:fc1b473bfa7f | 69 | |
jeserenom | 0:fc1b473bfa7f | 70 | void Shieldbot::setMaxSpeed(int left, int right){ |
jeserenom | 0:fc1b473bfa7f | 71 | setMaxLeftSpeed(left); |
jeserenom | 0:fc1b473bfa7f | 72 | setMaxRightSpeed(right); |
jeserenom | 0:fc1b473bfa7f | 73 | } |
jeserenom | 0:fc1b473bfa7f | 74 | |
jeserenom | 0:fc1b473bfa7f | 75 | void Shieldbot::setMaxLeftSpeed(int left){ |
jeserenom | 0:fc1b473bfa7f | 76 | speedmotorB=left; |
jeserenom | 0:fc1b473bfa7f | 77 | } |
jeserenom | 0:fc1b473bfa7f | 78 | |
jeserenom | 0:fc1b473bfa7f | 79 | void Shieldbot::setMaxRightSpeed(int right){ |
jeserenom | 0:fc1b473bfa7f | 80 | speedmotorA=right; |
jeserenom | 0:fc1b473bfa7f | 81 | } |
jeserenom | 0:fc1b473bfa7f | 82 | |
jeserenom | 0:fc1b473bfa7f | 83 | int Shieldbot::readS1(){ |
jeserenom | 0:fc1b473bfa7f | 84 | return finder1; |
jeserenom | 0:fc1b473bfa7f | 85 | } |
jeserenom | 0:fc1b473bfa7f | 86 | |
jeserenom | 0:fc1b473bfa7f | 87 | int Shieldbot::readS2(){ |
jeserenom | 0:fc1b473bfa7f | 88 | return finder2; |
jeserenom | 0:fc1b473bfa7f | 89 | } |
jeserenom | 0:fc1b473bfa7f | 90 | |
jeserenom | 0:fc1b473bfa7f | 91 | int Shieldbot::readS3(){ |
jeserenom | 0:fc1b473bfa7f | 92 | return finder3; |
jeserenom | 0:fc1b473bfa7f | 93 | } |
jeserenom | 0:fc1b473bfa7f | 94 | |
jeserenom | 0:fc1b473bfa7f | 95 | int Shieldbot::readS4(){ |
jeserenom | 0:fc1b473bfa7f | 96 | return finder4; |
jeserenom | 0:fc1b473bfa7f | 97 | } |
jeserenom | 0:fc1b473bfa7f | 98 | |
jeserenom | 0:fc1b473bfa7f | 99 | int Shieldbot::readS5(){ |
jeserenom | 0:fc1b473bfa7f | 100 | return finder5; |
jeserenom | 0:fc1b473bfa7f | 101 | } |
jeserenom | 0:fc1b473bfa7f | 102 | |
jeserenom | 0:fc1b473bfa7f | 103 | void Shieldbot::drive(char left, char right){ |
jeserenom | 0:fc1b473bfa7f | 104 | rightMotor(right); |
jeserenom | 0:fc1b473bfa7f | 105 | leftMotor(left); |
jeserenom | 0:fc1b473bfa7f | 106 | } |
jeserenom | 0:fc1b473bfa7f | 107 | |
jeserenom | 0:fc1b473bfa7f | 108 | //char is 128 to 127 |
jeserenom | 0:fc1b473bfa7f | 109 | //mag is the direction to spin the right motor |
jeserenom | 0:fc1b473bfa7f | 110 | //-128 backwards all the way |
jeserenom | 0:fc1b473bfa7f | 111 | //0 dont move |
jeserenom | 0:fc1b473bfa7f | 112 | //127 forwards all the way |
jeserenom | 0:fc1b473bfa7f | 113 | void Shieldbot::rightMotor(char mag){ |
jeserenom | 0:fc1b473bfa7f | 114 | int actualSpeed = 0; |
jeserenom | 0:fc1b473bfa7f | 115 | DigitalOut right1(PTA0); |
jeserenom | 0:fc1b473bfa7f | 116 | DigitalOut right2(PTD2); |
jeserenom | 0:fc1b473bfa7f | 117 | PwmOut speedPinRight(PTC4); |
jeserenom | 0:fc1b473bfa7f | 118 | speedPinRight.period(0.002f); |
jeserenom | 0:fc1b473bfa7f | 119 | DigitalOut left1(PTD3); |
jeserenom | 0:fc1b473bfa7f | 120 | DigitalOut left2(PTD1); |
jeserenom | 0:fc1b473bfa7f | 121 | PwmOut speedPinLeft(PTD0); |
jeserenom | 0:fc1b473bfa7f | 122 | speedPinLeft.period(0.002f); |
jeserenom | 0:fc1b473bfa7f | 123 | |
jeserenom | 0:fc1b473bfa7f | 124 | |
jeserenom | 0:fc1b473bfa7f | 125 | if(mag >0){ //forward |
jeserenom | 0:fc1b473bfa7f | 126 | float ratio = (float)mag/128; |
jeserenom | 0:fc1b473bfa7f | 127 | actualSpeed = (int)(ratio*speedmotorA); //define your speed based on global speed |
jeserenom | 0:fc1b473bfa7f | 128 | #if SHIELDBOTDEBUG |
jeserenom | 0:fc1b473bfa7f | 129 | Serial.print("forward right: "); |
jeserenom | 0:fc1b473bfa7f | 130 | Serial.println(actualSpeed); |
jeserenom | 0:fc1b473bfa7f | 131 | #endif |
jeserenom | 0:fc1b473bfa7f | 132 | speedPinRight.write(actualSpeed); |
jeserenom | 0:fc1b473bfa7f | 133 | right1=0; |
jeserenom | 0:fc1b473bfa7f | 134 | right2=1;//turn right motor clockwise |
jeserenom | 0:fc1b473bfa7f | 135 | }else if(mag == 0){ //neutral |
jeserenom | 0:fc1b473bfa7f | 136 | #if SHIELDBOTDEBUG |
jeserenom | 0:fc1b473bfa7f | 137 | Serial.println("nuetral right"); |
jeserenom | 0:fc1b473bfa7f | 138 | #endif |
jeserenom | 0:fc1b473bfa7f | 139 | stopRight(); |
jeserenom | 0:fc1b473bfa7f | 140 | }else { //meaning backwards |
jeserenom | 0:fc1b473bfa7f | 141 | float ratio = (float)abs(mag)/128; |
jeserenom | 0:fc1b473bfa7f | 142 | actualSpeed = ratio*speedmotorA; |
jeserenom | 0:fc1b473bfa7f | 143 | #if SHIELDBOTDEBUG |
jeserenom | 0:fc1b473bfa7f | 144 | Serial.print("backward right: "); |
jeserenom | 0:fc1b473bfa7f | 145 | Serial.println(actualSpeed); |
jeserenom | 0:fc1b473bfa7f | 146 | #endif |
jeserenom | 0:fc1b473bfa7f | 147 | speedPinRight.write(actualSpeed); |
jeserenom | 0:fc1b473bfa7f | 148 | right1=1; |
jeserenom | 0:fc1b473bfa7f | 149 | right2=0;//turn right motor counterclockwise |
jeserenom | 0:fc1b473bfa7f | 150 | } |
jeserenom | 0:fc1b473bfa7f | 151 | } |
jeserenom | 0:fc1b473bfa7f | 152 | |
jeserenom | 0:fc1b473bfa7f | 153 | //TODO shouldnt these share one function and just input the differences? |
jeserenom | 0:fc1b473bfa7f | 154 | void Shieldbot::leftMotor(char mag){ |
jeserenom | 0:fc1b473bfa7f | 155 | DigitalOut right1(PTA0); |
jeserenom | 0:fc1b473bfa7f | 156 | DigitalOut right2(PTD2); |
jeserenom | 0:fc1b473bfa7f | 157 | PwmOut speedPinRight(PTC4); |
jeserenom | 0:fc1b473bfa7f | 158 | speedPinRight.period(0.002f); |
jeserenom | 0:fc1b473bfa7f | 159 | DigitalOut left1(PTD3); |
jeserenom | 0:fc1b473bfa7f | 160 | DigitalOut left2(PTD1); |
jeserenom | 0:fc1b473bfa7f | 161 | PwmOut speedPinLeft(PTD0); |
jeserenom | 0:fc1b473bfa7f | 162 | speedPinLeft.period(0.002f); |
jeserenom | 0:fc1b473bfa7f | 163 | |
jeserenom | 0:fc1b473bfa7f | 164 | |
jeserenom | 0:fc1b473bfa7f | 165 | int actualSpeed = 0; |
jeserenom | 0:fc1b473bfa7f | 166 | if(mag >0){ //forward |
jeserenom | 0:fc1b473bfa7f | 167 | float ratio = (float)(mag)/128; |
jeserenom | 0:fc1b473bfa7f | 168 | actualSpeed = (int)(ratio*speedmotorB); //define your speed based on global speed |
jeserenom | 0:fc1b473bfa7f | 169 | #if SHIELDBOTDEBUG |
jeserenom | 0:fc1b473bfa7f | 170 | Serial.print("forward left: "); |
jeserenom | 0:fc1b473bfa7f | 171 | Serial.println(actualSpeed); |
jeserenom | 0:fc1b473bfa7f | 172 | #endif |
jeserenom | 0:fc1b473bfa7f | 173 | speedPinLeft.write(actualSpeed); |
jeserenom | 0:fc1b473bfa7f | 174 | left1=0; |
jeserenom | 0:fc1b473bfa7f | 175 | left2=1;//turn left motor counter-clockwise |
jeserenom | 0:fc1b473bfa7f | 176 | }else if(mag == 0){ //neutral |
jeserenom | 0:fc1b473bfa7f | 177 | #if SHIELDBOTDEBUG |
jeserenom | 0:fc1b473bfa7f | 178 | Serial.println("nuetral left"); |
jeserenom | 0:fc1b473bfa7f | 179 | #endif |
jeserenom | 0:fc1b473bfa7f | 180 | stopLeft(); |
jeserenom | 0:fc1b473bfa7f | 181 | }else { //meaning backwards |
jeserenom | 0:fc1b473bfa7f | 182 | float ratio = (float)abs(mag)/128; |
jeserenom | 0:fc1b473bfa7f | 183 | actualSpeed = ratio*speedmotorB; |
jeserenom | 0:fc1b473bfa7f | 184 | #if SHIELDBOTDEBUG |
jeserenom | 0:fc1b473bfa7f | 185 | Serial.print("backward left: "); |
jeserenom | 0:fc1b473bfa7f | 186 | Serial.println(actualSpeed); |
jeserenom | 0:fc1b473bfa7f | 187 | #endif |
jeserenom | 0:fc1b473bfa7f | 188 | speedPinLeft.write(actualSpeed); |
jeserenom | 0:fc1b473bfa7f | 189 | left1=1; |
jeserenom | 0:fc1b473bfa7f | 190 | left2=0;//turn left motor counterclockwise |
jeserenom | 0:fc1b473bfa7f | 191 | } |
jeserenom | 0:fc1b473bfa7f | 192 | } |
jeserenom | 0:fc1b473bfa7f | 193 | |
jeserenom | 0:fc1b473bfa7f | 194 | void Shieldbot::forward(){ |
jeserenom | 0:fc1b473bfa7f | 195 | leftMotor(127); |
jeserenom | 0:fc1b473bfa7f | 196 | rightMotor(127); |
jeserenom | 0:fc1b473bfa7f | 197 | } |
jeserenom | 0:fc1b473bfa7f | 198 | |
jeserenom | 0:fc1b473bfa7f | 199 | void Shieldbot::backward(){ |
jeserenom | 0:fc1b473bfa7f | 200 | leftMotor(-128); |
jeserenom | 0:fc1b473bfa7f | 201 | rightMotor(-128); |
jeserenom | 0:fc1b473bfa7f | 202 | } |
jeserenom | 0:fc1b473bfa7f | 203 | |
jeserenom | 0:fc1b473bfa7f | 204 | void Shieldbot::stop(){ |
jeserenom | 0:fc1b473bfa7f | 205 | stopLeft(); |
jeserenom | 0:fc1b473bfa7f | 206 | stopRight(); |
jeserenom | 0:fc1b473bfa7f | 207 | } |
jeserenom | 0:fc1b473bfa7f | 208 | |
jeserenom | 0:fc1b473bfa7f | 209 | void Shieldbot::stopLeft(){ |
jeserenom | 0:fc1b473bfa7f | 210 | PwmOut speedPinLeft(PTD0); |
jeserenom | 0:fc1b473bfa7f | 211 | speedPinLeft.write(0); |
jeserenom | 0:fc1b473bfa7f | 212 | } |
jeserenom | 0:fc1b473bfa7f | 213 | |
jeserenom | 0:fc1b473bfa7f | 214 | void Shieldbot::stopRight(){ |
jeserenom | 0:fc1b473bfa7f | 215 | PwmOut speedPinRight(PTC4); |
jeserenom | 0:fc1b473bfa7f | 216 | speedPinRight.write(0); |
jeserenom | 0:fc1b473bfa7f | 217 | } |
jeserenom | 0:fc1b473bfa7f | 218 | |
jeserenom | 0:fc1b473bfa7f | 219 | //may be dangerous to motor if reverse current into hbridge exceeds 2A |
jeserenom | 0:fc1b473bfa7f | 220 | void Shieldbot::fastStopLeft(){ |
jeserenom | 0:fc1b473bfa7f | 221 | DigitalOut left1(PTD3); |
jeserenom | 0:fc1b473bfa7f | 222 | DigitalOut left2(PTD1); |
jeserenom | 0:fc1b473bfa7f | 223 | left1=1; |
jeserenom | 0:fc1b473bfa7f | 224 | left2=1;//turn DC Motor B move clockwise |
jeserenom | 0:fc1b473bfa7f | 225 | } |
jeserenom | 0:fc1b473bfa7f | 226 | |
jeserenom | 0:fc1b473bfa7f | 227 | //may be dangerous to motor if reverse current into hbridge exceeds 2A |
jeserenom | 0:fc1b473bfa7f | 228 | void Shieldbot::fastStopRight(){ |
jeserenom | 0:fc1b473bfa7f | 229 | DigitalOut right1(PTA0); |
jeserenom | 0:fc1b473bfa7f | 230 | DigitalOut right2(PTD2); |
jeserenom | 0:fc1b473bfa7f | 231 | right1=1; |
jeserenom | 0:fc1b473bfa7f | 232 | right2=1;//turn DC Motor A move anticlockwise |
jeserenom | 0:fc1b473bfa7f | 233 | } |
jeserenom | 0:fc1b473bfa7f | 234 | |
jeserenom | 0:fc1b473bfa7f | 235 | //may be dangerous to motor if reverse current into hbridge exceeds 2A |
jeserenom | 0:fc1b473bfa7f | 236 | void Shieldbot::fastStop(){ |
jeserenom | 0:fc1b473bfa7f | 237 | fastStopRight(); |
jeserenom | 0:fc1b473bfa7f | 238 | fastStopLeft(); |
jeserenom | 0:fc1b473bfa7f | 239 | } |