MotHatLib.cpp

00001 #include "SysConfig.h"
00002 
00003 //Number of on-board devices
00004 const int LED_NUM_MAX = 1;
00005 const int MOT_NUM_MAX = 4;
00006 const int SRV_NUM_MAX = 4;
00007 const int VMON_NUM_MAX = 4;
00008 
00009 //The speed difference rate of left/right side motors when the vehicle turns head
00010 const int MOT_TURN_RATE = 4;
00011 
00012 //Fixed PWM period of servos
00013 const int SRV_PERIOD_US = 20000;
00014 
00015 //LEDs on mother board and daughter board
00016 //DigitalOut led_mb(LED1, 0); //Nucleo
00017 DigitalOut led_mb(PB_12, 1); //MOT HAT
00018 
00019 //USART2 to PC for debug
00020 Serial uart_pc(SERIAL_TX, SERIAL_RX);
00021 SerialDummy uart_dummy;
00022 
00023 //GPIO for L293DD motor direction control
00024 //Forward = 2b'01, Backward = 2b'10, Stop = 2b'00
00025 BusOut mot1_dir(PB_3, PA_11), mot2_dir(PA_15, PA_12), mot3_dir(PD_2, PC_12), mot4_dir(PB_6, PB_7);
00026 
00027 //GPIO for motor velocimeter
00028 InterruptIn spd1_in(PF_6), spd2_in(PF_7), spd3_in(PF_5), spd4_in(PC_4);
00029 //GPIO for ultrasound rangefinder
00030 InterruptIn us1_echo(PC_10), us2_echo(PC_5);
00031 DigitalOut us1_trig(PC_11), us2_trig(PF_4);
00032 
00033 //Timers for speed and distance counter
00034 Timer timer_spd, timer_us;
00035 
00036 //Analog inputs for power voltage monitoring
00037 AnalogIn vin_bat(PC_0); //ADC1_CH10
00038 AnalogIn vin_12v(PC_1); //ADC1_CH11
00039 AnalogIn vin_5v(PC_2);  //ADC1_CH12
00040 AnalogIn vin_3v3(PC_3);  //ADC1_CH13
00041 
00042 //TIM3 PWM channels
00043 PwmOut mot1_pwm(PB_5); //TIM3_CH2
00044 PwmOut mot2_pwm(PB_4); //TIM3_CH1
00045 PwmOut mot3_pwm(PB_0); //TIM3_CH3
00046 PwmOut mot4_pwm(PC_9); //TIM3_CH4
00047 
00048 //TIM15 PWM channels
00049 PwmOut servo1_pwm(PB_14); //TIM15_CH1
00050 PwmOut servo2_pwm(PB_15); //TIM15_CH2
00051 //TIM16 PWM channels
00052 PwmOut servo3_pwm(PB_8); //TIM16_CH1
00053 //TIM17 PWM channels
00054 PwmOut servo4_pwm(PB_9); //TIM17_CH1
00055 
00056 //Position calibration for servos
00057 //ServoCal[x][0] is -90 degrees, ServoCal[x][1] is +90 degrees
00058 const int ServoCal[][2] = {{1000, 2000}, {1000, 2000}, {1000, 2000}, {1000, 2000}};
00059 
00060 //Voltage division ratios
00061 const float VmonDivision[] = {0.1, 0.1, 0.5, 0.5};
00062 
00063 //Init board library
00064 //--> Success return 0
00065 int BoardLibInit(void)
00066 {    
00067     //Init LED status
00068     LedOnAll();
00069         
00070     //Init debug UART
00071     uart_pc.baud(115200);
00072     
00073     //Init DC motors
00074     Mot_Init();
00075     
00076     //Init servos
00077     Servo_Init();
00078     
00079     //Return 0 if success
00080     return 0; 
00081 }
00082 
00083 /*********************************************/
00084 /*         Voltage Monitor Functions         */
00085 /*********************************************/
00086 float Vmon_ReadVolt(int channel)
00087 {    
00088     switch(channel)
00089     {
00090         case 1:
00091             return vin_bat.read() * 3.3 / VmonDivision[channel - 1];
00092             
00093         case 2:
00094             return vin_12v.read() * 3.3 / VmonDivision[channel - 1];
00095             
00096         case 3:
00097             return vin_5v.read() * 3.3 / VmonDivision[channel - 1];
00098             
00099         case 4:
00100             return vin_3v3.read() * 3.3 / VmonDivision[channel - 1];
00101             
00102         default:
00103             return 0;
00104     }
00105 }
00106 
00107 
00108 /*********************************************/
00109 /*         DC Motor Control Functions        */
00110 /*********************************************/
00111 //Init the PWM and direction of motors to STOP state
00112 void Mot_Init(void)
00113 {
00114     for(int motNum = 1; motNum <= MOT_NUM_MAX; motNum++)
00115     {
00116         Mot_PwmSetup(motNum, 1000, 0); //Freq = 1KHz, Speed = 0%
00117         Mot_SetDirection(motNum, 's'); //Direction = STOP
00118     }
00119 }
00120 
00121 //Control the movement of vehicle
00122 //dir: forward = 'f', backward = 'b', stop  = 's'
00123 //turn: left = 'l', right = 'r', fixed turning rate
00124 //speed: 0.0~1.0 (0~100%, 0.245 = 24.5%)
00125 void Mot_Ctrl(char dir, char turn, float speed)
00126 {
00127     if(speed < 0)
00128         speed = 0;
00129     if(speed > 1)
00130         speed = 1;
00131     
00132     switch(turn)
00133     {
00134         case 'm':
00135             Mot_PwmWrite(1, speed);
00136             Mot_SetDirection(1, dir);
00137             Mot_PwmWrite(2, speed);
00138             Mot_SetDirection(2, dir);
00139             
00140             if(MOT_NUM_MAX > 2)
00141             {
00142                 Mot_PwmWrite(3, speed);
00143                 Mot_SetDirection(3, dir);
00144                 Mot_PwmWrite(4, speed);
00145                 Mot_SetDirection(4, dir); 
00146             }            
00147             break;
00148             
00149         case 'l':
00150             Mot_PwmWrite(1, speed / MOT_TURN_RATE);
00151             Mot_SetDirection(1, dir);
00152             Mot_PwmWrite(2, speed);
00153             Mot_SetDirection(2, dir);
00154             
00155             if(MOT_NUM_MAX > 2)
00156             {
00157                 Mot_PwmWrite(3, speed / MOT_TURN_RATE);
00158                 Mot_SetDirection(3, dir);
00159                 Mot_PwmWrite(4, speed);
00160                 Mot_SetDirection(4, dir); 
00161             }            
00162             break;
00163             
00164         case 'r':
00165             Mot_PwmWrite(1, speed);
00166             Mot_SetDirection(1, dir);
00167             Mot_PwmWrite(2, speed / MOT_TURN_RATE);
00168             Mot_SetDirection(2, dir);
00169             
00170             if(MOT_NUM_MAX > 2)
00171             {
00172                 Mot_PwmWrite(3, speed);
00173                 Mot_SetDirection(3, dir);
00174                 Mot_PwmWrite(4, speed / MOT_TURN_RATE);
00175                 Mot_SetDirection(4, dir); 
00176             }
00177             break;
00178             
00179         case 's':
00180             for(int motNum = 1; motNum <= MOT_NUM_MAX; motNum++)
00181             {
00182                 Mot_PwmWrite(motNum, 0);
00183                 Mot_SetDirection(motNum, 's');
00184             }
00185             break;
00186     }    
00187 }
00188 
00189 //channel: 1~4, period: us, duty: 0.0~1.0 (0.245 = 24.5%)
00190 void Mot_PwmSetup(int channel, int period, float duty)
00191 {
00192     if(duty < 0)
00193         duty = 0;
00194     else if(duty > 1)
00195         duty = 1;
00196     
00197     switch(channel)
00198     {
00199         case 1:
00200             mot1_pwm.period_us(period);
00201             mot1_pwm = duty;
00202             break;
00203         case 2:   
00204             mot2_pwm.period_us(period);  
00205             mot2_pwm = duty;
00206             break;  
00207         case 3:
00208             mot3_pwm.period_us(period);
00209             mot3_pwm = duty;
00210             break;
00211         case 4:
00212             mot4_pwm.period_us(period);
00213             mot4_pwm = duty;
00214             break;
00215             
00216         default:
00217             break;
00218     }
00219 }
00220 
00221 //channel: 1~4, duty: 0.0~1.0 (0.245 = 24.5%)
00222 void Mot_PwmWrite(int channel, float duty)
00223 {
00224     if(duty < 0)
00225         duty = 0;
00226     else if(duty > 1)
00227         duty = 1;
00228     
00229     switch(channel)
00230     {
00231         //All channels in TIM3 share the same period
00232         case 1:
00233             mot1_pwm = duty;
00234             break;
00235         case 2:            
00236             mot2_pwm = duty;           
00237             break;
00238         case 3:
00239             mot3_pwm = duty;
00240             break;
00241         case 4:
00242             mot4_pwm = duty;
00243             break;    
00244             
00245         default:
00246             break;
00247     }
00248 }
00249 
00250 //channel: 1~4, dir: forward = 'f', backward = 'b', stop  = 's'
00251 void Mot_SetDirection(int channel, char dir)
00252 {
00253     switch(channel)    
00254     {
00255         case 1:
00256             if(dir == 'f')
00257                 mot1_dir = 0x1;
00258             else if(dir == 'b')
00259                 mot1_dir = 0x2;
00260             else
00261                 mot1_dir = 0x0;
00262             break;
00263         case 2:            
00264             if(dir == 'f')
00265                 mot2_dir = 0x1;
00266             else if(dir == 'b')
00267                 mot2_dir = 0x2;
00268             else
00269                 mot2_dir = 0x0;        
00270             break;
00271         case 3:
00272             if(dir == 'f')
00273                 mot3_dir = 0x1;
00274             else if(dir == 'b')
00275                 mot3_dir = 0x2;
00276             else
00277                 mot3_dir = 0x0;
00278             break;
00279         case 4:
00280             if(dir == 'f')
00281                 mot4_dir = 0x1;
00282             else if(dir == 'b')
00283                 mot4_dir = 0x2;
00284             else
00285                 mot4_dir = 0x0;
00286             break;      
00287     }
00288 }
00289 
00290 /*********************************************/
00291 /*        Motor Velocimetoer Functions       */
00292 /*********************************************/
00293 int MotSpdPulseTime[] = {0, 0, 0, 0};
00294 int MotSpeed[] = {0, 0, 0, 0};
00295 
00296 void Mot_StartVelocimeter(void)
00297 {
00298     timer_spd.start(); 
00299     
00300     spd1_in.rise(&Mot_SpdPluseIntHandler1);
00301     spd2_in.rise(&Mot_SpdPluseIntHandler2);
00302     spd3_in.rise(&Mot_SpdPluseIntHandler3);
00303     spd4_in.rise(&Mot_SpdPluseIntHandler4);    
00304 }
00305 
00306 void Mot_StopVelocimeter(void)
00307 {
00308     spd1_in.disable_irq();
00309     spd2_in.disable_irq();
00310     spd3_in.disable_irq();
00311     spd4_in.disable_irq();
00312     
00313     timer_spd.stop();
00314     for(int i=1; i<=MOT_NUM_MAX; i++)
00315         MotSpdPulseTime[i-1] = 0;    
00316 }
00317 
00318 int Mot_GetSpeed(int channel)
00319 {
00320     if(channel < 1)
00321         channel = 1;
00322     else if(channel > MOT_NUM_MAX)
00323         channel = MOT_NUM_MAX;
00324         
00325     return MotSpeed[channel - 1];
00326 }
00327 
00328 void Mot_SpdPluseIntHandler1(void)
00329 {
00330     int time = timer_spd.read_ms();
00331     MotSpeed[0] = 3000 / (time - MotSpdPulseTime[0]);
00332     MotSpdPulseTime[0] = time;
00333 }
00334 
00335 void Mot_SpdPluseIntHandler2(void)
00336 {
00337     int time = timer_spd.read_ms();
00338     MotSpeed[1] = 3000 / (time - MotSpdPulseTime[1]);
00339     MotSpdPulseTime[1] = time;
00340 }
00341 
00342 void Mot_SpdPluseIntHandler3(void)
00343 {
00344     int time = timer_spd.read_ms();
00345     MotSpeed[2] = 3000 / (time - MotSpdPulseTime[2]);
00346     MotSpdPulseTime[2] = time;
00347 }
00348 
00349 void Mot_SpdPluseIntHandler4(void)
00350 {
00351     int time = timer_spd.read_ms();
00352     MotSpeed[3] = 3000 / (time - MotSpdPulseTime[3]);
00353     MotSpdPulseTime[3] = time;
00354 }
00355 
00356 /*********************************************/
00357 /*          Servo Control Functions          */
00358 /*********************************************/
00359 //Init the PWM and direction of motors to STOP state
00360 void Servo_Init(void)
00361 {
00362     for(int servoNum = 1; servoNum <= SRV_NUM_MAX; servoNum++)
00363     {
00364         Servo_SetAngle(servoNum, 0);
00365     }
00366 }
00367 
00368 //channel: 1~4, angle: -90.0~+90.0
00369 void Servo_SetAngle(int channel, float angle)
00370 {
00371     if(channel > 0 && channel <= SRV_NUM_MAX)
00372     {
00373         int pulse = (int)((angle + 90.0) * (ServoCal[channel - 1][1] - ServoCal[channel-1][0]) / 180.0 + ServoCal[channel-1][0]);
00374         Servo_PwmWrite(channel, pulse);
00375     }
00376 }
00377 
00378 //channel: 1~4, pulse_us: 0~20000us
00379 void Servo_PwmSetup(int channel, int pulse_us)
00380 {  
00381     if(pulse_us < 0)
00382         pulse_us = 0;
00383     else if(pulse_us > 20000)
00384         pulse_us = 20000;
00385           
00386     switch(channel)
00387     {
00388         case 1:
00389             servo1_pwm.period_us(SRV_PERIOD_US);
00390             servo1_pwm.pulsewidth_us(pulse_us);
00391             break;
00392         case 2:   
00393             servo2_pwm.period_us(SRV_PERIOD_US);  
00394             servo2_pwm.pulsewidth_us(pulse_us);
00395             break;  
00396         case 3:
00397             servo3_pwm.period_us(SRV_PERIOD_US);
00398             servo3_pwm.pulsewidth_us(pulse_us);
00399             break;
00400         case 4:
00401             servo4_pwm.period_us(SRV_PERIOD_US);
00402             servo4_pwm.pulsewidth_us(pulse_us);
00403             break;
00404             
00405         default:
00406             break;
00407     }
00408 }
00409 
00410 //channel: 1~4, pulse_us: 0~20000us
00411 void Servo_PwmWrite(int channel, int pulse_us)
00412 {
00413     if(pulse_us < 0)
00414         pulse_us = 0;
00415     else if(pulse_us > 20000)
00416         pulse_us = 20000;
00417     
00418     switch(channel)
00419     {
00420         //All channels in TIM3 share the same period
00421         case 1:
00422             servo1_pwm.pulsewidth_us(pulse_us);
00423             break;
00424         case 2:            
00425             servo2_pwm.pulsewidth_us(pulse_us);
00426             break;
00427         case 3:
00428             servo3_pwm.pulsewidth_us(pulse_us);
00429             break;
00430         case 4:
00431             servo4_pwm.pulsewidth_us(pulse_us);
00432             break;    
00433             
00434         default:
00435             break;
00436     }
00437 }
00438 
00439 /*********************************************/
00440 /*      Ultrasound Rangefinder Functions     */
00441 /*********************************************/
00442 //xxx
00443 
00444 
00445 /*********************************************/
00446 /*          LED Control Functions            */
00447 /*********************************************/
00448 void LedOn(int ch)
00449 {
00450     switch(ch)
00451     {
00452         case 0:
00453             led_mb = 0;
00454             break;  
00455                         
00456         default:
00457             break;   
00458     }           
00459 }
00460 
00461 void LedOff(int ch)
00462 {
00463     switch(ch)
00464     {
00465         case 0:
00466             led_mb = 1;
00467             break; 
00468             
00469         default:
00470             break;   
00471     }           
00472 }
00473 
00474 void LedToggle(int ch)
00475 {
00476     switch(ch)
00477     {
00478         case 0:
00479             led_mb = !led_mb;
00480             break;  
00481                         
00482         default:
00483             break;   
00484     }           
00485 }
00486 
00487 void LedOnAll(void)
00488 {
00489     for(int ledNum = 0; ledNum < LED_NUM_MAX; ledNum++)
00490         LedOn(ledNum);    
00491 }
00492 
00493 void LedOffAll(void)
00494 {
00495     for(int ledNum = 0; ledNum < LED_NUM_MAX; ledNum++)
00496         LedOff(ledNum);    
00497 }