Diff: PeripheralLayer/MotHatLib.cpp

Revision:

0:633cef71e6ba

Child:

1:a6bcf44b90df

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PeripheralLayer/MotHatLib.cpp	Sun Aug 09 09:37:55 2015 +0000
@@ -0,0 +1,397 @@
+#include "SysConfig.h"
+
+//Number of on-board devices
+const int LED_NUM_MAX = 1;
+const int MOT_NUM_MAX = 4;
+const int SRV_NUM_MAX = 4;
+const int VMON_NUM_MAX = 4;
+
+//The speed difference rate of left/right side motors when the vehicle turns head
+const int MOT_TURN_RATE = 4;
+
+//Fixed PWM period of servos
+const int SRV_PERIOD_US = 20000;
+
+//LEDs on mother board and daughter board
+DigitalOut led_mb(LED1, 0); //Nucleo
+//DigitalOut led_mb(PB_12, 1); //MOT HAT
+
+//GPIO for L293DD motor direction control
+//Forward = 2b'01, Backward = 2b'10, Stop = 2b'00
+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);
+
+//Analog inputs for power voltage monitoring
+AnalogIn vin_bat(PC_0); //ADC1_CH10
+AnalogIn vin_12v(PC_1); //ADC1_CH11
+AnalogIn vin_5v(PC_2);  //ADC1_CH12
+AnalogIn vin_3v3(PC_3);  //ADC1_CH13
+
+//TIM3 PWM channels
+PwmOut mot1_pwm(PB_5); //TIM3_CH2
+PwmOut mot2_pwm(PB_4); //TIM3_CH1
+PwmOut mot3_pwm(PB_0); //TIM3_CH3
+PwmOut mot4_pwm(PC_9); //TIM3_CH4
+
+//TIM15 PWM channels
+PwmOut servo1_pwm(PB_14); //TIM15_CH1
+PwmOut servo2_pwm(PB_15); //TIM15_CH2
+//TIM16 PWM channels
+PwmOut servo3_pwm(PA_6); //TIM17_CH1
+//TIM17 PWM channels
+PwmOut servo4_pwm(PA_7); //TIM17_CH1
+
+//Position calibration for servos
+//ServoCal[x][0] is -90 degrees, ServoCal[x][1] is +90 degrees
+const int ServoCal[][2] = {{1000, 2000}, {1000, 2000}, {1000, 2000}, {1000, 2000}};
+
+//Voltage division ratios
+const float VmonDivision[] = {0.1, 0.1, 0.5, 0.5};
+
+//Init board library
+//--> Success return 0
+int BoardLibInit(void)
+{    
+    //Init LED status
+    LedOnAll();
+        
+    //Init debug UART
+    //baud(115200);
+    
+    //Init DC motors
+    Mot_Init();
+    
+    //Init servos
+    Servo_Init();
+    
+    //Return 0 if success
+    return 0; 
+}
+
+/*********************************************/
+/*         Voltage Monitor Functions         */
+/*********************************************/
+float Vmon_ReadVolt(int channel)
+{    
+    switch(channel)
+    {
+        case 1:
+            return vin_bat.read() * 3.3 / VmonDivision[channel - 1];
+            
+        case 2:
+            return vin_12v.read() * 3.3 / VmonDivision[channel - 1];
+            
+        case 3:
+            return vin_5v.read() * 3.3 / VmonDivision[channel - 1];
+            
+        case 4:
+            return vin_3v3.read() * 3.3 / VmonDivision[channel - 1];
+            
+        default:
+            return 0;
+    }
+}
+
+
+/*********************************************/
+/*         DC Motor Control Functions        */
+/*********************************************/
+//Init the PWM and direction of motors to STOP state
+void Mot_Init(void)
+{
+    for(int motNum = 1; motNum <= MOT_NUM_MAX; motNum++)
+    {
+        Mot_PwmSetup(motNum, 1000, 0); //Freq = 1KHz, Speed = 0%
+        Mot_SetDirection(motNum, 's'); //Direction = STOP
+    }
+}
+
+//Control the movement of vehicle
+//dir: forward = 'f', backward = 'b', stop  = 's'
+//turn: left = 'l', right = 'r', fixed turning rate
+//speed: 0.0~1.0 (0~100%, 0.245 = 24.5%)
+void Mot_Ctrl(char dir, char turn, float speed)
+{
+    if(speed < 0)
+        speed = 0;
+    if(speed > 1)
+        speed = 1;
+    
+    switch(turn)
+    {
+        case 'l':
+            Mot_PwmWrite(1, speed / MOT_TURN_RATE);
+            Mot_SetDirection(1, dir);
+            Mot_PwmWrite(2, speed);
+            Mot_SetDirection(2, dir);
+            
+            if(MOT_NUM_MAX > 2)
+            {
+                Mot_PwmWrite(3, speed / MOT_TURN_RATE);
+                Mot_SetDirection(3, dir);
+                Mot_PwmWrite(4, speed);
+                Mot_SetDirection(4, dir); 
+            }            
+            break;
+            
+        case 'r':
+            Mot_PwmWrite(1, speed);
+            Mot_SetDirection(1, dir);
+            Mot_PwmWrite(2, speed / MOT_TURN_RATE);
+            Mot_SetDirection(2, dir);
+            
+            if(MOT_NUM_MAX > 2)
+            {
+                Mot_PwmWrite(3, speed);
+                Mot_SetDirection(3, dir);
+                Mot_PwmWrite(4, speed / MOT_TURN_RATE);
+                Mot_SetDirection(4, dir); 
+            }
+            break;
+            
+        case 's':
+            for(int motNum = 1; motNum <= MOT_NUM_MAX; motNum++)
+            {
+                Mot_PwmWrite(motNum, 0);
+                Mot_SetDirection(motNum, 's');
+            }
+            break;
+    }    
+}
+
+//channel: 1~4, period: us, duty: 0.0~1.0 (0.245 = 24.5%)
+void Mot_PwmSetup(int channel, int period, float duty)
+{
+    if(duty < 0)
+        duty = 0;
+    else if(duty > 1)
+        duty = 1;
+    
+    switch(channel)
+    {
+        case 1:
+            mot1_pwm.period_us(period);
+            mot1_pwm = duty;
+            break;
+        case 2:   
+            mot2_pwm.period_us(period);  
+            mot2_pwm = duty;
+            break;  
+        case 3:
+            mot3_pwm.period_us(period);
+            mot3_pwm = duty;
+            break;
+        case 4:
+            mot4_pwm.period_us(period);
+            mot4_pwm = duty;
+            break;
+            
+        default:
+            break;
+    }
+}
+
+//channel: 1~4, duty: 0.0~1.0 (0.245 = 24.5%)
+void Mot_PwmWrite(int channel, float duty)
+{
+    if(duty < 0)
+        duty = 0;
+    else if(duty > 1)
+        duty = 1;
+    
+    switch(channel)
+    {
+        //All channels in TIM3 share the same period
+        case 1:
+            mot1_pwm = duty;
+            break;
+        case 2:            
+            mot2_pwm = duty;           
+            break;
+        case 3:
+            mot3_pwm = duty;
+            break;
+        case 4:
+            mot4_pwm = duty;
+            break;    
+            
+        default:
+            break;
+    }
+}
+
+//channel: 1~4, dir: forward = 'f', backward = 'b', stop  = 's'
+void Mot_SetDirection(int channel, char dir)
+{
+    switch(channel)    
+    {
+        case 1:
+            if(dir == 'f')
+                mot1_dir = 0x1;
+            else if(dir == 'b')
+                mot1_dir = 0x2;
+            else
+                mot1_dir = 0x0;
+            break;
+        case 2:            
+            if(dir == 'f')
+                mot2_dir = 0x1;
+            else if(dir == 'b')
+                mot2_dir = 0x2;
+            else
+                mot2_dir = 0x0;        
+            break;
+        case 3:
+            if(dir == 'f')
+                mot3_dir = 0x1;
+            else if(dir == 'b')
+                mot3_dir = 0x2;
+            else
+                mot3_dir = 0x0;
+            break;
+        case 4:
+            if(dir == 'f')
+                mot4_dir = 0x1;
+            else if(dir == 'b')
+                mot4_dir = 0x2;
+            else
+                mot4_dir = 0x0;
+            break;      
+    }
+}
+
+/*********************************************/
+/*          Servo Control Functions          */
+/*********************************************/
+//Init the PWM and direction of motors to STOP state
+void Servo_Init(void)
+{
+    for(int servoNum = 1; servoNum <= SRV_NUM_MAX; servoNum++)
+    {
+        Servo_SetAngle(servoNum, 0);
+    }
+}
+
+//channel: 1~4, angle: -90.0~+90.0
+void Servo_SetAngle(int channel, float angle)
+{
+    if(channel > 0 && channel <= SRV_NUM_MAX)
+    {
+        int pulse = (int)((angle + 90.0) * (ServoCal[channel - 1][1] - ServoCal[channel-1][0]) / 180.0 + ServoCal[channel-1][0]);
+        Servo_PwmWrite(channel, pulse);
+    }
+}
+
+//channel: 1~4, pulse_us: 0~20000us
+void Servo_PwmSetup(int channel, int pulse_us)
+{  
+    if(pulse_us < 0)
+        pulse_us = 0;
+    else if(pulse_us > 20000)
+        pulse_us = 20000;
+          
+    switch(channel)
+    {
+        case 1:
+            servo1_pwm.period_us(SRV_PERIOD_US);
+            servo1_pwm.pulsewidth_us(pulse_us);
+            break;
+        case 2:   
+            servo2_pwm.period_us(SRV_PERIOD_US);  
+            servo2_pwm.pulsewidth_us(pulse_us);
+            break;  
+        case 3:
+            servo3_pwm.period_us(SRV_PERIOD_US);
+            servo3_pwm.pulsewidth_us(pulse_us);
+            break;
+        case 4:
+            servo4_pwm.period_us(SRV_PERIOD_US);
+            servo4_pwm.pulsewidth_us(pulse_us);
+            break;
+            
+        default:
+            break;
+    }
+}
+
+//channel: 1~4, pulse_us: 0~20000us
+void Servo_PwmWrite(int channel, int pulse_us)
+{
+    if(pulse_us < 0)
+        pulse_us = 0;
+    else if(pulse_us > 20000)
+        pulse_us = 20000;
+    
+    switch(channel)
+    {
+        //All channels in TIM3 share the same period
+        case 1:
+            servo1_pwm.pulsewidth_us(pulse_us);
+            break;
+        case 2:            
+            servo2_pwm.pulsewidth_us(pulse_us);
+            break;
+        case 3:
+            servo3_pwm.pulsewidth_us(pulse_us);
+            break;
+        case 4:
+            servo4_pwm.pulsewidth_us(pulse_us);
+            break;    
+            
+        default:
+            break;
+    }
+}
+
+/*********************************************/
+/*          LED Control Functions            */
+/*********************************************/
+void LedOn(int ch)
+{
+    switch(ch)
+    {
+        case 0:
+            led_mb = 1;
+            break;  
+                        
+        default:
+            break;   
+    }           
+}
+
+void LedOff(int ch)
+{
+    switch(ch)
+    {
+        case 0:
+            led_mb = 0;
+            break; 
+            
+        default:
+            break;   
+    }           
+}
+
+void LedToggle(int ch)
+{
+    switch(ch)
+    {
+        case 0:
+            led_mb = !led_mb;
+            break;  
+                        
+        default:
+            break;   
+    }           
+}
+
+void LedOnAll(void)
+{
+    for(int ledNum = 0; ledNum < LED_NUM_MAX; ledNum++)
+        LedOn(ledNum);    
+}
+
+void LedOffAll(void)
+{
+    for(int ledNum = 0; ledNum < LED_NUM_MAX; ledNum++)
+        LedOff(ledNum);    
+}