BeaconAvoid code for AHRC competition.
Dependencies: MODSERIAL PiSlingers m3pi mbed
IRBehaviorController.cpp
- Committer:
- mpanetta
- Date:
- 2012-11-02
- Revision:
- 2:b789f31e6d94
- Parent:
- 1:ac4eff391f12
- Child:
- 3:aa1a77b080eb
File content as of revision 2:b789f31e6d94:
#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) { float tmp; if (debug != NULL) debug->printf("IRController: Avoidance Task Start.\r\n"); scanIR(); tmp = centeroid; // Centeroid value needs to be split for avoidance mode. if (tmp < 0) tmp += 3; else tmp -=3; if (brightness > activationThreshold) output = pid->run(tmp); 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); } }