IRBehaviorController.cpp

00001 #include "mbed.h"
00002 
00003 #include "IRBehaviorController.h"
00004 
00005 
00006 // Public methods
00007 
00008 void
00009 IRBehaviorController::setActiveThreshold(float threshold)
00010 {
00011     NVIC_DisableIRQ(TIMER3_IRQn);   // Disable Ticker IRQ for atomicity
00012     activationThreshold = threshold;
00013     NVIC_EnableIRQ(TIMER3_IRQn);    // Enable Ticker IRQ
00014 }
00015 
00016 
00017 void
00018 IRBehaviorController::runSeeking(void)
00019 {
00020     if (debug != NULL)
00021         debug->printf("IRController: Seeking Task Start.\r\n");
00022     
00023     scanIR();
00024 
00025     if (brightness > activationThreshold)
00026         output = pid->run(centeroid);
00027     else
00028         output = 0;
00029     
00030     if (debug != NULL)
00031         debug->printf("IRController: Seeking Task Complete.\r\n");
00032 }
00033 
00034 void
00035 IRBehaviorController::runAvoidance(void)
00036 {
00037     float tmp;
00038     
00039     if (debug != NULL)
00040         debug->printf("IRController: Avoidance Task Start.\r\n");
00041     
00042     scanIR();
00043 
00044     tmp = centeroid;
00045     
00046     // Centeroid value needs to be split for avoidance mode.
00047     if (tmp < 0)
00048         tmp += 3.0f;
00049     else
00050         tmp -= 3.0f;
00051         
00052     if (brightness > activationThreshold)
00053         output = pid->run(tmp);    
00054     else
00055         output = 0;
00056     
00057     if (debug != NULL)
00058         debug->printf("IRController: Avoidance Task Complete.\r\n");
00059 }
00060 
00061 float
00062 IRBehaviorController::getPower(void)
00063 {
00064     float tmp;
00065     
00066     NVIC_DisableIRQ(TIMER3_IRQn);   // Disable Ticker IRQ for atomicity
00067     tmp = output;
00068     NVIC_EnableIRQ(TIMER3_IRQn);    // Enable Ticker IRQ
00069     
00070     return tmp;
00071 }
00072 
00073 float
00074 IRBehaviorController::getBrightness(void)
00075 {
00076     float tmp;
00077     
00078     NVIC_DisableIRQ(TIMER3_IRQn);   // Disable Ticker IRQ for atomicity
00079     tmp = brightness;
00080     NVIC_EnableIRQ(TIMER3_IRQn);    // Enable Ticker IRQ
00081     
00082     return tmp;
00083 }
00084 
00085 float
00086 IRBehaviorController::getCenteroid(void)
00087 {
00088     float tmp;
00089     
00090     NVIC_DisableIRQ(TIMER3_IRQn);   // Disable Ticker IRQ for atomicity
00091     tmp = centeroid;
00092     NVIC_EnableIRQ(TIMER3_IRQn);    // Enable Ticker IRQ
00093     
00094     return tmp;
00095 }
00096 
00097 void
00098 IRBehaviorController::dumpDebug(Serial *debug)
00099 {
00100     if (debug != NULL)
00101     {
00102         debug->printf("IRController: Centeroid  = %3.2f\r\n", centeroid);
00103         debug->printf("IRController: Brightness = %3.2f\r\n", brightness);
00104         debug->printf("IRController: Power      = %3.2f\r\n", output);
00105     }   
00106 }
00107 
00108 // Private methods
00109 
00110 void
00111 IRBehaviorController::scanIR(void)
00112 {
00113     if (debug != NULL)
00114         debug->printf("IRController: Scanning IR.\r\n");
00115     
00116     ird.scan();
00117     
00118     centeroid  = ird.get_centeroid();
00119     brightness = ird.get_weighted_avg_brightness();
00120     
00121     if (debug != NULL)
00122     {
00123         debug->printf("IRController: Scan complete.\r\n");
00124         debug->printf("IRController: Centeroid  = %3.2f\r\n", centeroid);
00125         debug->printf("IRController: Brightness = %3.2f\r\n", brightness);
00126     }   
00127 }