Shieldbot.cpp

00001 /*
00002   Created to support the release of the Sheildbot from SeeedStudios
00003   http://www.seeedstudio.com/wiki/Shield_Bot
00004 
00005   Created by Jacob Rosenthal and Colin Ho, December 30, 2012.
00006   Released into the public domain.
00007 */
00008 
00009 // include core Wiring API
00010 #include "mbed.h"
00011 
00012 // include this library's description file
00013 #include "Shieldbot.h"
00014 
00015 #define SHIELDBOTDEBUG 0
00016 // for shieldbot Version 0.9 and 1.0
00017  
00018 /*#define right1 5          //define I1 interface
00019 #define speedPinRight 6     //enable right motor (bridge A)
00020 #define right2 7            //define I2 interface 
00021 #define left1 8             //define I3 interface 
00022 #define speedPinLeft 9      //enable motor B
00023 #define left2 10            //define I4 interface */
00024  
00025 // for shieldbot Version 1.1, we changed to the driver pins ,
00026 //if you shieldbot is ver1.0 or 0.9, please using the above pin mapping;
00027 #define right1 PTA0         //define I1 interface
00028 #define speedPinRight PTC4  //enable right motor (bridge A)
00029 #define right2 PTD2             //define I2 interface 
00030 #define left1 PTD3              //define I3 interface 
00031 #define speedPinLeft PTD0   //enable motor B
00032 #define left2 PTD1          //define I4 interface 
00033 
00034 #define finder1 PTB2        //define line finder S1
00035 #define finder2 PTB3        //define line finder S2
00036 #define finder3 PTB10           //define line finder S3
00037 #define finder4 PTB11           //define line finder S4
00038 #define finder5 PTC11           //define line finder S5
00039 
00040 int speedmotorA = 255; //define the speed of motorA
00041 int speedmotorB = 255; //define the speed of motorB
00042   
00043 
00044 
00045 Shieldbot::Shieldbot()
00046 {
00047   DigitalOut right1(PTA0);
00048   DigitalOut right2(PTD2);
00049   PwmOut speedPinRight(PTC4);
00050   speedPinRight.period(0.002f);
00051   DigitalOut left1(PTD3);
00052   DigitalOut left2(PTD1);
00053   PwmOut speedPinLeft(PTD0);
00054   speedPinLeft.period(0.002f);
00055   DigitalIn finder1(PTB2);
00056   DigitalIn finder2(PTB3);
00057   DigitalIn finder3(PTB10);
00058   DigitalIn finder4(PTB11);
00059   DigitalIn finder5(PTC11);
00060 }
00061 
00062 
00063 
00064 //sets same max speed to both motors
00065 void Shieldbot::setMaxSpeed(int both){
00066   setMaxLeftSpeed(both);
00067   setMaxRightSpeed(both);
00068 }
00069 
00070 void Shieldbot::setMaxSpeed(int left, int right){
00071   setMaxLeftSpeed(left);
00072   setMaxRightSpeed(right);
00073 }
00074 
00075 void Shieldbot::setMaxLeftSpeed(int left){
00076   speedmotorB=left;
00077 }
00078 
00079 void Shieldbot::setMaxRightSpeed(int right){
00080   speedmotorA=right;
00081 }
00082 
00083 int Shieldbot::readS1(){
00084   return finder1;
00085 }
00086 
00087 int Shieldbot::readS2(){
00088   return finder2;
00089 }
00090 
00091 int Shieldbot::readS3(){
00092   return finder3;
00093 }
00094 
00095 int Shieldbot::readS4(){
00096   return finder4;
00097 }
00098 
00099 int Shieldbot::readS5(){
00100   return finder5;
00101 }
00102 
00103 void Shieldbot::drive(char left, char right){
00104   rightMotor(right);
00105   leftMotor(left);
00106 }
00107 
00108 //char is 128 to 127
00109 //mag is the direction to spin the right motor
00110 //-128 backwards all the way
00111 //0 dont move
00112 //127 forwards all the way
00113 void Shieldbot::rightMotor(char mag){
00114   int actualSpeed = 0;
00115   DigitalOut right1(PTA0);
00116   DigitalOut right2(PTD2);
00117   PwmOut speedPinRight(PTC4);
00118   speedPinRight.period(0.002f);
00119   DigitalOut left1(PTD3);
00120   DigitalOut left2(PTD1);
00121   PwmOut speedPinLeft(PTD0);
00122   speedPinLeft.period(0.002f);
00123  
00124 
00125   if(mag >0){ //forward
00126     float ratio = (float)mag/128;
00127     actualSpeed = (int)(ratio*speedmotorA); //define your speed based on global speed
00128     #if SHIELDBOTDEBUG
00129       Serial.print("forward right: ");
00130       Serial.println(actualSpeed);
00131     #endif
00132     speedPinRight.write(actualSpeed);
00133     right1=0;
00134     right2=1;//turn right motor clockwise
00135   }else if(mag == 0){ //neutral
00136     #if SHIELDBOTDEBUG
00137       Serial.println("nuetral right");
00138     #endif
00139     stopRight();
00140   }else { //meaning backwards
00141     float ratio = (float)abs(mag)/128;
00142     actualSpeed = ratio*speedmotorA;
00143     #if SHIELDBOTDEBUG
00144       Serial.print("backward right: ");
00145       Serial.println(actualSpeed);
00146     #endif
00147     speedPinRight.write(actualSpeed);
00148     right1=1;
00149     right2=0;//turn right motor counterclockwise
00150   }
00151 }
00152 
00153 //TODO shouldnt these share one function and just input the differences?
00154 void Shieldbot::leftMotor(char mag){
00155   DigitalOut right1(PTA0);
00156   DigitalOut right2(PTD2);
00157   PwmOut speedPinRight(PTC4);
00158   speedPinRight.period(0.002f);
00159   DigitalOut left1(PTD3);
00160   DigitalOut left2(PTD1);
00161   PwmOut speedPinLeft(PTD0);
00162   speedPinLeft.period(0.002f);
00163 
00164   
00165   int actualSpeed = 0;  
00166   if(mag >0){ //forward
00167     float ratio = (float)(mag)/128;
00168     actualSpeed = (int)(ratio*speedmotorB); //define your speed based on global speed
00169     #if SHIELDBOTDEBUG
00170       Serial.print("forward left: ");
00171       Serial.println(actualSpeed);
00172     #endif
00173     speedPinLeft.write(actualSpeed);
00174     left1=0;
00175     left2=1;//turn left motor counter-clockwise
00176   }else if(mag == 0){ //neutral
00177     #if SHIELDBOTDEBUG
00178       Serial.println("nuetral left");
00179     #endif
00180     stopLeft();
00181   }else { //meaning backwards
00182     float ratio = (float)abs(mag)/128;
00183     actualSpeed = ratio*speedmotorB;
00184     #if SHIELDBOTDEBUG
00185       Serial.print("backward left: ");
00186       Serial.println(actualSpeed);
00187     #endif
00188     speedPinLeft.write(actualSpeed);
00189     left1=1;
00190     left2=0;//turn left motor counterclockwise
00191   }
00192 }
00193 
00194 void Shieldbot::forward(){
00195   leftMotor(127);
00196   rightMotor(127); 
00197 }
00198 
00199 void Shieldbot::backward(){
00200   leftMotor(-128);
00201   rightMotor(-128); 
00202 }
00203 
00204 void Shieldbot::stop(){
00205   stopLeft();
00206   stopRight();
00207 }
00208 
00209 void Shieldbot::stopLeft(){
00210   PwmOut speedPinLeft(PTD0);
00211   speedPinLeft.write(0);
00212 }
00213 
00214 void Shieldbot::stopRight(){
00215   PwmOut speedPinRight(PTC4);
00216   speedPinRight.write(0);
00217 }
00218 
00219 //may be dangerous to motor if reverse current into hbridge exceeds 2A
00220 void Shieldbot::fastStopLeft(){
00221   DigitalOut left1(PTD3);
00222   DigitalOut left2(PTD1);
00223   left1=1;
00224   left2=1;//turn DC Motor B move clockwise
00225 }
00226 
00227 //may be dangerous to motor if reverse current into hbridge exceeds 2A
00228 void Shieldbot::fastStopRight(){
00229   DigitalOut right1(PTA0);
00230   DigitalOut right2(PTD2);
00231   right1=1;
00232   right2=1;//turn DC Motor A move anticlockwise
00233 }
00234 
00235 //may be dangerous to motor if reverse current into hbridge exceeds 2A
00236 void Shieldbot::fastStop(){
00237     fastStopRight();
00238     fastStopLeft();
00239 }