Oscar Quiroz Ortega / Mbed 2 deprecated robotLibreria

na

Dependencies:   SeeedShieldBot mbed

Fork of robotLibreria by juan esteban sereno mesa

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?

UserRevisionLine numberNew 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 }