Diff: Shieldbot.cpp

Revision:

0:fc1b473bfa7f

diff -r 000000000000 -r fc1b473bfa7f Shieldbot.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Shieldbot.cpp	Tue Aug 11 04:49:45 2015 +0000
@@ -0,0 +1,239 @@
+/*
+  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.
+*/
+
+// include core Wiring API
+#include "mbed.h"
+
+// include this library's description file
+#include "Shieldbot.h"
+
+#define SHIELDBOTDEBUG 0
+// for shieldbot Version 0.9 and 1.0
+ 
+/*#define right1 5			//define I1 interface
+#define speedPinRight 6  	//enable right motor (bridge A)
+#define right2 7      		//define I2 interface 
+#define left1 8      		//define I3 interface 
+#define speedPinLeft 9  	//enable motor B
+#define left2 10     		//define I4 interface */
+ 
+// for shieldbot Version 1.1, we changed to the driver pins ,
+//if you shieldbot is ver1.0 or 0.9, please using the above pin mapping;
+#define right1 PTA0			//define I1 interface
+#define speedPinRight PTC4 	//enable right motor (bridge A)
+#define right2 PTD2      		//define I2 interface 
+#define left1 PTD3      		//define I3 interface 
+#define speedPinLeft PTD0  	//enable motor B
+#define left2 PTD1    		//define I4 interface 
+
+#define finder1 PTB2   		//define line finder S1
+#define finder2 PTB3   		//define line finder S2
+#define finder3 PTB10   		//define line finder S3
+#define finder4 PTB11   		//define line finder S4
+#define finder5 PTC11    		//define line finder S5
+
+int speedmotorA = 255; //define the speed of motorA
+int speedmotorB = 255; //define the speed of motorB
+  
+
+
+Shieldbot::Shieldbot()
+{
+  DigitalOut right1(PTA0);
+  DigitalOut right2(PTD2);
+  PwmOut speedPinRight(PTC4);
+  speedPinRight.period(0.002f);
+  DigitalOut left1(PTD3);
+  DigitalOut left2(PTD1);
+  PwmOut speedPinLeft(PTD0);
+  speedPinLeft.period(0.002f);
+  DigitalIn finder1(PTB2);
+  DigitalIn finder2(PTB3);
+  DigitalIn finder3(PTB10);
+  DigitalIn finder4(PTB11);
+  DigitalIn finder5(PTC11);
+}
+
+
+
+//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;
+}
+
+int Shieldbot::readS2(){
+  return finder2;
+}
+
+int Shieldbot::readS3(){
+  return finder3;
+}
+
+int Shieldbot::readS4(){
+  return finder4;
+}
+
+int Shieldbot::readS5(){
+  return finder5;
+}
+
+void Shieldbot::drive(char left, 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(char mag){
+  int actualSpeed = 0;
+  DigitalOut right1(PTA0);
+  DigitalOut right2(PTD2);
+  PwmOut speedPinRight(PTC4);
+  speedPinRight.period(0.002f);
+  DigitalOut left1(PTD3);
+  DigitalOut left2(PTD1);
+  PwmOut speedPinLeft(PTD0);
+  speedPinLeft.period(0.002f);
+ 
+
+  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
+    speedPinRight.write(actualSpeed);
+    right1=0;
+    right2=1;//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
+    speedPinRight.write(actualSpeed);
+    right1=1;
+    right2=0;//turn right motor counterclockwise
+  }
+}
+
+//TODO shouldnt these share one function and just input the differences?
+void Shieldbot::leftMotor(char mag){
+  DigitalOut right1(PTA0);
+  DigitalOut right2(PTD2);
+  PwmOut speedPinRight(PTC4);
+  speedPinRight.period(0.002f);
+  DigitalOut left1(PTD3);
+  DigitalOut left2(PTD1);
+  PwmOut speedPinLeft(PTD0);
+  speedPinLeft.period(0.002f);
+
+  
+  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
+    speedPinLeft.write(actualSpeed);
+    left1=0;
+    left2=1;//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
+    speedPinLeft.write(actualSpeed);
+    left1=1;
+    left2=0;//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(){
+  PwmOut speedPinLeft(PTD0);
+  speedPinLeft.write(0);
+}
+
+void Shieldbot::stopRight(){
+  PwmOut speedPinRight(PTC4);
+  speedPinRight.write(0);
+}
+
+//may be dangerous to motor if reverse current into hbridge exceeds 2A
+void Shieldbot::fastStopLeft(){
+  DigitalOut left1(PTD3);
+  DigitalOut left2(PTD1);
+  left1=1;
+  left2=1;//turn DC Motor B move clockwise
+}
+
+//may be dangerous to motor if reverse current into hbridge exceeds 2A
+void Shieldbot::fastStopRight(){
+  DigitalOut right1(PTA0);
+  DigitalOut right2(PTD2);
+  right1=1;
+  right2=1;//turn DC Motor A move anticlockwise
+}
+
+//may be dangerous to motor if reverse current into hbridge exceeds 2A
+void Shieldbot::fastStop(){
+	fastStopRight();
+	fastStopLeft();
+}