Revision 1:ac4eff391f12, committed 2012-04-07

Comitter:

mpanetta

Date:

Sat Apr 07 02:22:00 2012 +0000

Parent:

0:9ac4a91b71fa

Child:

2:b789f31e6d94

Commit message:

Added first version of beacon killer code.

Changed in this revision

--- a/IRBehaviorController.cpp	Wed Apr 04 17:07:49 2012 +0000
+++ b/IRBehaviorController.cpp	Sat Apr 07 02:22:00 2012 +0000
@@ -1,99 +1,99 @@
-#include "mbed.h"
-
-#include "IRBehaviorController.h"
-
-
-// Public methods
-
-void
-IRBehaviorController::setActiveThreshold(float threshold)
-{
-    NVIC_DisableIRQ(TIMER3_IRQn);   // Disable Ticker IRQ for atomicity
-    activationThreshold = threshold;
-    NVIC_EnableIRQ(TIMER3_IRQn);    // Enable Ticker IRQ
-}
-
-
-void
-IRBehaviorController::runSeeking(void)
-{
-    if (debug != NULL)
-        debug->printf("IRController: Seeking Task Start.\r\n");
-    
-    scanIR();
-
-    if (brightness > activationThreshold)
-        output = pid->run(centeroid);
-    else
-        output = 0;
-    
-    if (debug != NULL)
-        debug->printf("IRController: Seeking Task Complete.\r\n");
-}
-
-void
-IRBehaviorController::runAvoidance(void)
-{
-    if (debug != NULL)
-        debug->printf("IRController: Avoidance Task Start.\r\n");
-    
-    scanIR();
-
-    // Centeroid value needs to be split for avoidance mode.
-    if (centeroid < 0)
-        centeroid += 3;
-    else
-        centeroid -=3;
-        
-    if (brightness > activationThreshold)
-        output = pid->run(centeroid);    
-    else
-        output = 0;
-    
-    if (debug != NULL)
-        debug->printf("IRController: Avoidance Task Complete.\r\n");
-}
-
-float
-IRBehaviorController::getPower(void)
-{
-    float tmp;
-    
-    NVIC_DisableIRQ(TIMER3_IRQn);   // Disable Ticker IRQ for atomicity
-    tmp = output;
-    NVIC_EnableIRQ(TIMER3_IRQn);    // Enable Ticker IRQ
-    
-    return tmp;
-}
-
-void
-IRBehaviorController::dumpDebug(Serial *debug)
-{
-    if (debug != NULL)
-    {
-        debug->printf("IRController: Centeroid  = %3.2f\r\n", centeroid);
-        debug->printf("IRController: Brightness = %3.2f\r\n", brightness);
-        debug->printf("IRController: Power      = %3.2f\r\n", output);
-    }   
-}
-
-// Private methods
-
-void
-IRBehaviorController::scanIR(void)
-{
-    if (debug != NULL)
-        debug->printf("IRController: Scanning IR.\r\n");
-    
-    ird.scan();
-    
-    centeroid  = ird.get_centeroid();
-    brightness = ird.get_weighted_avg_brightness();
-    
-    if (debug != NULL)
-    {
-        debug->printf("IRController: Scan complete.\r\n");
-        debug->printf("IRController: Centeroid  = %3.2f\r\n", centeroid);
-        debug->printf("IRController: Brightness = %3.2f\r\n", brightness);
-    }   
+#include "mbed.h"
+
+#include "IRBehaviorController.h"
+
+
+// Public methods
+
+void
+IRBehaviorController::setActiveThreshold(float threshold)
+{
+    NVIC_DisableIRQ(TIMER3_IRQn);   // Disable Ticker IRQ for atomicity
+    activationThreshold = threshold;
+    NVIC_EnableIRQ(TIMER3_IRQn);    // Enable Ticker IRQ
+}
+
+
+void
+IRBehaviorController::runSeeking(void)
+{
+    if (debug != NULL)
+        debug->printf("IRController: Seeking Task Start.\r\n");
+    
+    scanIR();
+
+    if (brightness > activationThreshold)
+        output = pid->run(centeroid);
+    else
+        output = 0;
+    
+    if (debug != NULL)
+        debug->printf("IRController: Seeking Task Complete.\r\n");
+}
+
+void
+IRBehaviorController::runAvoidance(void)
+{
+    if (debug != NULL)
+        debug->printf("IRController: Avoidance Task Start.\r\n");
+    
+    scanIR();
+
+    // Centeroid value needs to be split for avoidance mode.
+    if (centeroid < 0)
+        centeroid += 3;
+    else
+        centeroid -=3;
+        
+    if (brightness > activationThreshold)
+        output = pid->run(centeroid);    
+    else
+        output = 0;
+    
+    if (debug != NULL)
+        debug->printf("IRController: Avoidance Task Complete.\r\n");
+}
+
+float
+IRBehaviorController::getPower(void)
+{
+    float tmp;
+    
+    NVIC_DisableIRQ(TIMER3_IRQn);   // Disable Ticker IRQ for atomicity
+    tmp = output;
+    NVIC_EnableIRQ(TIMER3_IRQn);    // Enable Ticker IRQ
+    
+    return tmp;
+}
+
+void
+IRBehaviorController::dumpDebug(Serial *debug)
+{
+    if (debug != NULL)
+    {
+        debug->printf("IRController: Centeroid  = %3.2f\r\n", centeroid);
+        debug->printf("IRController: Brightness = %3.2f\r\n", brightness);
+        debug->printf("IRController: Power      = %3.2f\r\n", output);
+    }   
+}
+
+// Private methods
+
+void
+IRBehaviorController::scanIR(void)
+{
+    if (debug != NULL)
+        debug->printf("IRController: Scanning IR.\r\n");
+    
+    ird.scan();
+    
+    centeroid  = ird.get_centeroid();
+    brightness = ird.get_weighted_avg_brightness();
+    
+    if (debug != NULL)
+    {
+        debug->printf("IRController: Scan complete.\r\n");
+        debug->printf("IRController: Centeroid  = %3.2f\r\n", centeroid);
+        debug->printf("IRController: Brightness = %3.2f\r\n", brightness);
+    }   
 }
\ No newline at end of file

--- a/main.cpp	Wed Apr 04 17:07:49 2012 +0000
+++ b/main.cpp	Sat Apr 07 02:22:00 2012 +0000
@@ -17,10 +17,10 @@
 m3pi            base;
 
 // PID terms
-#define P_TERM    0.04f
+#define P_TERM    0.06f
 #define I_TERM    0.00f
 #define D_TERM    0.00f
-#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);
@@ -40,14 +40,67 @@
     led1 = !led1;
 }
 
+void
+findBeacon(void)
+{
+    uint8_t check[8]  = {0, 0, 0, 0, 0, 0, 0, 0 };
+    uint8_t done      = 0;
+    uint8_t threshold = 0x60;
+    
+    base.left_motor(0.2);
+    base.right_motor(-0.2);
+
+    // Search for the strongest signal, hopefully this will avoid reflections.  I need to add some delays in here to fix things a bit.
+    while (!done)
+    {
+        int i;
+        // Get 8 readings and put them in an array
+        for (i = 0; i < 8; i++)
+        {
+            uint8_t tmp;
+            beacon.scan();
+        
+            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);
+        }
+        
+        // Check all readings against threshold, stop turning and exit search loop when all readings are above threshold.
+        if ((check[0] > threshold) && (check[1] > threshold) && (check[2] > threshold) && (check[3] > threshold) && (check[4] > threshold))
+        {
+            base.left_motor(0);
+            base.right_motor(0);
+            
+            done = 1;
+        }
+    }
+}
+
+// PID terms
+#define BEACON_P_TERM 1.0f
+#define BEACON_I_TERM 0.0f
+#define BEACON_D_TERM 0.0f
+
+PID beaconPID(BEACON_P_TERM, BEACON_I_TERM, BEACON_D_TERM);
+
 int 
 main() 
 {
-    float power = 0.00f;
-    float speed = 0.20f;
-    float max   = 1.00f;
+    float power  = 0.00f;
+    float bpower = 0.00f;
+    float ipower = 0.00f;
+    float speed  = 0.20f;
+    float max    = 1.00f;
     float right, left;
    
+    float centeroid;
+            
+    uint8_t rssi      = 0x00;
+    uint8_t prev_rssi = 0x00;
+    
+
+    
     //FunctionPointer ird_scan(&ird, &IRObjDetector::scan);
     
         
@@ -62,8 +115,13 @@
     irController.setActiveThreshold(THRESHOLD);
     
     //taskTimer.attach(&irController, &IRBehaviorController::runSeeking, 0.1);
-    taskTimer.attach(&taskFunc, 0.01);
-        
+    taskTimer.attach(&taskFunc, 0.10);
+    
+    blueTooth.printf("Find Beacon\r\n");
+    findBeacon();    
+    blueTooth.printf("Beacon Found!\r\n");
+    wait(1);
+    blueTooth.printf("Start Main Loop.\r\n");        
     while(1)
     {
         blueTooth.printf("\e[1J");      // Clear terminal screen
@@ -74,9 +132,30 @@
         pid.dumpDebug(&blueTooth);
         irController.dumpDebug(&blueTooth);
         
-        power = irController.getPower();
+        ipower    = irController.getPower();
+        
+        beacon.scan();
+        
+        centeroid = beacon.get_centeroid();
+        rssi      = beacon.get_max_rssi();
         
-        
+        bpower    = beaconPID.run(centeroid);
+    
+        blueTooth.printf("Beacon: centeroid = %3.2f\r\n", centeroid);
+        blueTooth.printf("Beacon: power     = %3.2f\r\n", bpower);
+        blueTooth.printf("Beacon: rssi      = 0x%2.2x\r\n", rssi);
+                
+        prev_rssi = rssi;
+
+        if (ipower != 0)
+            power = ipower;
+        else
+            power = bpower;
+        if (rssi > 0x7d)
+        {
+            speed = 0;
+        }
+                        
         right = speed - power;
         left  = speed + power;
         
@@ -99,7 +178,7 @@
         base.right_motor(right);
         
         blueTooth.printf("Main Loop End.\r\n");
-        wait(0.1);
+        wait(0.25);
     };
     
 //    testIRObjDet();