Mike Panetta / Mbed 2 deprecated BeaconAvoid

BeaconAvoid code for AHRC competition.

Dependencies:   MODSERIAL PiSlingers m3pi mbed

IRBehaviorController.cpp@1:ac4eff391f12, 2012-04-07 (annotated)

Committer:
mpanetta
Date:
Sat Apr 07 02:22:00 2012 +0000
Revision:
1:ac4eff391f12
Parent:
0:9ac4a91b71fa
Child:
2:b789f31e6d94
Added first version of beacon killer code.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mpanetta 1:ac4eff391f12 1 #include "mbed.h"
mpanetta 1:ac4eff391f12 2
mpanetta 1:ac4eff391f12 3 #include "IRBehaviorController.h"
mpanetta 1:ac4eff391f12 4
mpanetta 1:ac4eff391f12 5
mpanetta 1:ac4eff391f12 6 // Public methods
mpanetta 1:ac4eff391f12 7
mpanetta 1:ac4eff391f12 8 void
mpanetta 1:ac4eff391f12 9 IRBehaviorController::setActiveThreshold(float threshold)
mpanetta 1:ac4eff391f12 10 {
mpanetta 1:ac4eff391f12 11 NVIC_DisableIRQ(TIMER3_IRQn); // Disable Ticker IRQ for atomicity
mpanetta 1:ac4eff391f12 12 activationThreshold = threshold;
mpanetta 1:ac4eff391f12 13 NVIC_EnableIRQ(TIMER3_IRQn); // Enable Ticker IRQ
mpanetta 1:ac4eff391f12 14 }
mpanetta 1:ac4eff391f12 15
mpanetta 1:ac4eff391f12 16
mpanetta 1:ac4eff391f12 17 void
mpanetta 1:ac4eff391f12 18 IRBehaviorController::runSeeking(void)
mpanetta 1:ac4eff391f12 19 {
mpanetta 1:ac4eff391f12 20 if (debug != NULL)
mpanetta 1:ac4eff391f12 21 debug->printf("IRController: Seeking Task Start.\r\n");
mpanetta 1:ac4eff391f12 22
mpanetta 1:ac4eff391f12 23 scanIR();
mpanetta 1:ac4eff391f12 24
mpanetta 1:ac4eff391f12 25 if (brightness > activationThreshold)
mpanetta 1:ac4eff391f12 26 output = pid->run(centeroid);
mpanetta 1:ac4eff391f12 27 else
mpanetta 1:ac4eff391f12 28 output = 0;
mpanetta 1:ac4eff391f12 29
mpanetta 1:ac4eff391f12 30 if (debug != NULL)
mpanetta 1:ac4eff391f12 31 debug->printf("IRController: Seeking Task Complete.\r\n");
mpanetta 1:ac4eff391f12 32 }
mpanetta 1:ac4eff391f12 33
mpanetta 1:ac4eff391f12 34 void
mpanetta 1:ac4eff391f12 35 IRBehaviorController::runAvoidance(void)
mpanetta 1:ac4eff391f12 36 {
mpanetta 1:ac4eff391f12 37 if (debug != NULL)
mpanetta 1:ac4eff391f12 38 debug->printf("IRController: Avoidance Task Start.\r\n");
mpanetta 1:ac4eff391f12 39
mpanetta 1:ac4eff391f12 40 scanIR();
mpanetta 1:ac4eff391f12 41
mpanetta 1:ac4eff391f12 42 // Centeroid value needs to be split for avoidance mode.
mpanetta 1:ac4eff391f12 43 if (centeroid < 0)
mpanetta 1:ac4eff391f12 44 centeroid += 3;
mpanetta 1:ac4eff391f12 45 else
mpanetta 1:ac4eff391f12 46 centeroid -=3;
mpanetta 1:ac4eff391f12 47
mpanetta 1:ac4eff391f12 48 if (brightness > activationThreshold)
mpanetta 1:ac4eff391f12 49 output = pid->run(centeroid);
mpanetta 1:ac4eff391f12 50 else
mpanetta 1:ac4eff391f12 51 output = 0;
mpanetta 1:ac4eff391f12 52
mpanetta 1:ac4eff391f12 53 if (debug != NULL)
mpanetta 1:ac4eff391f12 54 debug->printf("IRController: Avoidance Task Complete.\r\n");
mpanetta 1:ac4eff391f12 55 }
mpanetta 1:ac4eff391f12 56
mpanetta 1:ac4eff391f12 57 float
mpanetta 1:ac4eff391f12 58 IRBehaviorController::getPower(void)
mpanetta 1:ac4eff391f12 59 {
mpanetta 1:ac4eff391f12 60 float tmp;
mpanetta 1:ac4eff391f12 61
mpanetta 1:ac4eff391f12 62 NVIC_DisableIRQ(TIMER3_IRQn); // Disable Ticker IRQ for atomicity
mpanetta 1:ac4eff391f12 63 tmp = output;
mpanetta 1:ac4eff391f12 64 NVIC_EnableIRQ(TIMER3_IRQn); // Enable Ticker IRQ
mpanetta 1:ac4eff391f12 65
mpanetta 1:ac4eff391f12 66 return tmp;
mpanetta 1:ac4eff391f12 67 }
mpanetta 1:ac4eff391f12 68
mpanetta 1:ac4eff391f12 69 void
mpanetta 1:ac4eff391f12 70 IRBehaviorController::dumpDebug(Serial *debug)
mpanetta 1:ac4eff391f12 71 {
mpanetta 1:ac4eff391f12 72 if (debug != NULL)
mpanetta 1:ac4eff391f12 73 {
mpanetta 1:ac4eff391f12 74 debug->printf("IRController: Centeroid = %3.2f\r\n", centeroid);
mpanetta 1:ac4eff391f12 75 debug->printf("IRController: Brightness = %3.2f\r\n", brightness);
mpanetta 1:ac4eff391f12 76 debug->printf("IRController: Power = %3.2f\r\n", output);
mpanetta 1:ac4eff391f12 77 }
mpanetta 1:ac4eff391f12 78 }
mpanetta 1:ac4eff391f12 79
mpanetta 1:ac4eff391f12 80 // Private methods
mpanetta 1:ac4eff391f12 81
mpanetta 1:ac4eff391f12 82 void
mpanetta 1:ac4eff391f12 83 IRBehaviorController::scanIR(void)
mpanetta 1:ac4eff391f12 84 {
mpanetta 1:ac4eff391f12 85 if (debug != NULL)
mpanetta 1:ac4eff391f12 86 debug->printf("IRController: Scanning IR.\r\n");
mpanetta 1:ac4eff391f12 87
mpanetta 1:ac4eff391f12 88 ird.scan();
mpanetta 1:ac4eff391f12 89
mpanetta 1:ac4eff391f12 90 centeroid = ird.get_centeroid();
mpanetta 1:ac4eff391f12 91 brightness = ird.get_weighted_avg_brightness();
mpanetta 1:ac4eff391f12 92
mpanetta 1:ac4eff391f12 93 if (debug != NULL)
mpanetta 1:ac4eff391f12 94 {
mpanetta 1:ac4eff391f12 95 debug->printf("IRController: Scan complete.\r\n");
mpanetta 1:ac4eff391f12 96 debug->printf("IRController: Centeroid = %3.2f\r\n", centeroid);
mpanetta 1:ac4eff391f12 97 debug->printf("IRController: Brightness = %3.2f\r\n", brightness);
mpanetta 1:ac4eff391f12 98 }
mpanetta 0:9ac4a91b71fa 99 }