ShieldBot_v12.cpp

00001 /*    
00002  * ShieldBot_v12.cpp  
00003  * A library for ShieldBot
00004  *   
00005  * Copyright (c) 2015 seeed technology inc.  
00006  * Author      : Jiankai.li
00007  * Create Time:  Sep  2015
00008  * Change Log : 
00009  *
00010  * The MIT License (MIT)
00011  *
00012  * Permission is hereby granted, free of charge, to any person obtaining a copy
00013  * of this software and associated documentation files (the "Software"), to deal
00014  * in the Software without restriction, including without limitation the rights
00015  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00016  * copies of the Software, and to permit persons to whom the Software is
00017  * furnished to do so, subject to the following conditions:
00018  * 
00019  * The above copyright notice and this permission notice shall be included in
00020  * all copies or substantial portions of the Software.
00021  * 
00022  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00023  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00024  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00025  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00026  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00027  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00028  * THE SOFTWARE.
00029  */
00030 
00031 #include "mbed.h"
00032 #include "ShieldBot_v12.h"
00033 #include "nrf51.h"
00034 #include "nrf51_bitfields.h"
00035 /*
00036   Created to support the release of the Sheildbot from SeeedStudios
00037   http://www.seeedstudio.com/wiki/Shield_Bot
00038 
00039   Created by Jacob Rosenthal and Colin Ho, December 30, 2012.
00040   Released into the public domain.
00041 */
00042 
00043 
00044 #define SHIELDBOTDEBUG 0
00045 
00046 #define right1        p18            //define I1 interface
00047 #define speedPinRight p23     //enable right motor (bridge A)
00048 #define right2        p25     //define I2 interface 
00049 #define left1         p28     //define I3 interface 
00050 #define speedPinLeft  p24     //enable motor B
00051 #define left2         p29             //define I4 interface 
00052 
00053 #define HIGH          1
00054 #define LOW           0
00055 
00056 PwmOut MotorRight(speedPinRight);
00057 DigitalIn finder1(p1); // define line finder S1
00058 DigitalIn finder2(p2); // define line finder S2
00059 DigitalIn finder3(p3); // define line finder S3
00060 DigitalIn finder4(p4); // define line finder S4
00061 DigitalIn finder5(p11);  // define line finder S5
00062 
00063 DigitalOut Right1(right1);
00064 DigitalOut Right2(right2);
00065 DigitalOut Left1(left1);
00066 DigitalOut Left2(left2);
00067 
00068 PwmOut     SpeedRight(speedPinRight);
00069 PwmOut     SpeedLeft(speedPinLeft);
00070 
00071 int speedmotorA = 255; //define the speed of motorA
00072 int speedmotorB = 255; //define the speed of motorB
00073 
00074 Shieldbot::Shieldbot()
00075 {
00076     SpeedRight.period_us(100);
00077     SpeedLeft.period_us(100);
00078 }
00079 
00080 //sets same max speed to both motors
00081 void Shieldbot::setMaxSpeed(int both){
00082   setMaxLeftSpeed(both);
00083   setMaxRightSpeed(both);
00084 }
00085 
00086 void Shieldbot::setMaxSpeed(int left, int right){
00087   setMaxLeftSpeed(left);
00088   setMaxRightSpeed(right);
00089 }
00090 
00091 void Shieldbot::setMaxLeftSpeed(int left){
00092   speedmotorB=left;
00093 }
00094 
00095 void Shieldbot::setMaxRightSpeed(int right){
00096   speedmotorA=right;
00097 }
00098 
00099 int Shieldbot::readS1(){
00100   return finder1.read();
00101 }
00102 
00103 int Shieldbot::readS2(){
00104   return finder2.read();
00105 }
00106 
00107 int Shieldbot::readS3(){
00108   return finder3.read();
00109 }
00110 
00111 int Shieldbot::readS4(){
00112   return finder4.read();
00113 }
00114 
00115 int Shieldbot::readS5(){
00116   return finder5.read();
00117 }
00118 
00119 void Shieldbot::drive(signed char left,signed char right){
00120   rightMotor(right);
00121   leftMotor(left);
00122 }
00123 
00124 //char is 128 to 127
00125 //mag is the direction to spin the right motor
00126 //-128 backwards all the way
00127 //0 dont move
00128 //127 forwards all the way
00129 void Shieldbot::rightMotor(signed char mag){
00130   int actualSpeed = 0;  
00131   if(mag >0){ //forward
00132     float ratio = (float)mag/128;
00133     actualSpeed = (int)(ratio*speedmotorA); //define your speed based on global speed
00134     #if SHIELDBOTDEBUG
00135       Serial.print("forward right: ");
00136       Serial.println(actualSpeed);
00137     #endif
00138     SpeedRight.write((float)(actualSpeed/255.0));
00139     // analogWrite(speedPinRight,actualSpeed);
00140     Right1 = HIGH;
00141     Right2 = LOW;
00142     // digitalWrite(right1,HIGH);
00143     // digitalWrite(right2,LOW);//turn right motor clockwise
00144   }else if(mag == 0){ //neutral
00145     #if SHIELDBOTDEBUG
00146       Serial.println("nuetral right");
00147     #endif
00148     stopRight();
00149   }else { //meaning backwards
00150     float ratio = (float)abs(mag)/128;
00151     actualSpeed = ratio*speedmotorA;
00152     #if SHIELDBOTDEBUG
00153       Serial.print("backward right: ");
00154       Serial.println(actualSpeed);
00155     #endif
00156     SpeedRight.write((float)(actualSpeed/255.0));
00157     //analogWrite(speedPinRight,actualSpeed);
00158     Right1 = LOW;
00159     Right2 = HIGH;
00160     // digitalWrite(right1,LOW);
00161     // digitalWrite(right2,HIGH);//turn right motor counterclockwise
00162   }
00163 }
00164 
00165 //TODO shouldnt these share one function and just input the differences?
00166 void Shieldbot::leftMotor(signed char mag){
00167   int actualSpeed = 0;  
00168   if(mag >0){ //forward
00169     float ratio = (float)(mag)/128;
00170     actualSpeed = (int)(ratio*speedmotorB); //define your speed based on global speed
00171     #if SHIELDBOTDEBUG
00172       Serial.print("forward left: ");
00173       Serial.println(actualSpeed);
00174     #endif
00175     SpeedLeft.write((float)(actualSpeed/255.0));
00176     Left1 = HIGH;
00177     Left2 = LOW;
00178     // analogWrite(speedPinLeft,actualSpeed);
00179     // digitalWrite(left1,HIGH);
00180     // digitalWrite(left2,LOW);//turn left motor counter-clockwise
00181   }else if(mag == 0){ //neutral
00182     #if SHIELDBOTDEBUG
00183       Serial.println("nuetral left");
00184     #endif
00185     stopLeft();
00186   }else { //meaning backwards
00187     float ratio = (float)abs(mag)/128;
00188     actualSpeed = ratio*speedmotorB;
00189     #if SHIELDBOTDEBUG
00190       Serial.print("backward left: ");
00191       Serial.println(actualSpeed);
00192     #endif
00193     SpeedLeft.write((float)(actualSpeed/255.0));
00194     Left1 = LOW;
00195     Left2 = HIGH;
00196     // analogWrite(speedPinLeft,actualSpeed);
00197     // digitalWrite(left1,LOW);
00198     // digitalWrite(left2,HIGH);//turn left motor counterclockwise
00199   }
00200 }
00201 
00202 void Shieldbot::forward(){
00203   leftMotor(127);
00204   rightMotor(127); 
00205 }
00206 
00207 void Shieldbot::backward(){
00208   leftMotor(-128);
00209   rightMotor(-128); 
00210 }
00211 
00212 void Shieldbot::stop(){
00213   stopLeft();
00214   stopRight();
00215 }
00216 
00217 void Shieldbot::stopLeft(){
00218   // analogWrite(speedPinLeft,0);
00219   SpeedLeft.write(0);
00220 }
00221 
00222 void Shieldbot::stopRight(){
00223   //analogWrite(speedPinRight,0);
00224   SpeedRight.write(0);
00225 }
00226 
00227 //may be dangerous to motor if reverse current into hbridge exceeds 2A
00228 void Shieldbot::fastStopLeft(){
00229     Left1 = LOW;
00230     Left2 = LOW;
00231   // digitalWrite(left1,LOW);
00232   // digitalWrite(left2,LOW);//turn DC Motor B move clockwise
00233 }
00234 
00235 //may be dangerous to motor if reverse current into hbridge exceeds 2A
00236 void Shieldbot::fastStopRight(){
00237     Right1 = LOW;
00238     Right2 = LOW;
00239   // digitalWrite(right1,LOW);
00240   // digitalWrite(right2,LOW);//turn DC Motor A move anticlockwise
00241 }
00242 
00243 //may be dangerous to motor if reverse current into hbridge exceeds 2A
00244 void Shieldbot::fastStop(){
00245     fastStopRight();
00246     fastStopLeft();
00247 }