Diff: PeripheralLayer/MotHatLib.cpp

Revision:

3:171f4d0ca77b

Parent:

1:a6bcf44b90df

--- a/PeripheralLayer/MotHatLib.cpp	Tue Aug 18 11:03:37 2015 +0000
+++ b/PeripheralLayer/MotHatLib.cpp	Thu Aug 20 11:36:01 2015 +0000
@@ -16,6 +16,10 @@
 //DigitalOut led_mb(LED1, 0); //Nucleo
 DigitalOut led_mb(PB_12, 1); //MOT HAT
 
+//USART2 to PC for debug
+Serial uart_pc(SERIAL_TX, SERIAL_RX);
+SerialDummy uart_dummy;
+
 //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);
@@ -45,9 +49,9 @@
 PwmOut servo1_pwm(PB_14); //TIM15_CH1
 PwmOut servo2_pwm(PB_15); //TIM15_CH2
 //TIM16 PWM channels
-PwmOut servo3_pwm(PA_6); //TIM17_CH1
+PwmOut servo3_pwm(PB_8); //TIM16_CH1
 //TIM17 PWM channels
-PwmOut servo4_pwm(PA_7); //TIM17_CH1
+PwmOut servo4_pwm(PB_9); //TIM17_CH1
 
 //Position calibration for servos
 //ServoCal[x][0] is -90 degrees, ServoCal[x][1] is +90 degrees
@@ -64,7 +68,7 @@
     LedOnAll();
         
     //Init debug UART
-    //baud(115200);
+    uart_pc.baud(115200);
     
     //Init DC motors
     Mot_Init();
@@ -307,8 +311,8 @@
     spd4_in.disable_irq();
     
     timer_spd.stop();
-    for(int i=0; i<=MOT_NUM_MAX; i++)
-        MotSpdPulseTime[i] = 0;    
+    for(int i=1; i<=MOT_NUM_MAX; i++)
+        MotSpdPulseTime[i-1] = 0;    
 }
 
 int Mot_GetSpeed(int channel)
@@ -446,7 +450,7 @@
     switch(ch)
     {
         case 0:
-            led_mb = 1;
+            led_mb = 0;
             break;  
                         
         default:
@@ -459,7 +463,7 @@
     switch(ch)
     {
         case 0:
-            led_mb = 0;
+            led_mb = 1;
             break; 
             
         default: