Diff: motors.cpp

Revision:

8:78b1ff957cba

Parent:

7:85b8b5acfb22

Child:

9:9734347b5756

--- a/motors.cpp	Fri Feb 21 00:00:43 2014 +0000
+++ b/motors.cpp	Sat Feb 22 02:23:21 2014 +0000
@@ -4,18 +4,29 @@
 // Application     mbed pin    LPC1768
 // Motor 1 PWM     p26         P2[0]/PWM1[1]
 // Motor 1 dir     p25
-// Motor 2 PWM     p27         P2[2]/PWM1[3]
-// Motor 2 dir     p28
+// Motor 2 PWM     p24         P2[2]/PWM1[3]
+// Motor 2 dir     p23
 
-static PwmOut motor1Pwm(p26);
+// Clock structure:
+// System clock: 96 MHz
+// In LPC_SC->PCLKSEL0, PWM is assigned to system clock / 4 by default, so it ticks at 24 MHz.
+// This allows us to have 1200 possible speeds at 20 kHz.
+
 DigitalOut motor1Dir(p25);
-
-static PwmOut motor2Pwm(p24);
 DigitalOut motor2Dir(p23);
 
 void motors_init()
 {
-    motor1Pwm.period_us(100);
+    //PwmOut(p26).period_us(100);
+    
+    // Power the PWM module by setting PCPWM1 bit in PCONP register.  (Table 46).
+    LPC_SC->PCONP |= (1 << 6);
+    
+    // In PCLKSEL0 register, set the clock for PWM1 to be equal to CCLK/4 (96/4 = 24 MHz).
+    LPC_SC->PCLKSEL0 &= ~(3 << 12);
+
+    // Select the functions of P2.0 and P2.2 as PWM.  (Table 83).
+    LPC_PINCON->PINSEL4 = (LPC_PINCON->PINSEL4 & ~((3 << 0) | (3 << 4))) | ((1 << 0) | (1 << 4));
     
     // Set most parts of the PWM module to their defaults.
     LPC_PWM1->TCR = 0;
@@ -32,12 +43,31 @@
     motors_speed_set(0, 0);
     
     LPC_PWM1->TCR = (1 << 0) | (1 << 3);   // Enable the PWM counter and enable PWM.
-
 }
 
 void motors_speed_set(int16_t motor1_speed, int16_t motor2_speed)
 {
+    if (motor1_speed < 0)
+    {
+        motor1_speed = -motor1_speed;
+        motor1Dir = 0;   
+    }
+    else
+    {
+        motor1Dir = 1;    
+    }
     LPC_PWM1->MR1 = motor1_speed;
+    
+    if (motor2_speed < 0)
+    {
+        motor2_speed = -motor2_speed;   
+        motor2Dir = 0;
+    }
+    else
+    {
+        motor2Dir = 1;
+    }
     LPC_PWM1->MR3 = motor2_speed;
+    
     LPC_PWM1->LER |= (1<<1) | (1<<3);
 }