Kevin Lee / ShieldBot_v12

This is a Shield Bot Library

Fork of Shield_Bot_v12 by Kevin Lee

ShieldBot_v12.cpp@1:1d96ab1dbcc4, 2015-09-15 (annotated)

Committer:
Kevin_Lee
Date:
Tue Sep 15 13:40:55 2015 +0000
Revision:
1:1d96ab1dbcc4
Parent:
0:7535295d1670 
This is a library for ShieldBot

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Kevin_Lee 1:1d96ab1dbcc4 1 /*
Kevin_Lee 1:1d96ab1dbcc4 2 * ShieldBot_v12.cpp
Kevin_Lee 1:1d96ab1dbcc4 3 * A library for ShieldBot
Kevin_Lee 1:1d96ab1dbcc4 4 *
Kevin_Lee 1:1d96ab1dbcc4 5 * Copyright (c) 2015 seeed technology inc.
Kevin_Lee 1:1d96ab1dbcc4 6 * Author : Jiankai.li
Kevin_Lee 1:1d96ab1dbcc4 7 * Create Time: Sep 2015
Kevin_Lee 1:1d96ab1dbcc4 8 * Change Log :
Kevin_Lee 0:7535295d1670 9 *
Kevin_Lee 0:7535295d1670 10 * The MIT License (MIT)
Kevin_Lee 0:7535295d1670 11 *
Kevin_Lee 0:7535295d1670 12 * Permission is hereby granted, free of charge, to any person obtaining a copy
Kevin_Lee 0:7535295d1670 13 * of this software and associated documentation files (the "Software"), to deal
Kevin_Lee 0:7535295d1670 14 * in the Software without restriction, including without limitation the rights
Kevin_Lee 0:7535295d1670 15 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
Kevin_Lee 0:7535295d1670 16 * copies of the Software, and to permit persons to whom the Software is
Kevin_Lee 0:7535295d1670 17 * furnished to do so, subject to the following conditions:
Kevin_Lee 1:1d96ab1dbcc4 18 *
Kevin_Lee 0:7535295d1670 19 * The above copyright notice and this permission notice shall be included in
Kevin_Lee 0:7535295d1670 20 * all copies or substantial portions of the Software.
Kevin_Lee 1:1d96ab1dbcc4 21 *
Kevin_Lee 0:7535295d1670 22 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
Kevin_Lee 0:7535295d1670 23 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
Kevin_Lee 0:7535295d1670 24 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
Kevin_Lee 0:7535295d1670 25 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
Kevin_Lee 0:7535295d1670 26 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
Kevin_Lee 0:7535295d1670 27 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
Kevin_Lee 0:7535295d1670 28 * THE SOFTWARE.
Kevin_Lee 0:7535295d1670 29 */
Kevin_Lee 0:7535295d1670 30
Kevin_Lee 1:1d96ab1dbcc4 31 #include "mbed.h"
Kevin_Lee 0:7535295d1670 32 #include "ShieldBot_v12.h"
Kevin_Lee 0:7535295d1670 33 #include "nrf51.h"
Kevin_Lee 0:7535295d1670 34 #include "nrf51_bitfields.h"
Kevin_Lee 1:1d96ab1dbcc4 35 /*
Kevin_Lee 1:1d96ab1dbcc4 36 Created to support the release of the Sheildbot from SeeedStudios
Kevin_Lee 1:1d96ab1dbcc4 37 http://www.seeedstudio.com/wiki/Shield_Bot
Kevin_Lee 0:7535295d1670 38
Kevin_Lee 1:1d96ab1dbcc4 39 Created by Jacob Rosenthal and Colin Ho, December 30, 2012.
Kevin_Lee 1:1d96ab1dbcc4 40 Released into the public domain.
Kevin_Lee 1:1d96ab1dbcc4 41 */
Kevin_Lee 1:1d96ab1dbcc4 42
Kevin_Lee 1:1d96ab1dbcc4 43
Kevin_Lee 1:1d96ab1dbcc4 44 #define SHIELDBOTDEBUG 0
Kevin_Lee 1:1d96ab1dbcc4 45
Kevin_Lee 1:1d96ab1dbcc4 46 #define right1 p18 //define I1 interface
Kevin_Lee 1:1d96ab1dbcc4 47 #define speedPinRight p23 //enable right motor (bridge A)
Kevin_Lee 1:1d96ab1dbcc4 48 #define right2 p25 //define I2 interface
Kevin_Lee 1:1d96ab1dbcc4 49 #define left1 p28 //define I3 interface
Kevin_Lee 1:1d96ab1dbcc4 50 #define speedPinLeft p24 //enable motor B
Kevin_Lee 1:1d96ab1dbcc4 51 #define left2 p29 //define I4 interface
Kevin_Lee 1:1d96ab1dbcc4 52
Kevin_Lee 1:1d96ab1dbcc4 53 #define HIGH 1
Kevin_Lee 1:1d96ab1dbcc4 54 #define LOW 0
Kevin_Lee 0:7535295d1670 55
Kevin_Lee 1:1d96ab1dbcc4 56 PwmOut MotorRight(speedPinRight);
Kevin_Lee 1:1d96ab1dbcc4 57 DigitalIn finder1(p1); // define line finder S1
Kevin_Lee 1:1d96ab1dbcc4 58 DigitalIn finder2(p2); // define line finder S2
Kevin_Lee 1:1d96ab1dbcc4 59 DigitalIn finder3(p3); // define line finder S3
Kevin_Lee 1:1d96ab1dbcc4 60 DigitalIn finder4(p4); // define line finder S4
Kevin_Lee 1:1d96ab1dbcc4 61 DigitalIn finder5(p11); // define line finder S5
Kevin_Lee 0:7535295d1670 62
Kevin_Lee 1:1d96ab1dbcc4 63 DigitalOut Right1(right1);
Kevin_Lee 1:1d96ab1dbcc4 64 DigitalOut Right2(right2);
Kevin_Lee 1:1d96ab1dbcc4 65 DigitalOut Left1(left1);
Kevin_Lee 1:1d96ab1dbcc4 66 DigitalOut Left2(left2);
Kevin_Lee 1:1d96ab1dbcc4 67
Kevin_Lee 1:1d96ab1dbcc4 68 PwmOut SpeedRight(speedPinRight);
Kevin_Lee 1:1d96ab1dbcc4 69 PwmOut SpeedLeft(speedPinLeft);
Kevin_Lee 0:7535295d1670 70
Kevin_Lee 1:1d96ab1dbcc4 71 int speedmotorA = 255; //define the speed of motorA
Kevin_Lee 1:1d96ab1dbcc4 72 int speedmotorB = 255; //define the speed of motorB
Kevin_Lee 1:1d96ab1dbcc4 73
Kevin_Lee 1:1d96ab1dbcc4 74 Shieldbot::Shieldbot()
Kevin_Lee 0:7535295d1670 75 {
Kevin_Lee 1:1d96ab1dbcc4 76 SpeedRight.period_us(100);
Kevin_Lee 1:1d96ab1dbcc4 77 SpeedLeft.period_us(100);
Kevin_Lee 0:7535295d1670 78 }
Kevin_Lee 0:7535295d1670 79
Kevin_Lee 1:1d96ab1dbcc4 80 //sets same max speed to both motors
Kevin_Lee 1:1d96ab1dbcc4 81 void Shieldbot::setMaxSpeed(int both){
Kevin_Lee 1:1d96ab1dbcc4 82 setMaxLeftSpeed(both);
Kevin_Lee 1:1d96ab1dbcc4 83 setMaxRightSpeed(both);
Kevin_Lee 1:1d96ab1dbcc4 84 }
Kevin_Lee 0:7535295d1670 85
Kevin_Lee 1:1d96ab1dbcc4 86 void Shieldbot::setMaxSpeed(int left, int right){
Kevin_Lee 1:1d96ab1dbcc4 87 setMaxLeftSpeed(left);
Kevin_Lee 1:1d96ab1dbcc4 88 setMaxRightSpeed(right);
Kevin_Lee 1:1d96ab1dbcc4 89 }
Kevin_Lee 0:7535295d1670 90
Kevin_Lee 1:1d96ab1dbcc4 91 void Shieldbot::setMaxLeftSpeed(int left){
Kevin_Lee 1:1d96ab1dbcc4 92 speedmotorB=left;
Kevin_Lee 1:1d96ab1dbcc4 93 }
Kevin_Lee 1:1d96ab1dbcc4 94
Kevin_Lee 1:1d96ab1dbcc4 95 void Shieldbot::setMaxRightSpeed(int right){
Kevin_Lee 1:1d96ab1dbcc4 96 speedmotorA=right;
Kevin_Lee 0:7535295d1670 97 }
Kevin_Lee 0:7535295d1670 98
Kevin_Lee 1:1d96ab1dbcc4 99 int Shieldbot::readS1(){
Kevin_Lee 1:1d96ab1dbcc4 100 return finder1.read();
Kevin_Lee 1:1d96ab1dbcc4 101 }
Kevin_Lee 1:1d96ab1dbcc4 102
Kevin_Lee 1:1d96ab1dbcc4 103 int Shieldbot::readS2(){
Kevin_Lee 1:1d96ab1dbcc4 104 return finder2.read();
Kevin_Lee 1:1d96ab1dbcc4 105 }
Kevin_Lee 1:1d96ab1dbcc4 106
Kevin_Lee 1:1d96ab1dbcc4 107 int Shieldbot::readS3(){
Kevin_Lee 1:1d96ab1dbcc4 108 return finder3.read();
Kevin_Lee 0:7535295d1670 109 }
Kevin_Lee 0:7535295d1670 110
Kevin_Lee 1:1d96ab1dbcc4 111 int Shieldbot::readS4(){
Kevin_Lee 1:1d96ab1dbcc4 112 return finder4.read();
Kevin_Lee 0:7535295d1670 113 }
Kevin_Lee 0:7535295d1670 114
Kevin_Lee 1:1d96ab1dbcc4 115 int Shieldbot::readS5(){
Kevin_Lee 1:1d96ab1dbcc4 116 return finder5.read();
Kevin_Lee 1:1d96ab1dbcc4 117 }
Kevin_Lee 1:1d96ab1dbcc4 118
Kevin_Lee 1:1d96ab1dbcc4 119 void Shieldbot::drive(signed char left,signed char right){
Kevin_Lee 1:1d96ab1dbcc4 120 rightMotor(right);
Kevin_Lee 1:1d96ab1dbcc4 121 leftMotor(left);
Kevin_Lee 0:7535295d1670 122 }
Kevin_Lee 0:7535295d1670 123
Kevin_Lee 1:1d96ab1dbcc4 124 //char is 128 to 127
Kevin_Lee 1:1d96ab1dbcc4 125 //mag is the direction to spin the right motor
Kevin_Lee 1:1d96ab1dbcc4 126 //-128 backwards all the way
Kevin_Lee 1:1d96ab1dbcc4 127 //0 dont move
Kevin_Lee 1:1d96ab1dbcc4 128 //127 forwards all the way
Kevin_Lee 1:1d96ab1dbcc4 129 void Shieldbot::rightMotor(signed char mag){
Kevin_Lee 1:1d96ab1dbcc4 130 int actualSpeed = 0;
Kevin_Lee 1:1d96ab1dbcc4 131 if(mag >0){ //forward
Kevin_Lee 1:1d96ab1dbcc4 132 float ratio = (float)mag/128;
Kevin_Lee 1:1d96ab1dbcc4 133 actualSpeed = (int)(ratio*speedmotorA); //define your speed based on global speed
Kevin_Lee 1:1d96ab1dbcc4 134 #if SHIELDBOTDEBUG
Kevin_Lee 1:1d96ab1dbcc4 135 Serial.print("forward right: ");
Kevin_Lee 1:1d96ab1dbcc4 136 Serial.println(actualSpeed);
Kevin_Lee 1:1d96ab1dbcc4 137 #endif
Kevin_Lee 1:1d96ab1dbcc4 138 SpeedRight.write((float)(actualSpeed/255.0));
Kevin_Lee 1:1d96ab1dbcc4 139 // analogWrite(speedPinRight,actualSpeed);
Kevin_Lee 1:1d96ab1dbcc4 140 Right1 = HIGH;
Kevin_Lee 1:1d96ab1dbcc4 141 Right2 = LOW;
Kevin_Lee 1:1d96ab1dbcc4 142 // digitalWrite(right1,HIGH);
Kevin_Lee 1:1d96ab1dbcc4 143 // digitalWrite(right2,LOW);//turn right motor clockwise
Kevin_Lee 1:1d96ab1dbcc4 144 }else if(mag == 0){ //neutral
Kevin_Lee 1:1d96ab1dbcc4 145 #if SHIELDBOTDEBUG
Kevin_Lee 1:1d96ab1dbcc4 146 Serial.println("nuetral right");
Kevin_Lee 1:1d96ab1dbcc4 147 #endif
Kevin_Lee 1:1d96ab1dbcc4 148 stopRight();
Kevin_Lee 1:1d96ab1dbcc4 149 }else { //meaning backwards
Kevin_Lee 1:1d96ab1dbcc4 150 float ratio = (float)abs(mag)/128;
Kevin_Lee 1:1d96ab1dbcc4 151 actualSpeed = ratio*speedmotorA;
Kevin_Lee 1:1d96ab1dbcc4 152 #if SHIELDBOTDEBUG
Kevin_Lee 1:1d96ab1dbcc4 153 Serial.print("backward right: ");
Kevin_Lee 1:1d96ab1dbcc4 154 Serial.println(actualSpeed);
Kevin_Lee 1:1d96ab1dbcc4 155 #endif
Kevin_Lee 1:1d96ab1dbcc4 156 SpeedRight.write((float)(actualSpeed/255.0));
Kevin_Lee 1:1d96ab1dbcc4 157 //analogWrite(speedPinRight,actualSpeed);
Kevin_Lee 1:1d96ab1dbcc4 158 Right1 = LOW;
Kevin_Lee 1:1d96ab1dbcc4 159 Right2 = HIGH;
Kevin_Lee 1:1d96ab1dbcc4 160 // digitalWrite(right1,LOW);
Kevin_Lee 1:1d96ab1dbcc4 161 // digitalWrite(right2,HIGH);//turn right motor counterclockwise
Kevin_Lee 1:1d96ab1dbcc4 162 }
Kevin_Lee 0:7535295d1670 163 }
Kevin_Lee 0:7535295d1670 164
Kevin_Lee 1:1d96ab1dbcc4 165 //TODO shouldnt these share one function and just input the differences?
Kevin_Lee 1:1d96ab1dbcc4 166 void Shieldbot::leftMotor(signed char mag){
Kevin_Lee 1:1d96ab1dbcc4 167 int actualSpeed = 0;
Kevin_Lee 1:1d96ab1dbcc4 168 if(mag >0){ //forward
Kevin_Lee 1:1d96ab1dbcc4 169 float ratio = (float)(mag)/128;
Kevin_Lee 1:1d96ab1dbcc4 170 actualSpeed = (int)(ratio*speedmotorB); //define your speed based on global speed
Kevin_Lee 1:1d96ab1dbcc4 171 #if SHIELDBOTDEBUG
Kevin_Lee 1:1d96ab1dbcc4 172 Serial.print("forward left: ");
Kevin_Lee 1:1d96ab1dbcc4 173 Serial.println(actualSpeed);
Kevin_Lee 1:1d96ab1dbcc4 174 #endif
Kevin_Lee 1:1d96ab1dbcc4 175 SpeedLeft.write((float)(actualSpeed/255.0));
Kevin_Lee 1:1d96ab1dbcc4 176 Left1 = HIGH;
Kevin_Lee 1:1d96ab1dbcc4 177 Left2 = LOW;
Kevin_Lee 1:1d96ab1dbcc4 178 // analogWrite(speedPinLeft,actualSpeed);
Kevin_Lee 1:1d96ab1dbcc4 179 // digitalWrite(left1,HIGH);
Kevin_Lee 1:1d96ab1dbcc4 180 // digitalWrite(left2,LOW);//turn left motor counter-clockwise
Kevin_Lee 1:1d96ab1dbcc4 181 }else if(mag == 0){ //neutral
Kevin_Lee 1:1d96ab1dbcc4 182 #if SHIELDBOTDEBUG
Kevin_Lee 1:1d96ab1dbcc4 183 Serial.println("nuetral left");
Kevin_Lee 1:1d96ab1dbcc4 184 #endif
Kevin_Lee 1:1d96ab1dbcc4 185 stopLeft();
Kevin_Lee 1:1d96ab1dbcc4 186 }else { //meaning backwards
Kevin_Lee 1:1d96ab1dbcc4 187 float ratio = (float)abs(mag)/128;
Kevin_Lee 1:1d96ab1dbcc4 188 actualSpeed = ratio*speedmotorB;
Kevin_Lee 1:1d96ab1dbcc4 189 #if SHIELDBOTDEBUG
Kevin_Lee 1:1d96ab1dbcc4 190 Serial.print("backward left: ");
Kevin_Lee 1:1d96ab1dbcc4 191 Serial.println(actualSpeed);
Kevin_Lee 1:1d96ab1dbcc4 192 #endif
Kevin_Lee 1:1d96ab1dbcc4 193 SpeedLeft.write((float)(actualSpeed/255.0));
Kevin_Lee 1:1d96ab1dbcc4 194 Left1 = LOW;
Kevin_Lee 1:1d96ab1dbcc4 195 Left2 = HIGH;
Kevin_Lee 1:1d96ab1dbcc4 196 // analogWrite(speedPinLeft,actualSpeed);
Kevin_Lee 1:1d96ab1dbcc4 197 // digitalWrite(left1,LOW);
Kevin_Lee 1:1d96ab1dbcc4 198 // digitalWrite(left2,HIGH);//turn left motor counterclockwise
Kevin_Lee 1:1d96ab1dbcc4 199 }
Kevin_Lee 1:1d96ab1dbcc4 200 }
Kevin_Lee 0:7535295d1670 201
Kevin_Lee 1:1d96ab1dbcc4 202 void Shieldbot::forward(){
Kevin_Lee 1:1d96ab1dbcc4 203 leftMotor(127);
Kevin_Lee 1:1d96ab1dbcc4 204 rightMotor(127);
Kevin_Lee 0:7535295d1670 205 }
Kevin_Lee 0:7535295d1670 206
Kevin_Lee 1:1d96ab1dbcc4 207 void Shieldbot::backward(){
Kevin_Lee 1:1d96ab1dbcc4 208 leftMotor(-128);
Kevin_Lee 1:1d96ab1dbcc4 209 rightMotor(-128);
Kevin_Lee 1:1d96ab1dbcc4 210 }
Kevin_Lee 1:1d96ab1dbcc4 211
Kevin_Lee 1:1d96ab1dbcc4 212 void Shieldbot::stop(){
Kevin_Lee 1:1d96ab1dbcc4 213 stopLeft();
Kevin_Lee 1:1d96ab1dbcc4 214 stopRight();
Kevin_Lee 1:1d96ab1dbcc4 215 }
Kevin_Lee 1:1d96ab1dbcc4 216
Kevin_Lee 1:1d96ab1dbcc4 217 void Shieldbot::stopLeft(){
Kevin_Lee 1:1d96ab1dbcc4 218 // analogWrite(speedPinLeft,0);
Kevin_Lee 1:1d96ab1dbcc4 219 SpeedLeft.write(0);
Kevin_Lee 1:1d96ab1dbcc4 220 }
Kevin_Lee 1:1d96ab1dbcc4 221
Kevin_Lee 1:1d96ab1dbcc4 222 void Shieldbot::stopRight(){
Kevin_Lee 1:1d96ab1dbcc4 223 //analogWrite(speedPinRight,0);
Kevin_Lee 1:1d96ab1dbcc4 224 SpeedRight.write(0);
Kevin_Lee 0:7535295d1670 225 }
Kevin_Lee 0:7535295d1670 226
Kevin_Lee 1:1d96ab1dbcc4 227 //may be dangerous to motor if reverse current into hbridge exceeds 2A
Kevin_Lee 1:1d96ab1dbcc4 228 void Shieldbot::fastStopLeft(){
Kevin_Lee 1:1d96ab1dbcc4 229 Left1 = LOW;
Kevin_Lee 1:1d96ab1dbcc4 230 Left2 = LOW;
Kevin_Lee 1:1d96ab1dbcc4 231 // digitalWrite(left1,LOW);
Kevin_Lee 1:1d96ab1dbcc4 232 // digitalWrite(left2,LOW);//turn DC Motor B move clockwise
Kevin_Lee 0:7535295d1670 233 }
Kevin_Lee 0:7535295d1670 234
Kevin_Lee 1:1d96ab1dbcc4 235 //may be dangerous to motor if reverse current into hbridge exceeds 2A
Kevin_Lee 1:1d96ab1dbcc4 236 void Shieldbot::fastStopRight(){
Kevin_Lee 1:1d96ab1dbcc4 237 Right1 = LOW;
Kevin_Lee 1:1d96ab1dbcc4 238 Right2 = LOW;
Kevin_Lee 1:1d96ab1dbcc4 239 // digitalWrite(right1,LOW);
Kevin_Lee 1:1d96ab1dbcc4 240 // digitalWrite(right2,LOW);//turn DC Motor A move anticlockwise
Kevin_Lee 0:7535295d1670 241 }
Kevin_Lee 0:7535295d1670 242
Kevin_Lee 1:1d96ab1dbcc4 243 //may be dangerous to motor if reverse current into hbridge exceeds 2A
Kevin_Lee 1:1d96ab1dbcc4 244 void Shieldbot::fastStop(){
Kevin_Lee 1:1d96ab1dbcc4 245 fastStopRight();
Kevin_Lee 1:1d96ab1dbcc4 246 fastStopLeft();
Kevin_Lee 1:1d96ab1dbcc4 247 }