This is a Shield Bot Library
Fork of Shield_Bot_v12 by
ShieldBot_v12.cpp
- Committer:
- Kevin_Lee
- Date:
- 2015-09-15
- Revision:
- 1:1d96ab1dbcc4
- Parent:
- 0:7535295d1670
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(); }