Motor.cpp

00001 /*--------------------------------------------------------------------------------
00002 Filename: main.cpp
00003 Description: Main program loop
00004 --------------------------------------------------------------------------------*/
00005 #include "main.h"
00006 #include "Buzzer.h"
00007 #include "Motor.h"
00008 #include "LED.h"
00009 #include <ros.h>
00010 #include <Servo.h>
00011 #include <std_msgs/UInt8.h>
00012 //#include <std_msgs/UInt16.h>
00013 
00014 
00015 cMotor MotorL(M1_PWM, M1_IN1, M1_IN2);  //Left motor class and pin initialisation
00016 cMotor MotorR(M2_PWM, M2_IN1, M2_IN2);  //Right motor class and pin initialisation
00017 
00018 //Servo servo1(srvoTilt);
00019 //Servo servo2(srvoPan);
00020 
00021 //Class definitions
00022 cBuzzer cBuzzer(Buzz);
00023 cRGB_LED cRGB_LED(DIAG_RED, DIAG_BLU, DIAG_GRN);
00024 
00025 int Servo1Pos;
00026 int Servo2Pos;
00027 
00028 /*--------------------------------------------------------------------------------
00029 Function name: KeySub
00030 Input Parameters: std_msgs::UInt8 &KeyPress
00031 Output Parameters:
00032 Description:
00033 ----------------------------------------------------------------------------------*/
00034 void MotorKeySub(const std_msgs::UInt8 &keyPress)
00035 {
00036     printf("%d", keyPress.data);
00037 
00038     // Lowercase chars //
00039     if (keyPress.data == 119) { // w
00040         if (keyPress.data  == 88 || 120) { // X
00041             MotorR.Stop();
00042             MotorL.Stop();
00043         }
00044         MotorR.Forwards(1.0f);
00045         MotorL.Forwards(1.0f);
00046         wait(0.05);
00047     }
00048 
00049     else if (keyPress.data == 97) { // a
00050         if (keyPress.data  == 88 || 120) { // X
00051             MotorR.Stop();
00052             MotorL.Stop();
00053         }
00054         MotorR.Forwards(1.0f);
00055         MotorL.Backwards(1.0f);
00056         wait(0.05);
00057     }
00058 
00059     else if (keyPress.data  == 100) { // d
00060         if (keyPress.data  == 88 || 120) { // X
00061             MotorR.Stop();
00062             MotorL.Stop();
00063         }
00064         MotorR.Backwards(1.0f);
00065         MotorL.Forwards(1.0f);
00066         wait(0.05);
00067     }
00068 
00069     else if (keyPress.data  == 115) { // s
00070         if (keyPress.data  == 88 || 120) { // X
00071             MotorR.Stop();
00072             MotorL.Stop();
00073         }
00074         MotorR.Backwards(1.0f);
00075         MotorL.Backwards(1.0f);
00076         wait(0.05);
00077     }
00078 
00079 
00080     // Upper Case Chars
00081     else if (keyPress.data == 87) { // W
00082         if (keyPress.data  == 88 || 120) { // X
00083             MotorR.Stop();
00084             MotorL.Stop();
00085         }
00086         MotorR.Forwards(1.0f);
00087         MotorL.Forwards(1.0f);
00088         wait(0.387);
00089     }
00090 
00091     else if (keyPress.data == 65) { // A
00092         if (keyPress.data  == 88 || 120) { // X
00093             MotorR.Stop();
00094             MotorL.Stop();
00095         }
00096         MotorR.Forwards(1.0f);
00097         MotorL.Backwards(1.0f);
00098         wait(0.387);
00099     }
00100 
00101     else if (keyPress.data  == 68) { // D
00102         if (keyPress.data  == 88 || 120) { // X
00103             MotorR.Stop();
00104             MotorL.Stop();
00105         }
00106         MotorR.Backwards(1.0f);
00107         MotorL.Forwards(1.0f);
00108         wait(0.387);
00109     }
00110 
00111     else if (keyPress.data  == 83) { // S
00112         if (keyPress.data  == 88 || 120) { // X
00113             MotorR.Stop();
00114             MotorL.Stop();
00115         }
00116         MotorR.Backwards(1.0f);
00117         MotorL.Backwards(1.0f);
00118         wait(0.387);
00119     }
00120 
00121 
00122     else {
00123         MotorR.Stop();
00124         MotorL.Stop();
00125     }
00126 }
00127 
00128 /*--------------------------------------------------------------------------------
00129 Function name: update_power_levels
00130 Input Parameters: vBatt
00131 Output Parameters: N/A
00132 Description: Function used to send the battery level over to the LED class locally from global source files
00133 ----------------------------------------------------------------------------------*/
00134 void update_power_levels(float vBatt)
00135 {
00136     cRGB_LED.record_power(vBatt);
00137 }
00138 
00139 /*--------------------------------------------------------------------------------
00140 Function name: Check_for_obstacles
00141 Input Parameters: cMotor - MotorL (left motor) cMotor MotorR (right motor) TOFrange[4] - sensor measurements
00142 Output Parameters: N/A
00143 Description: Simple obstacle avoidance functionality
00144 ----------------------------------------------------------------------------------*/
00145 void Check_for_obstacles(uint8_t TOFRange[4])
00146 {
00147     //If top right sensor(6) detects something
00148     if (TOFRange[2]<200) {
00149 
00150         if(TOFRange[2]>150) {   //Provided its 15cm away...
00151             cBuzzer.Beep();
00152             cRGB_LED.blue_led();
00153             wait(0.02);
00154             cRGB_LED.display_power();
00155             if(TOFRange[3]<200) {   //...and the back sensor detects something
00156                 //Smooth turn right
00157                 MotorR.Forwards(0.8f);
00158                 MotorL.Forwards(0.2f);
00159 
00160             } else if(TOFRange[1]<200) { //...and the top left sensor detects something
00161                 //Smooth turn left
00162                 MotorR.Forwards(0.2f);
00163                 MotorL.Forwards(0.8f);
00164 
00165             } else {
00166                 MotorR.Forwards(0.8f);
00167                 MotorL.Forwards(0.2f);
00168             }
00169 
00170         } else {
00171 
00172             if(TOFRange[3]<200) {
00173                 MotorR.Backwards(0.1f);
00174                 MotorL.Backwards(0.9f);
00175 
00176             } else if(TOFRange[1]<200) {
00177                 MotorR.Backwards(0.9f);
00178                 MotorL.Backwards(0.1f);
00179 
00180             } else {
00181                 MotorR.Backwards(0.1f);
00182                 MotorL.Backwards(0.9f);
00183             }
00184         }
00185 
00186     } else if(TOFRange[3]<200) {
00187         cBuzzer.Beep();
00188         cRGB_LED.blue_led();
00189         wait(0.02);
00190         cRGB_LED.display_power();
00191 
00192         if(TOFRange[1]<200) {
00193             MotorR.Forwards(1.0f);
00194             MotorL.Forwards(1.0f);
00195         } else {
00196 
00197             MotorR.Forwards(0.9f);
00198             MotorL.Forwards(0.1f);
00199         }
00200 
00201     } else if(TOFRange[1]<200) {
00202         cBuzzer.Beep();
00203         cRGB_LED.blue_led();
00204         wait(0.02);
00205         cRGB_LED.display_power();
00206         MotorR.Forwards(0.1f);
00207         MotorL.Forwards(0.9f);
00208 
00209     } else if(TOFRange[0]<200) {
00210         cBuzzer.Beep();
00211         cRGB_LED.blue_led();
00212         wait(0.02);
00213         cRGB_LED.display_power();
00214         MotorR.Forwards(1.0f);
00215         MotorL.Forwards(1.0f);
00216     }
00217 
00218     else {
00219         MotorR.Forwards(1.0f);
00220         MotorL.Forwards(1.0f);
00221     }
00222 }
00223 
00224 /*--------------------------------------------------------------------------------
00225 Function name: cMotor
00226 Input Parameters: PwmOut pwm - Motor PWM, DigitalOut fwd - Motor input1, DigitalOut rev - Motor Input2
00227 Output Parameters: N/A
00228 Description: Class constructor (Initialisation upon creating class)
00229 ----------------------------------------------------------------------------------*/
00230 cMotor::cMotor(PwmOut pwm, DigitalOut fwd, DigitalOut rev):_pwm(pwm), _fwd(fwd), _rev(rev)
00231 {
00232 
00233     // Set initial condition of PWM
00234     _pwm.period(0.001); //1KHz
00235     _pwm = 0;
00236 
00237     // Initial condition of output enables
00238     _fwd = 0;
00239     _rev = 0;
00240 }
00241 
00242 /*--------------------------------------------------------------------------------
00243 Function name: Forwards
00244 Input Parameters: float speed - PWM duty between 0-1
00245 Output Parameters: N/A
00246 Description: Drives the motor forwards at a designated speed
00247 ----------------------------------------------------------------------------------*/
00248 void cMotor::Forwards(float speed)
00249 {
00250     _fwd = 1;
00251     _rev = 0;
00252     _pwm = speed;
00253 }
00254 
00255 /*--------------------------------------------------------------------------------
00256 Function name: Backwards
00257 Input Parameters: float speed - PWM duty between 0-1
00258 Output Parameters: N/A
00259 Description: Drives the motor backwards at a designated speed
00260 ----------------------------------------------------------------------------------*/
00261 void cMotor::Backwards(float speed)
00262 {
00263     _fwd = 0;
00264     _rev = 1;
00265     _pwm = speed;
00266 }
00267 
00268 /*--------------------------------------------------------------------------------
00269 Function name: Stop
00270 Input Parameters: N/A
00271 Output Parameters: N/A
00272 Description: Drives the motor backwards at a designated speed
00273 ----------------------------------------------------------------------------------*/
00274 void cMotor::Stop()
00275 {
00276     _fwd = 0;
00277     _rev = 0;
00278     _pwm = 0;
00279 }
00280 
00281 
00282 
00283 /*--------------------------------------------------------------------------------
00284 Function name: ServoKeySub
00285 Input Parameters:
00286 Output Parameters:
00287 Description:
00288 ----------------------------------------------------------------------------------*/
00289 /* Ascii values for arrow Keys
00290 37(left arrow)
00291 38(up arrow)
00292 39(right arrow)
00293 40(down arrow)
00294 */
00295 
00296 void servoKeySub(const std_msgs::UInt8 &ServoKeyPress)
00297 {
00298     printf("%d", ServoKeyPress.data);
00299 
00300     if (ServoKeyPress.data == 106) {   // j for Pan Left
00301         if(Servo1Pos>-45) {
00302             Servo1Pos = Servo1Pos-IncSize;
00303             servo1.position(Servo1Pos);
00304             wait(0.01);
00305         }
00306     }
00307 
00308     else if (ServoKeyPress.data == 108) {   // l for Pan Right
00309         if(Servo1Pos<45) {
00310             Servo1Pos = Servo1Pos+IncSize;
00311             servo1.position(Servo1Pos);
00312             wait(0.01);
00313         }
00314     }
00315 
00316 
00317     else if (ServoKeyPress.data == 105) {   // i for Tilt Up
00318         if(Servo2Pos>-45){
00319         Servo2Pos = Servo2Pos-IncSize;
00320         servo2.position(Servo2Pos);
00321         wait(0.01);
00322         }
00323     }
00324 
00325     else if (ServoKeyPress.data == 107) {   // K for Tilt Down
00326         if(Servo2Pos<45){
00327         Servo2Pos = Servo2Pos+IncSize;
00328         servo2.position(Servo2Pos);
00329         wait(0.01);
00330          }
00331     }
00332 
00333     else if (ServoKeyPress.data == 44) {   //  for Up
00334         if(IncSize<20) {
00335             IncSize=(IncSize+1);
00336         }
00337     } else if (ServoKeyPress.data == 46) { //  for Up
00338         if(IncSize>1) {
00339             IncSize=(IncSize-1);
00340         }
00341     }
00342 
00343 
00344 
00345     pc.printf("Servo 1 = %d  Servo 2 = %d \n\r",Servo1Pos,Servo2Pos);
00346     servo1.position(Servo1Pos);
00347     servo2.position(Servo2Pos);
00348 
00349 }
00350 
00351