ParallaxRobotShield.cpp

00001 #include "ParallaxRobotShield.h"
00002 #include "mbed.h"
00003 
00004 //ParallaxRobotShield::ParallaxRobotShield(PinName leftServoPin, PinName rightServoPin) : leftServo(leftServoPin) , rightServo(rightServoPin) , leftWhisker(NULL) , rightWhisker(NULL)
00005 //{
00006 //    leftPosition = 1450;
00007 //    rightPosition = 1550;    
00008 //}
00009 
00010 ParallaxRobotShield::ParallaxRobotShield(PinName leftServoPin, PinName rightServoPin, PinName leftWhiskerPin, PinName rightWhiskerPin) : 
00011                      leftServo(leftServoPin) , rightServo(rightServoPin) , leftWhisker(leftWhiskerPin) , rightWhisker(rightWhiskerPin)
00012 {
00013     leftPosition = 1450;
00014     rightPosition = 1550;
00015 }
00016 
00017 void ParallaxRobotShield::left_servo(int speed)
00018 {
00019     speed = 1490 - (speed * 2);
00020     leftPosition = speed;
00021 }
00022 
00023 
00024 void ParallaxRobotShield::right_servo(int speed)
00025 {
00026     speed = 1496 + (speed * 2);
00027     rightPosition = speed;
00028 }
00029 
00030 //void ParallaxRobotShield::StartPulse() 
00031 //{
00032 //    ServoPin = 1;
00033 //    PulseStop.attach_us(this, &ParallaxRobotShield::EndPulse, Position);
00034 //}
00035 
00036 void ParallaxRobotShield::disable_left_motor()  // Just for internal functions
00037 {
00038     leftServo = 0;
00039 }
00040 //void ParallaxRobotShield::EndPulse() 
00041 //{
00042 //    ServoPin = 0;
00043 //}
00044 void ParallaxRobotShield::disable_right_motor() // Just for internal functions
00045 {
00046     rightServo = 0;
00047 }
00048 
00049 
00050 void ParallaxRobotShield::enable_left_motor()
00051 {
00052     leftServo = 1;
00053     leftPulse.attach_us(this, &ParallaxRobotShield::enable_left_motor, 20000);      // Period is set to 20000 us by default which is equal to 50 Hz
00054     leftPulseStop.attach_us(this, &ParallaxRobotShield::disable_left_motor, leftPosition);
00055 }
00056 
00057 
00058 void ParallaxRobotShield::enable_right_motor()
00059 {
00060     rightServo = 1;
00061     rightPulse.attach_us(this, &ParallaxRobotShield::enable_right_motor, 20000);    // Period is set to 20000 us by default which is equal to 50 Hz
00062     rightPulseStop.attach_us(this, &ParallaxRobotShield::disable_right_motor, rightPosition);
00063 }
00064 
00065 
00066 void ParallaxRobotShield::turn_left(int speed)
00067 {
00068     stopRight();
00069     int i;
00070     for(i = 0; i < 100; i++)
00071     {
00072         left_servo(speed);
00073         wait_ms(10);    
00074     }
00075     enable_right_motor();
00076 }
00077 
00078 
00079 void ParallaxRobotShield::turn_right(int speed)
00080 {
00081     stopLeft();
00082     int i;
00083     for(i = 0; i < 100; i++)
00084     {
00085         right_servo(speed);
00086         wait_ms(10);    
00087     }
00088     enable_left_motor();
00089 }
00090 
00091 
00092 void ParallaxRobotShield::forward(int speed)
00093 {
00094     int i;
00095     for(i = 0; i < 100; i++)
00096     {
00097         left_servo(speed);
00098         right_servo(speed); 
00099         wait_ms(20);
00100     }
00101 }
00102 
00103 
00104 void ParallaxRobotShield::backward(int speed)
00105 {
00106     forward(-speed);
00107 }
00108 
00109 
00110 void ParallaxRobotShield::left(int speed)
00111 {
00112     int i;
00113     for(i = 0; i < 100; i++)
00114     {
00115         left_servo(speed);
00116         right_servo(-speed); 
00117         wait_ms(20);
00118     }
00119 }
00120 
00121 
00122 void ParallaxRobotShield::right(int speed)
00123 {
00124     left(-speed);
00125 }
00126 
00127 
00128 void ParallaxRobotShield::stopLeft()
00129 {
00130     leftPulse.detach();
00131 }
00132 
00133 void ParallaxRobotShield::stopRight()
00134 {
00135     rightPulse.detach();
00136 }
00137 
00138 
00139 void ParallaxRobotShield::stop(int motor)
00140 {
00141     if(motor == 1)
00142         stopRight();
00143     else if(motor == 2)
00144         stopLeft();
00145 }
00146 
00147 
00148 void ParallaxRobotShield::stopAll()
00149 {
00150     stopRight();
00151     stopLeft();
00152 }
00153 
00154 
00155 //void ParallaxRobotShield::whiskersLED()
00156 //{
00157 //    DigitalOut leftLED(PA_10);
00158 //    DigitalOut rightLED(PA_5);
00159 //    while(leftWhiskerContact())
00160 //        leftLED = 1;
00161 //    while(rightWhiskerContact())
00162 //        rightLED = 1;    
00163 //    leftLED = 0;
00164 //    rightLED = 0;
00165 //}
00166 
00167 int ParallaxRobotShield::leftWhiskerContact()
00168 {
00169     if(leftWhisker.read() == 0)
00170         return 1;
00171     else     
00172         return 0;
00173 }
00174 
00175 
00176 int ParallaxRobotShield::rightWhiskerContact()
00177 {
00178     if(rightWhisker.read() == 0)
00179         return 1;
00180     else     
00181         return 0;
00182 }
00183 
00184 
00185 //void ParallaxRobotShield::whisker()
00186 //{
00187 //    DigitalOut leftLED(PA_10);
00188 //    DigitalOut rightLED(PA_5);
00189 //    while(1)
00190 //    {
00191 //        if(leftWhisker.read() == 0)
00192 //        {
00193 //            leftLED = 1;
00194 //            leftWhiskerContact();
00195 //        }
00196 //        leftLED = 0;
00197 //        if(rightWhisker.read() == 0)
00198 //        {
00199 //            rightLED = 1;
00200 //            rightWhiskerContact();
00201 //        }
00202 //        rightLED = 0;
00203 //        left_servo(100);
00204 //        right_servo(100);
00205 //    }
00206 //}
00207 
00208 
00209 
00210 
00211 //void ParallaxRobotShield::Enable(int StartPos, int Period) 
00212 //{
00213 //    leftPosition = StartPos;
00214 //    leftPulse.attach_us(this, &ParallaxRobotShield::enable_left_motor, Period);
00215 //}