Revision 3:aa1a77b080eb, committed 2012-11-03

Comitter:

mpanetta

Date:

Sat Nov 03 01:03:33 2012 +0000

Parent:

2:b789f31e6d94

Child:

4:369caebdf5dc

Commit message:

This code will find the beacon and stop.

Changed in this revision

--- a/IRBehaviorController.cpp	Fri Nov 02 03:00:54 2012 +0000
+++ b/IRBehaviorController.cpp	Sat Nov 03 01:03:33 2012 +0000
@@ -45,9 +45,9 @@
     
     // Centeroid value needs to be split for avoidance mode.
     if (tmp < 0)
-        tmp += 3;
+        tmp += 3.0f;
     else
-        tmp -=3;
+        tmp -= 3.0f;
         
     if (brightness > activationThreshold)
         output = pid->run(tmp);    

--- a/PID.cpp	Fri Nov 02 03:00:54 2012 +0000
+++ b/PID.cpp	Sat Nov 03 01:03:33 2012 +0000
@@ -67,13 +67,19 @@
     error = in;
     
     v_p  = error;
-    v_i += v_p;
+    
+    if (k_i != 0)
+        v_i += v_p;
+    else
+        v_i = 0;
+        
     v_d  = error - prev_error;    
     
     if (v_i > clip)
         v_i = clip;
-    if (v_i < clip)
+    if (v_i < -clip)
         v_i = -clip;
+        
     if (v_i != v_i) // NAN check...
         v_i = 0;
         

--- a/PiSlingers.lib	Fri Nov 02 03:00:54 2012 +0000
+++ b/PiSlingers.lib	Sat Nov 03 01:03:33 2012 +0000
@@ -1,1 +1,1 @@
-PiSlingers#695f4f4442d3
+PiSlingers#a9351d7f92b4

--- a/main.cpp	Fri Nov 02 03:00:54 2012 +0000
+++ b/main.cpp	Sat Nov 03 01:03:33 2012 +0000
@@ -17,10 +17,11 @@
 m3pi            base;
 
 // PID terms
-#define P_TERM    0.08f
+#define P_TERM    0.10f
 #define I_TERM    0.00f
 #define D_TERM    0.05f
-#define THRESHOLD 0.30f
+#define THRESHOLD 0.40f
+
 //PID                  pid(P_TERM, I_TERM, D_TERM, &blueTooth);
 PID                  pid(P_TERM, I_TERM, D_TERM);
 //IRObjDetector        ird(&blueTooth);
@@ -53,7 +54,7 @@
 {
     uint8_t check[8]  = {0, 0, 0, 0, 0, 0, 0, 0 };
     uint8_t done      = 0;
-    uint8_t threshold = 0x60;
+    uint8_t threshold = 90;
     
     base.left_motor(0.2);
     base.right_motor(-0.2);
@@ -71,7 +72,7 @@
             tmp = beacon.get_max_rssi();
             //blueTooth.printf("\r\nBeacon scan %d: rssi = 0x%2.2x\r\n", i, tmp);    
             check[i] = tmp;
-            //wait(0.1);
+            wait(0.1);
         }
         
         // Check all readings against threshold, stop turning and exit search loop when all readings are above threshold.
@@ -89,6 +90,7 @@
 #define BEACON_P_TERM 1.0f
 #define BEACON_I_TERM 0.0f
 #define BEACON_D_TERM 0.0f
+#define GO_SPEED 0.8f
 
 PID beaconPID(BEACON_P_TERM, BEACON_I_TERM, BEACON_D_TERM);
 
@@ -98,14 +100,14 @@
     float power  = 0.00f;
     float bpower = 0.00f;
     float ipower = 0.00f;
-    float speed  = 0.50f;
+    float speed  = GO_SPEED;
     float max    = 1.00f;
     float right, left;
    
     float centeroid;
             
-    uint8_t rssi      = 0x00;
-    uint8_t prev_rssi = 0x00;
+    uint16_t rssi      = 0x00;
+    uint16_t prev_rssi = 0x00;
     
 
     
@@ -124,6 +126,8 @@
  
     irController.setActiveThreshold(THRESHOLD);
     
+    //beacon.calibrate(&blueTooth);
+    
     //taskTimer.attach(&irController, &IRBehaviorController::runSeeking, 0.1);
     taskTimer.attach(&taskFunc, 0.10);
     
@@ -134,29 +138,40 @@
     blueTooth.printf("Start Main Loop.\r\n");        
     while(1)
     {
+        uint16_t rssiL;
+        uint16_t rssiR;
+        
         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());    
         
-        pid.dumpDebug(&blueTooth);
-        irController.dumpDebug(&blueTooth);
+        //pid.dumpDebug(&blueTooth);
+        //irController.dumpDebug(&blueTooth);
         
+        blueTooth.printf("Getting power from IR.\r\n");
         ipower    = irController.getPower();
+        blueTooth.printf("IR:     power     = %3.2f\r\n", ipower);
+        ipower    = 0; // Temp disable.
         
         beacon.scan();
         
+        //NVIC_DisableIRQ(TIMER3_IRQn);   // Disable Ticker IRQ for atomicity
         centeroid = beacon.get_centeroid();
         rssi      = beacon.get_max_rssi();
+        rssiR     = beacon.getR();
+        rssiL     = beacon.getL();
+        //NVIC_EnableIRQ(TIMER3_IRQn);    // Enable Ticker IRQ
         
         bpower    = beaconPID.run(centeroid);
-        bpower = 0;
+        //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);
+        blueTooth.printf("Beacon: rssiL     = %5.5d\r\n", rssiL);
+        blueTooth.printf("Beacon: rssiR     = %5.5d\r\n", rssiR);
+        blueTooth.printf("Beacon: rssiD     = %5.5d\r\n", rssiR - rssiL);
+        blueTooth.printf("Beacon: rssiM     = %5.5d\r\n", rssi);
                 
         prev_rssi = rssi;
 
@@ -165,10 +180,12 @@
         else
             power = bpower;
             
-        if (rssi > 0xFF)
+        if (rssi > 140)
         {
             speed = 0;
         }
+        else
+            speed = GO_SPEED;
                         
         right = speed - power;
         left  = speed + power;