robot unalmed
Revision 0:fc1b473bfa7f, committed 2015-08-11
- Comitter:
- jeserenom
- Date:
- Tue Aug 11 04:49:45 2015 +0000
- Commit message:
- robots proyecto unalmed
Changed in this revision
Shieldbot.cpp | Show annotated file Show diff for this revision Revisions of this file |
Shieldbot.h | Show annotated file Show diff for this revision Revisions of this file |
diff -r 000000000000 -r fc1b473bfa7f Shieldbot.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Shieldbot.cpp Tue Aug 11 04:49:45 2015 +0000 @@ -0,0 +1,239 @@ +/* + 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(); +}
diff -r 000000000000 -r fc1b473bfa7f Shieldbot.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Shieldbot.h Tue Aug 11 04:49:45 2015 +0000 @@ -0,0 +1,46 @@ +/* + 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. +*/ + +// ensure this library description is only included once +#ifndef Shieldbot_h +#define Shieldbot_h + + +// include types & constants of Wiring core API +#include "mbed.h" + +// library interface description +class Shieldbot +{ + // user-accessible "public" interface + public: + Shieldbot(); + int readS1(); + int readS2(); + int readS3(); + int readS4(); + int readS5(); + void setMaxSpeed(int); + void setMaxSpeed(int, int); + void setMaxLeftSpeed(int); + void setMaxRightSpeed(int); + void rightMotor(char); + void leftMotor(char); + void drive(char, char); + void forward(); + void backward(); + void stop(); + void stopRight(); + void stopLeft(); + void fastStopLeft(); + void fastStopRight(); + void fastStop(); + +}; + +#endif