Diff: main.cpp

Revision:

2:b789f31e6d94

Parent:

1:ac4eff391f12

Child:

3:aa1a77b080eb

--- a/main.cpp	Sat Apr 07 02:22:00 2012 +0000
+++ b/main.cpp	Fri Nov 02 03:00:54 2012 +0000
@@ -12,15 +12,15 @@
 DigitalOut led2(LED2);
 
 //Serial          blueTooth(p13, p14);
-MODSERIAL       blueTooth(p13, p14);
+MODSERIAL       blueTooth(p13, p14); 
 Ticker          taskTimer;
 m3pi            base;
 
 // PID terms
-#define P_TERM    0.06f
+#define P_TERM    0.08f
 #define I_TERM    0.00f
-#define D_TERM    0.00f
-#define THRESHOLD 0.40f
+#define D_TERM    0.05f
+#define THRESHOLD 0.30f
 //PID                  pid(P_TERM, I_TERM, D_TERM, &blueTooth);
 PID                  pid(P_TERM, I_TERM, D_TERM);
 //IRObjDetector        ird(&blueTooth);
@@ -29,6 +29,14 @@
 Beacon               beacon;
 
 
+#define USR_POWERDOWN    (0x104)
+int semihost_powerdown()
+{
+    uint32_t arg;
+    return __semihost(USR_POWERDOWN, &arg);
+}
+
+
 void 
 taskFunc(void)
 {
@@ -90,7 +98,7 @@
     float power  = 0.00f;
     float bpower = 0.00f;
     float ipower = 0.00f;
-    float speed  = 0.20f;
+    float speed  = 0.50f;
     float max    = 1.00f;
     float right, left;
    
@@ -107,8 +115,10 @@
     base.reset();
     base.locate(0, 0);
     base.printf("IR Track");
-    wait(10);
-    
+    wait(5);
+
+    //semihost_powerdown(); // Disable the secondary controller, this reduces both power and noise.
+        
     blueTooth.baud(115200);
     blueTooth.printf("\r\nSystem start!\r\n");
  
@@ -118,7 +128,7 @@
     taskTimer.attach(&taskFunc, 0.10);
     
     blueTooth.printf("Find Beacon\r\n");
-    findBeacon();    
+    //findBeacon();    
     blueTooth.printf("Beacon Found!\r\n");
     wait(1);
     blueTooth.printf("Start Main Loop.\r\n");        
@@ -127,7 +137,7 @@
         blueTooth.printf("\e[1J");      // Clear terminal screen
         blueTooth.printf("\e[H");       // Home terminal screen
         blueTooth.printf("Main Loop Start.\r\n");
-        blueTooth.printf("MAIN: Battery Voltage:\t%1.2f\r\n", base.battery());    
+        //blueTooth.printf("MAIN: Battery Voltage:\t%1.2f\r\n", base.battery());    
         
         pid.dumpDebug(&blueTooth);
         irController.dumpDebug(&blueTooth);
@@ -140,9 +150,12 @@
         rssi      = beacon.get_max_rssi();
         
         bpower    = beaconPID.run(centeroid);
-    
+        bpower = 0;
+        
         blueTooth.printf("Beacon: centeroid = %3.2f\r\n", centeroid);
         blueTooth.printf("Beacon: power     = %3.2f\r\n", bpower);
+        blueTooth.printf("Beacon: rssiL     = 0x%2.2x\r\n", beacon.getL());
+        blueTooth.printf("Beacon: rssiR     = 0x%2.2x\r\n", beacon.getR());
         blueTooth.printf("Beacon: rssi      = 0x%2.2x\r\n", rssi);
                 
         prev_rssi = rssi;
@@ -151,7 +164,8 @@
             power = ipower;
         else
             power = bpower;
-        if (rssi > 0x7d)
+            
+        if (rssi > 0xFF)
         {
             speed = 0;
         }