Deze moet je hebben Menner Dennis

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers motorDriver.cpp Source File

motorDriver.cpp

00001 /* This code is written for EMILY the robot Waitress */
00002 #include "motorDriver.h"
00003 /*============================================================================*/
00004 //                            acceleration speed in ms
00005 int accSpeed = 500;  
00006 int BatteryPubTime =2000;
00007 
00008 
00009 /*============================================================================*/
00010 
00011 
00012 motorDriver::motorDriver(PinName Left1, PinName Left2, PinName Right1, PinName Right2, PinName PWML, PinName PWMR /*, PinName spdReadL, PinName spdReadR, PinName Bat*/):  
00013 speedLeft1(Left1), speedLeft2(Left2),  speedRight1(Right1),  speedRight2(Right2) , pwmL(PWML), pwmR(PWMR)/*, speedReadL(spdReadL), speedReadR(spdReadR), BatteryRead(Bat)*/
00014 {
00015     //initial conditions of PWM left and right
00016        pwmL.period_ms(10);
00017        pwmR.period_ms(10);
00018        
00019     //initial conditions of the left motor 
00020        speedLeft1= 0;
00021        speedLeft2= 0;
00022         
00023     //initial conditions of the right motor 
00024        speedRight1=0;
00025        speedRight2=0;
00026        
00027 }
00028 
00029 
00030 
00031 
00032 void motorDriver::setDirection(bool  keyForward,  bool  keyBackward, bool  keyLeft, bool  keyRight) 
00033 {
00034     if(keyForward && !keyBackward && !keyLeft && !keyRight)   // forward 
00035     { 
00036           speedLeft2  = 0;
00037           speedRight2 = 0;
00038         
00039           if(speedLeft1 < 1 && speedRight1 <1)
00040           {
00041                  pwmL.pulsewidth_ms(0);
00042                  pwmR.pulsewidth_ms(0);
00043                  speedRight1 = 1;
00044                  speedLeft1  = 1;
00045               
00046                  for(int i=0; i<=10; i++)
00047                  {
00048                   pwmL.pulsewidth_ms(i);
00049                   pwmR.pulsewidth_ms(i);   
00050                   Thread::wait(accSpeed);                                  
00051                  }
00052               
00053           }
00054           
00055           if(speedLeft1 && speedRight1)
00056           {
00057              speedLeft1  = 1;
00058              speedRight1 = 1; 
00059            
00060           }       
00061     }   
00062     
00063 
00064 if(!keyForward && keyBackward && !keyLeft && !keyRight)   // Backwards
00065     { 
00066           speedLeft1  = 0;
00067           speedRight1 = 0;
00068         
00069           if(speedLeft2 < 1 && speedRight2 <1)
00070           {
00071                  pwmL.pulsewidth_ms(0);
00072                  pwmR.pulsewidth_ms(0);
00073                  speedRight2 = 1;
00074                  speedLeft2  = 1;
00075               
00076                  for(int i=0; i<=10; i++)
00077                  {
00078                   pwmL.pulsewidth_ms(i);
00079                   pwmR.pulsewidth_ms(i);   
00080                   Thread::wait(accSpeed);                                            
00081                  }
00082               
00083           }
00084           
00085           if(speedLeft2 && speedRight2)
00086           {
00087              speedLeft1  = 1;
00088              speedRight1 = 1; 
00089              Thread::wait(accSpeed);   
00090           }    
00091               
00092     }   
00093     
00094     if(!keyForward && !keyBackward && keyLeft && !keyRight)   // Left
00095     { 
00096           speedLeft1  = 0;
00097           speedLeft2  = 0;
00098           speedRight2 = 0;
00099         
00100           if(speedRight1 <1)
00101           {
00102                  pwmR.pulsewidth_ms(0);
00103                  speedRight1 = 1;
00104               
00105                  for(int i=0; i<=10; i++)
00106                  {
00107                   pwmR.pulsewidth_ms(i);   
00108                   Thread::wait(accSpeed);                                            
00109                  }
00110               
00111           }
00112           
00113           if(speedRight1)
00114           {
00115              speedRight1 = 1; 
00116              Thread::wait(accSpeed);   
00117           }
00118     }
00119           
00120     if(!keyForward && !keyBackward && !keyLeft && keyRight)   // Right
00121     { 
00122           speedRight1 = 0;
00123           speedRight2 = 0;
00124           speedLeft2  = 0;
00125         
00126           if(speedLeft1 <1)
00127           {
00128                  pwmL.pulsewidth_ms(0);
00129                  speedLeft1=1;
00130               
00131                  for(int i=0; i<=10; i++)
00132                  {
00133                   pwmL.pulsewidth_ms(i);   
00134                   Thread::wait(accSpeed);                                            
00135                  }
00136               
00137           }
00138           
00139              if(speedLeft1)
00140           {
00141              speedLeft1 = 1; 
00142              Thread::wait(accSpeed);      
00143           }       
00144       }     
00145  }
00146     
00147 
00148 //
00149 //
00150 //
00151 //void motorDriver::getBattery()
00152 //{
00153 //    
00154 //    ros::Publisher pub_battery("BatteryVoltage", &value_msg);
00155 //    nh.initNode();
00156 //    nh.advertise(pub_battery);
00157 //    
00158 //    float BatteryValue = 0;
00159 //    float avg_value = 0;
00160 //   
00161 //    while(1)
00162 //    {
00163 //        for( int i = 0; i < 10; i++)
00164 //       {
00165 //            avg_value += BatteryRead;          // uitlezen spanning
00166 //       }
00167 //       
00168 //        BatteryValue = (avg_value / 10);        // delen door 10 want 10 samples
00169 //        BatteryValue = BatteryValue * 3.3;     // *3.3 Vref
00170 //        value_msg.data = BatteryValue;
00171 //        pub_battery.publish(&value_msg);
00172 //        Thread::wait(BatteryPubTime);
00173 //        nh.spinOnce(); 
00174 //        break;
00175 //        
00176 //    }
00177 //}