File content as of revision 1:1d96ab1dbcc4:

/*    
 * ShieldBot_v12.cpp  
 * A library for ShieldBot
 *   
 * Copyright (c) 2015 seeed technology inc.  
 * Author      : Jiankai.li
 * Create Time:  Sep  2015
 * Change Log : 
 *
 * The MIT License (MIT)
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 * 
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 * 
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

#include "mbed.h"
#include "ShieldBot_v12.h"
#include "nrf51.h"
#include "nrf51_bitfields.h"
/*
  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.
*/


#define SHIELDBOTDEBUG 0

#define right1        p18            //define I1 interface
#define speedPinRight p23     //enable right motor (bridge A)
#define right2        p25     //define I2 interface 
#define left1         p28     //define I3 interface 
#define speedPinLeft  p24     //enable motor B
#define left2         p29             //define I4 interface 

#define HIGH          1
#define LOW           0

PwmOut MotorRight(speedPinRight);
DigitalIn finder1(p1); // define line finder S1
DigitalIn finder2(p2); // define line finder S2
DigitalIn finder3(p3); // define line finder S3
DigitalIn finder4(p4); // define line finder S4
DigitalIn finder5(p11);  // define line finder S5

DigitalOut Right1(right1);
DigitalOut Right2(right2);
DigitalOut Left1(left1);
DigitalOut Left2(left2);

PwmOut     SpeedRight(speedPinRight);
PwmOut     SpeedLeft(speedPinLeft);

int speedmotorA = 255; //define the speed of motorA
int speedmotorB = 255; //define the speed of motorB

Shieldbot::Shieldbot()
{
    SpeedRight.period_us(100);
    SpeedLeft.period_us(100);
}

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

int Shieldbot::readS2(){
  return finder2.read();
}

int Shieldbot::readS3(){
  return finder3.read();
}

int Shieldbot::readS4(){
  return finder4.read();
}

int Shieldbot::readS5(){
  return finder5.read();
}

void Shieldbot::drive(signed char left,signed 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(signed char mag){
  int actualSpeed = 0;  
  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
    SpeedRight.write((float)(actualSpeed/255.0));
    // analogWrite(speedPinRight,actualSpeed);
    Right1 = HIGH;
    Right2 = LOW;
    // digitalWrite(right1,HIGH);
    // digitalWrite(right2,LOW);//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
    SpeedRight.write((float)(actualSpeed/255.0));
    //analogWrite(speedPinRight,actualSpeed);
    Right1 = LOW;
    Right2 = HIGH;
    // digitalWrite(right1,LOW);
    // digitalWrite(right2,HIGH);//turn right motor counterclockwise
  }
}

//TODO shouldnt these share one function and just input the differences?
void Shieldbot::leftMotor(signed char mag){
  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
    SpeedLeft.write((float)(actualSpeed/255.0));
    Left1 = HIGH;
    Left2 = LOW;
    // analogWrite(speedPinLeft,actualSpeed);
    // digitalWrite(left1,HIGH);
    // digitalWrite(left2,LOW);//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
    SpeedLeft.write((float)(actualSpeed/255.0));
    Left1 = LOW;
    Left2 = HIGH;
    // analogWrite(speedPinLeft,actualSpeed);
    // digitalWrite(left1,LOW);
    // digitalWrite(left2,HIGH);//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(){
  // analogWrite(speedPinLeft,0);
  SpeedLeft.write(0);
}

void Shieldbot::stopRight(){
  //analogWrite(speedPinRight,0);
  SpeedRight.write(0);
}

//may be dangerous to motor if reverse current into hbridge exceeds 2A
void Shieldbot::fastStopLeft(){
    Left1 = LOW;
    Left2 = LOW;
  // digitalWrite(left1,LOW);
  // digitalWrite(left2,LOW);//turn DC Motor B move clockwise
}

//may be dangerous to motor if reverse current into hbridge exceeds 2A
void Shieldbot::fastStopRight(){
    Right1 = LOW;
    Right2 = LOW;
  // digitalWrite(right1,LOW);
  // digitalWrite(right2,LOW);//turn DC Motor A move anticlockwise
}

//may be dangerous to motor if reverse current into hbridge exceeds 2A
void Shieldbot::fastStop(){
    fastStopRight();
    fastStopLeft();
}