Basic Library for Pololu DRV8835
Dependents: ME503_VehicleAssembly
DRV8835.cpp
00001 /* File: DRV8835.cpp 00002 * Adopted from work by Cameron Isbell 00003 * 00004 * Description: library for DRV8835 Motor Driver 00005 * Assumptions: A is left and B is right 00006 */ 00007 00008 #include "mbed.h" 00009 #include "DRV8835.h" 00010 00011 00012 DRV8835::DRV8835( PinName pinPwmL, PinName pinLin, 00013 PinName pinPwmR, PinName pinRin) : 00014 pwmL(pinPwmL), 00015 Lin(pinLin), 00016 pwmR(pinPwmR), 00017 Rin(pinRin) 00018 { 00019 Lin = 0; 00020 Rin = 0; 00021 pwmL.period(DRV8835_PWM_PERIOD_DEFAULT); 00022 pwmL = DRV8835_PWM_PULSEWIDTH_DEFAULT; 00023 pwmR.period(DRV8835_PWM_PERIOD_DEFAULT); 00024 pwmR = DRV8835_PWM_PULSEWIDTH_DEFAULT; 00025 motorL_stop(); 00026 motorR_stop(); 00027 } 00028 00029 void DRV8835::stop() 00030 { 00031 motorL_stop(); 00032 motorR_stop(); 00033 } 00034 00035 void DRV8835::motorL_stop(void) 00036 { 00037 pwmL = 0; 00038 } 00039 00040 void DRV8835::motorR_stop(void) 00041 { 00042 pwmR = 0; 00043 } 00044 00045 void DRV8835::setSpeeds(float Right,float Left) 00046 { 00047 //Set Right Speed and Direction 00048 if(Right<0) 00049 { 00050 motorR_rev(Right*-1); 00051 } else { 00052 motorR_fwd(Right); 00053 } 00054 00055 //Set Left Speed and Direction 00056 if(Left<0) 00057 { 00058 motorL_rev(Left*-1); 00059 } else { 00060 motorL_fwd(Left); 00061 } 00062 } 00063 00064 void DRV8835::motorL_fwd(float fPulsewidth) 00065 { 00066 Lin = 0; 00067 pwmL = fPulsewidth; 00068 } 00069 void DRV8835::motorL_rev(float fPulsewidth) 00070 { 00071 Lin = 1; 00072 pwmL = fPulsewidth; 00073 } 00074 00075 void DRV8835::motorR_fwd(float fPulsewidth) 00076 { 00077 Rin = 1; 00078 pwmR = fPulsewidth; 00079 } 00080 void DRV8835::motorR_rev(float fPulsewidth) 00081 { 00082 Rin = 0; 00083 pwmR = fPulsewidth; 00084 } 00085
Generated on Thu Jul 21 2022 03:31:57 by
1.7.2