Mike Panetta / Mbed 2 deprecated BeaconAvoid

BeaconAvoid code for AHRC competition.

Dependencies:   MODSERIAL PiSlingers m3pi mbed

IRBehaviorController.cpp@0:9ac4a91b71fa, 2012-04-04 (annotated)

Committer:
mpanetta
Date:
Wed Apr 04 17:07:49 2012 +0000
Revision:
0:9ac4a91b71fa
Child:
1:ac4eff391f12
Initial commit.  Avoidance code complete, beacon code still needs to be added.

Who changed what in which revision?

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