Diff: main.cpp

Revision:

8:78b1ff957cba

Parent:

7:85b8b5acfb22

Child:

9:9734347b5756

--- a/main.cpp	Fri Feb 21 00:00:43 2014 +0000
+++ b/main.cpp	Sat Feb 22 02:23:21 2014 +0000
@@ -1,69 +1,36 @@
 #include <mbed.h>
-#include "PololuEncoder.h"
-#include "Pacer.h"
-#include "motors.h"
-
-DigitalOut led1(LED1), led2(LED2);
-
-Serial pc(USBTX, USBRX);
+#include <Pacer.h>
 
-#define ENCODER1 0x00
-#define ENCODER2 0x01
-
-const PinName encoderPin1A = p6,
-              encoderPin1B = p7,
-              encoderPin2A = p8,
-              encoderPin2B = p9;
-
-PololuEncoderBuffer encoderBuffer;
-PololuEncoder encoder1(encoderPin1A, encoderPin1B, &encoderBuffer, ENCODER1);
-PololuEncoder encoder2(encoderPin2A, encoderPin2B, &encoderBuffer, ENCODER2);
+#include "motors.h"
+#include "encoders.h"
+#include "pc_serial.h"
+#include "leds.h"
 
 int main()
 {
     pc.baud(115200);
     
     // Enable pull-ups on encoder pins and give them a chance to settle.
-    DigitalIn(encoderPin1A).mode(PullUp);
-    DigitalIn(encoderPin1B).mode(PullUp);
-    DigitalIn(encoderPin2A).mode(PullUp);
-    DigitalIn(encoderPin2B).mode(PullUp);
-    wait_us(50);
-    encoder1.init();
-    encoder2.init();
-    
+    encoders_init();
     motors_init();
 
-    motors_speed_set(600, 1195);
+    // Test routines
+    motors_test();
+    encoders_test();
 
     Pacer reportPacer(500000);
     Pacer blinkPacer(200000);
-    uint32_t eventCount = 0;
-    uint32_t count = 0;
     while(1)
     {
         while(encoderBuffer.hasEvents())
         {
             PololuEncoderEvent event = encoderBuffer.readEvent();
-            eventCount += 1;
-            if (event == POLOLU_ENCODER_EVENT_ERR | ENCODER1)
-            {
-                pc.puts("error\n");   
-            }
-            else if (event == POLOLU_ENCODER_EVENT_INC | ENCODER1)
-            {
-                count += 1;   
-            }
-            else if (event == POLOLU_ENCODER_EVENT_DEC | ENCODER1)
-            {
-                count -= 1;
-            }
         }
         
         if(reportPacer.pace())
         {
             led2 = 1;
-            pc.printf("%8x\n", LPC_PWM1->MCR);
+            pc.printf("%8d %8d\n", encoder1.getCount(), encoder2.getCount());
             led2 = 0;
         }