Diff: PeripheralLayer/MotHatLib.cpp

Revision:

1:a6bcf44b90df

Parent:

0:633cef71e6ba

Child:

3:171f4d0ca77b

diff -r 633cef71e6ba -r a6bcf44b90df PeripheralLayer/MotHatLib.cpp
--- a/PeripheralLayer/MotHatLib.cpp	Sun Aug 09 09:37:55 2015 +0000
+++ b/PeripheralLayer/MotHatLib.cpp	Tue Aug 18 01:44:45 2015 +0000
@@ -13,13 +13,22 @@
 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
+//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);
 
+//GPIO for motor velocimeter
+InterruptIn spd1_in(PF_6), spd2_in(PF_7), spd3_in(PF_5), spd4_in(PC_4);
+//GPIO for ultrasound rangefinder
+InterruptIn us1_echo(PC_10), us2_echo(PC_5);
+DigitalOut us1_trig(PC_11), us2_trig(PF_4);
+
+//Timers for speed and distance counter
+Timer timer_spd, timer_us;
+
 //Analog inputs for power voltage monitoring
 AnalogIn vin_bat(PC_0); //ADC1_CH10
 AnalogIn vin_12v(PC_1); //ADC1_CH11
@@ -118,6 +127,21 @@
     
     switch(turn)
     {
+        case 'm':
+            Mot_PwmWrite(1, speed);
+            Mot_SetDirection(1, dir);
+            Mot_PwmWrite(2, speed);
+            Mot_SetDirection(2, dir);
+            
+            if(MOT_NUM_MAX > 2)
+            {
+                Mot_PwmWrite(3, speed);
+                Mot_SetDirection(3, dir);
+                Mot_PwmWrite(4, speed);
+                Mot_SetDirection(4, dir); 
+            }            
+            break;
+            
         case 'l':
             Mot_PwmWrite(1, speed / MOT_TURN_RATE);
             Mot_SetDirection(1, dir);
@@ -260,6 +284,72 @@
 }
 
 /*********************************************/
+/*        Motor Velocimetoer Functions       */
+/*********************************************/
+int MotSpdPulseTime[] = {0, 0, 0, 0};
+int MotSpeed[] = {0, 0, 0, 0};
+
+void Mot_StartVelocimeter(void)
+{
+    timer_spd.start(); 
+    
+    spd1_in.rise(&Mot_SpdPluseIntHandler1);
+    spd2_in.rise(&Mot_SpdPluseIntHandler2);
+    spd3_in.rise(&Mot_SpdPluseIntHandler3);
+    spd4_in.rise(&Mot_SpdPluseIntHandler4);    
+}
+
+void Mot_StopVelocimeter(void)
+{
+    spd1_in.disable_irq();
+    spd2_in.disable_irq();
+    spd3_in.disable_irq();
+    spd4_in.disable_irq();
+    
+    timer_spd.stop();
+    for(int i=0; i<=MOT_NUM_MAX; i++)
+        MotSpdPulseTime[i] = 0;    
+}
+
+int Mot_GetSpeed(int channel)
+{
+    if(channel < 1)
+        channel = 1;
+    else if(channel > MOT_NUM_MAX)
+        channel = MOT_NUM_MAX;
+        
+    return MotSpeed[channel - 1];
+}
+
+void Mot_SpdPluseIntHandler1(void)
+{
+    int time = timer_spd.read_ms();
+    MotSpeed[0] = 3000 / (time - MotSpdPulseTime[0]);
+    MotSpdPulseTime[0] = time;
+}
+
+void Mot_SpdPluseIntHandler2(void)
+{
+    int time = timer_spd.read_ms();
+    MotSpeed[1] = 3000 / (time - MotSpdPulseTime[1]);
+    MotSpdPulseTime[1] = time;
+}
+
+void Mot_SpdPluseIntHandler3(void)
+{
+    int time = timer_spd.read_ms();
+    MotSpeed[2] = 3000 / (time - MotSpdPulseTime[2]);
+    MotSpdPulseTime[2] = time;
+}
+
+void Mot_SpdPluseIntHandler4(void)
+{
+    int time = timer_spd.read_ms();
+    MotSpeed[3] = 3000 / (time - MotSpdPulseTime[3]);
+    MotSpdPulseTime[3] = time;
+}
+
+/*********************************************/
 /*          Servo Control Functions          */
 /*********************************************/
 //Init the PWM and direction of motors to STOP state
@@ -343,6 +433,12 @@
 }
 
 /*********************************************/
+/*      Ultrasound Rangefinder Functions     */
+/*********************************************/
+//xxx
+
+
+/*********************************************/
 /*          LED Control Functions            */
 /*********************************************/
 void LedOn(int ch)