robot unalmed
Shieldbot.cpp
- Committer:
- jeserenom
- Date:
- 2015-08-11
- Revision:
- 0:fc1b473bfa7f
File content as of revision 0:fc1b473bfa7f:
/* Created to support the release of the Sheildbot from SeeedStudios http://www.seeedstudio.com/wiki/Shield_Bot Created by Jacob Rosenthal and Colin Ho, December 30, 2012. Released into the public domain. */ // include core Wiring API #include "mbed.h" // include this library's description file #include "Shieldbot.h" #define SHIELDBOTDEBUG 0 // for shieldbot Version 0.9 and 1.0 /*#define right1 5 //define I1 interface #define speedPinRight 6 //enable right motor (bridge A) #define right2 7 //define I2 interface #define left1 8 //define I3 interface #define speedPinLeft 9 //enable motor B #define left2 10 //define I4 interface */ // for shieldbot Version 1.1, we changed to the driver pins , //if you shieldbot is ver1.0 or 0.9, please using the above pin mapping; #define right1 PTA0 //define I1 interface #define speedPinRight PTC4 //enable right motor (bridge A) #define right2 PTD2 //define I2 interface #define left1 PTD3 //define I3 interface #define speedPinLeft PTD0 //enable motor B #define left2 PTD1 //define I4 interface #define finder1 PTB2 //define line finder S1 #define finder2 PTB3 //define line finder S2 #define finder3 PTB10 //define line finder S3 #define finder4 PTB11 //define line finder S4 #define finder5 PTC11 //define line finder S5 int speedmotorA = 255; //define the speed of motorA int speedmotorB = 255; //define the speed of motorB Shieldbot::Shieldbot() { DigitalOut right1(PTA0); DigitalOut right2(PTD2); PwmOut speedPinRight(PTC4); speedPinRight.period(0.002f); DigitalOut left1(PTD3); DigitalOut left2(PTD1); PwmOut speedPinLeft(PTD0); speedPinLeft.period(0.002f); DigitalIn finder1(PTB2); DigitalIn finder2(PTB3); DigitalIn finder3(PTB10); DigitalIn finder4(PTB11); DigitalIn finder5(PTC11); } //sets same max speed to both motors void Shieldbot::setMaxSpeed(int both){ setMaxLeftSpeed(both); setMaxRightSpeed(both); } void Shieldbot::setMaxSpeed(int left, int right){ setMaxLeftSpeed(left); setMaxRightSpeed(right); } void Shieldbot::setMaxLeftSpeed(int left){ speedmotorB=left; } void Shieldbot::setMaxRightSpeed(int right){ speedmotorA=right; } int Shieldbot::readS1(){ return finder1; } int Shieldbot::readS2(){ return finder2; } int Shieldbot::readS3(){ return finder3; } int Shieldbot::readS4(){ return finder4; } int Shieldbot::readS5(){ return finder5; } void Shieldbot::drive(char left, char right){ rightMotor(right); leftMotor(left); } //char is 128 to 127 //mag is the direction to spin the right motor //-128 backwards all the way //0 dont move //127 forwards all the way void Shieldbot::rightMotor(char mag){ int actualSpeed = 0; DigitalOut right1(PTA0); DigitalOut right2(PTD2); PwmOut speedPinRight(PTC4); speedPinRight.period(0.002f); DigitalOut left1(PTD3); DigitalOut left2(PTD1); PwmOut speedPinLeft(PTD0); speedPinLeft.period(0.002f); if(mag >0){ //forward float ratio = (float)mag/128; actualSpeed = (int)(ratio*speedmotorA); //define your speed based on global speed #if SHIELDBOTDEBUG Serial.print("forward right: "); Serial.println(actualSpeed); #endif speedPinRight.write(actualSpeed); right1=0; right2=1;//turn right motor clockwise }else if(mag == 0){ //neutral #if SHIELDBOTDEBUG Serial.println("nuetral right"); #endif stopRight(); }else { //meaning backwards float ratio = (float)abs(mag)/128; actualSpeed = ratio*speedmotorA; #if SHIELDBOTDEBUG Serial.print("backward right: "); Serial.println(actualSpeed); #endif speedPinRight.write(actualSpeed); right1=1; right2=0;//turn right motor counterclockwise } } //TODO shouldnt these share one function and just input the differences? void Shieldbot::leftMotor(char mag){ DigitalOut right1(PTA0); DigitalOut right2(PTD2); PwmOut speedPinRight(PTC4); speedPinRight.period(0.002f); DigitalOut left1(PTD3); DigitalOut left2(PTD1); PwmOut speedPinLeft(PTD0); speedPinLeft.period(0.002f); int actualSpeed = 0; if(mag >0){ //forward float ratio = (float)(mag)/128; actualSpeed = (int)(ratio*speedmotorB); //define your speed based on global speed #if SHIELDBOTDEBUG Serial.print("forward left: "); Serial.println(actualSpeed); #endif speedPinLeft.write(actualSpeed); left1=0; left2=1;//turn left motor counter-clockwise }else if(mag == 0){ //neutral #if SHIELDBOTDEBUG Serial.println("nuetral left"); #endif stopLeft(); }else { //meaning backwards float ratio = (float)abs(mag)/128; actualSpeed = ratio*speedmotorB; #if SHIELDBOTDEBUG Serial.print("backward left: "); Serial.println(actualSpeed); #endif speedPinLeft.write(actualSpeed); left1=1; left2=0;//turn left motor counterclockwise } } void Shieldbot::forward(){ leftMotor(127); rightMotor(127); } void Shieldbot::backward(){ leftMotor(-128); rightMotor(-128); } void Shieldbot::stop(){ stopLeft(); stopRight(); } void Shieldbot::stopLeft(){ PwmOut speedPinLeft(PTD0); speedPinLeft.write(0); } void Shieldbot::stopRight(){ PwmOut speedPinRight(PTC4); speedPinRight.write(0); } //may be dangerous to motor if reverse current into hbridge exceeds 2A void Shieldbot::fastStopLeft(){ DigitalOut left1(PTD3); DigitalOut left2(PTD1); left1=1; left2=1;//turn DC Motor B move clockwise } //may be dangerous to motor if reverse current into hbridge exceeds 2A void Shieldbot::fastStopRight(){ DigitalOut right1(PTA0); DigitalOut right2(PTD2); right1=1; right2=1;//turn DC Motor A move anticlockwise } //may be dangerous to motor if reverse current into hbridge exceeds 2A void Shieldbot::fastStop(){ fastStopRight(); fastStopLeft(); }