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();
}