1+ /*
2+ Created to support the release of the Sheildbot from SeeedStudios
3+ http://www.seeedstudio.com/wiki/Shield_Bot
4+
5+ Created by Jacob Rosenthal and Colin Ho, December 30, 2012.
6+ Released into the public domain.
7+ */
8+
9+ // include core Wiring API
10+ #include " Arduino.h"
11+
12+ // include this library's description file
13+ #include " Shieldbot.h"
14+
15+ #define SHIELDBOTDEBUG 0
16+
17+ #define right1 5 // define I1 interface
18+ #define speedPinRight 6 // enable right motor (bridge A)
19+ #define right2 7 // define I2 interface
20+ #define left1 8 // define I3 interface
21+ #define speedPinLeft 9 // enable motor B
22+ #define left2 10 // define I4 interface
23+ #define finder1 A0 // define line finder S1
24+ #define finder2 A1 // define line finder S2
25+ #define finder3 A2 // define line finder S3
26+ #define finder4 A3 // define line finder S4
27+ #define finder5 4 // define line finder S5
28+
29+ int speedmotorA = 255 ; // define the speed of motorA
30+ int speedmotorB = 255 ; // define the speed of motorB
31+
32+ Shieldbot::Shieldbot ()
33+ {
34+ pinMode (right1,OUTPUT);
35+ pinMode (right2,OUTPUT);
36+ pinMode (speedPinRight,OUTPUT);
37+ pinMode (left1,OUTPUT);
38+ pinMode (left2,OUTPUT);
39+ pinMode (speedPinLeft,OUTPUT);
40+ pinMode (finder1,INPUT);
41+ pinMode (finder2,INPUT);
42+ pinMode (finder3,INPUT);
43+ pinMode (finder4,INPUT);
44+ pinMode (finder5,INPUT);
45+ }
46+
47+ // sets same max speed to both motors
48+ void Shieldbot::setMaxSpeed (int both){
49+ setMaxLeftSpeed (both);
50+ setMaxRightSpeed (both);
51+ }
52+
53+ void Shieldbot::setMaxSpeed (int left, int right){
54+ setMaxLeftSpeed (left);
55+ setMaxRightSpeed (right);
56+ }
57+
58+ void Shieldbot::setMaxLeftSpeed (int left){
59+ speedmotorB=left;
60+ }
61+
62+ void Shieldbot::setMaxRightSpeed (int right){
63+ speedmotorA=right;
64+ }
65+
66+ int Shieldbot::readS1 (){
67+ return digitalRead (finder1);
68+ }
69+
70+ int Shieldbot::readS2 (){
71+ return digitalRead (finder2);
72+ }
73+
74+ int Shieldbot::readS3 (){
75+ return digitalRead (finder3);
76+ }
77+
78+ int Shieldbot::readS4 (){
79+ return digitalRead (finder4);
80+ }
81+
82+ int Shieldbot::readS5 (){
83+ return digitalRead (finder5);
84+ }
85+
86+ void Shieldbot::drive (char left, char right){
87+ rightMotor (right);
88+ leftMotor (left);
89+ }
90+
91+ // char is 128 to 127
92+ // mag is the direction to spin the right motor
93+ // -128 backwards all the way
94+ // 0 dont move
95+ // 127 forwards all the way
96+ void Shieldbot::rightMotor (char mag){
97+ int actualSpeed = 0 ;
98+ if (mag >0 ){ // forward
99+ float ratio = (float )mag/128 ;
100+ actualSpeed = (int )(ratio*speedmotorA); // define your speed based on global speed
101+ #if SHIELDBOTDEBUG
102+ Serial.print (" forward right: "
103+ Serial.println (actualSpeed);
104+ #endif
105+ analogWrite (speedPinRight,actualSpeed);
106+ digitalWrite (right1,HIGH);
107+ digitalWrite (right2,LOW);// turn right motor clockwise
108+ }else if (mag == 0 ){ // neutral
109+ #if SHIELDBOTDEBUG
110+ Serial.println (" nuetral right"
111+ #endif
112+ stopRight ();
113+ }else { // meaning backwards
114+ float ratio = (float )abs (mag)/128 ;
115+ actualSpeed = ratio*speedmotorA;
116+ #if SHIELDBOTDEBUG
117+ Serial.print (" backward right: "
118+ Serial.println (actualSpeed);
119+ #endif
120+ analogWrite (speedPinRight,actualSpeed);
121+ digitalWrite (right1,LOW);
122+ digitalWrite (right2,HIGH);// turn right motor counterclockwise
123+ }
124+ }
125+
126+ // TODO shouldnt these share one function and just input the differences?
127+ void Shieldbot::leftMotor (char mag){
128+ int actualSpeed = 0 ;
129+ if (mag >0 ){ // forward
130+ float ratio = (float )(mag)/128 ;
131+ actualSpeed = (int )(ratio*speedmotorB); // define your speed based on global speed
132+ #if SHIELDBOTDEBUG
133+ Serial.print (" forward left: "
134+ Serial.println (actualSpeed);
135+ #endif
136+ analogWrite (speedPinLeft,actualSpeed);
137+ digitalWrite (left1,HIGH);
138+ digitalWrite (left2,LOW);// turn left motor counter-clockwise
139+ }else if (mag == 0 ){ // neutral
140+ #if SHIELDBOTDEBUG
141+ Serial.println (" nuetral left"
142+ #endif
143+ stopLeft ();
144+ }else { // meaning backwards
145+ float ratio = (float )abs (mag)/128 ;
146+ actualSpeed = ratio*speedmotorB;
147+ #if SHIELDBOTDEBUG
148+ Serial.print (" backward left: "
149+ Serial.println (actualSpeed);
150+ #endif
151+ analogWrite (speedPinLeft,actualSpeed);
152+ digitalWrite (left1,LOW);
153+ digitalWrite (left2,HIGH);// turn left motor counterclockwise
154+ }
155+ }
156+
157+ void Shieldbot::forward (){
158+ leftMotor (127 );
159+ rightMotor (127 );
160+ }
161+
162+ void Shieldbot::backward (){
163+ leftMotor (-128 );
164+ rightMotor (-128 );
165+ }
166+
167+ void Shieldbot::stop (){
168+ stopLeft ();
169+ stopRight ();
170+ }
171+
172+ void Shieldbot::stopLeft (){
173+ analogWrite (speedPinLeft,0 );
174+ }
175+
176+ void Shieldbot::stopRight (){
177+ analogWrite (speedPinRight,0 );
178+ }
179+
180+ // may be dangerous to motor if reverse current into hbridge exceeds 2A
181+ void Shieldbot::fastStopLeft (){
182+ digitalWrite (left1,LOW);
183+ digitalWrite (left2,LOW);// turn DC Motor B move clockwise
184+ }
185+
186+ // may be dangerous to motor if reverse current into hbridge exceeds 2A
187+ void Shieldbot::fastStopRight (){
188+ digitalWrite (right1,LOW);
189+ digitalWrite (right2,LOW);// turn DC Motor A move anticlockwise
190+ }
191+
192+ // may be dangerous to motor if reverse current into hbridge exceeds 2A
193+ void Shieldbot::fastStop (){
194+ fastStopRight ();
195+ fastStopLeft ();
196+ }
0 commit comments