Mike Panetta / Mbed 2 deprecated BeaconAvoid

BeaconAvoid code for AHRC competition.

Dependencies:   MODSERIAL PiSlingers m3pi mbed

IRBehaviorController.cpp@4:369caebdf5dc, 2016-03-09 (annotated)

Committer:
mpanetta
Date:
Wed Mar 09 17:40:26 2016 +0000
Revision:
4:369caebdf5dc
Parent:
3:aa1a77b080eb 
Making Public

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 2:b789f31e6d94 37 float tmp;
mpanetta 2:b789f31e6d94 38
mpanetta 1:ac4eff391f12 39 if (debug != NULL)
mpanetta 1:ac4eff391f12 40 debug->printf("IRController: Avoidance Task Start.\r\n");
mpanetta 1:ac4eff391f12 41
mpanetta 1:ac4eff391f12 42 scanIR();
mpanetta 1:ac4eff391f12 43
mpanetta 2:b789f31e6d94 44 tmp = centeroid;
mpanetta 2:b789f31e6d94 45
mpanetta 1:ac4eff391f12 46 // Centeroid value needs to be split for avoidance mode.
mpanetta 2:b789f31e6d94 47 if (tmp < 0)
mpanetta 3:aa1a77b080eb 48 tmp += 3.0f;
mpanetta 1:ac4eff391f12 49 else
mpanetta 3:aa1a77b080eb 50 tmp -= 3.0f;
mpanetta 1:ac4eff391f12 51
mpanetta 1:ac4eff391f12 52 if (brightness > activationThreshold)
mpanetta 2:b789f31e6d94 53 output = pid->run(tmp);
mpanetta 1:ac4eff391f12 54 else
mpanetta 1:ac4eff391f12 55 output = 0;
mpanetta 1:ac4eff391f12 56
mpanetta 1:ac4eff391f12 57 if (debug != NULL)
mpanetta 1:ac4eff391f12 58 debug->printf("IRController: Avoidance Task Complete.\r\n");
mpanetta 1:ac4eff391f12 59 }
mpanetta 1:ac4eff391f12 60
mpanetta 1:ac4eff391f12 61 float
mpanetta 1:ac4eff391f12 62 IRBehaviorController::getPower(void)
mpanetta 1:ac4eff391f12 63 {
mpanetta 1:ac4eff391f12 64 float tmp;
mpanetta 1:ac4eff391f12 65
mpanetta 1:ac4eff391f12 66 NVIC_DisableIRQ(TIMER3_IRQn); // Disable Ticker IRQ for atomicity
mpanetta 1:ac4eff391f12 67 tmp = output;
mpanetta 1:ac4eff391f12 68 NVIC_EnableIRQ(TIMER3_IRQn); // Enable Ticker IRQ
mpanetta 1:ac4eff391f12 69
mpanetta 1:ac4eff391f12 70 return tmp;
mpanetta 1:ac4eff391f12 71 }
mpanetta 1:ac4eff391f12 72
mpanetta 4:369caebdf5dc 73 float
mpanetta 4:369caebdf5dc 74 IRBehaviorController::getBrightness(void)
mpanetta 4:369caebdf5dc 75 {
mpanetta 4:369caebdf5dc 76 float tmp;
mpanetta 4:369caebdf5dc 77
mpanetta 4:369caebdf5dc 78 NVIC_DisableIRQ(TIMER3_IRQn); // Disable Ticker IRQ for atomicity
mpanetta 4:369caebdf5dc 79 tmp = brightness;
mpanetta 4:369caebdf5dc 80 NVIC_EnableIRQ(TIMER3_IRQn); // Enable Ticker IRQ
mpanetta 4:369caebdf5dc 81
mpanetta 4:369caebdf5dc 82 return tmp;
mpanetta 4:369caebdf5dc 83 }
mpanetta 4:369caebdf5dc 84
mpanetta 4:369caebdf5dc 85 float
mpanetta 4:369caebdf5dc 86 IRBehaviorController::getCenteroid(void)
mpanetta 4:369caebdf5dc 87 {
mpanetta 4:369caebdf5dc 88 float tmp;
mpanetta 4:369caebdf5dc 89
mpanetta 4:369caebdf5dc 90 NVIC_DisableIRQ(TIMER3_IRQn); // Disable Ticker IRQ for atomicity
mpanetta 4:369caebdf5dc 91 tmp = centeroid;
mpanetta 4:369caebdf5dc 92 NVIC_EnableIRQ(TIMER3_IRQn); // Enable Ticker IRQ
mpanetta 4:369caebdf5dc 93
mpanetta 4:369caebdf5dc 94 return tmp;
mpanetta 4:369caebdf5dc 95 }
mpanetta 4:369caebdf5dc 96
mpanetta 1:ac4eff391f12 97 void
mpanetta 1:ac4eff391f12 98 IRBehaviorController::dumpDebug(Serial *debug)
mpanetta 1:ac4eff391f12 99 {
mpanetta 1:ac4eff391f12 100 if (debug != NULL)
mpanetta 1:ac4eff391f12 101 {
mpanetta 1:ac4eff391f12 102 debug->printf("IRController: Centeroid = %3.2f\r\n", centeroid);
mpanetta 1:ac4eff391f12 103 debug->printf("IRController: Brightness = %3.2f\r\n", brightness);
mpanetta 1:ac4eff391f12 104 debug->printf("IRController: Power = %3.2f\r\n", output);
mpanetta 1:ac4eff391f12 105 }
mpanetta 1:ac4eff391f12 106 }
mpanetta 1:ac4eff391f12 107
mpanetta 1:ac4eff391f12 108 // Private methods
mpanetta 1:ac4eff391f12 109
mpanetta 1:ac4eff391f12 110 void
mpanetta 1:ac4eff391f12 111 IRBehaviorController::scanIR(void)
mpanetta 1:ac4eff391f12 112 {
mpanetta 1:ac4eff391f12 113 if (debug != NULL)
mpanetta 1:ac4eff391f12 114 debug->printf("IRController: Scanning IR.\r\n");
mpanetta 1:ac4eff391f12 115
mpanetta 1:ac4eff391f12 116 ird.scan();
mpanetta 1:ac4eff391f12 117
mpanetta 1:ac4eff391f12 118 centeroid = ird.get_centeroid();
mpanetta 1:ac4eff391f12 119 brightness = ird.get_weighted_avg_brightness();
mpanetta 1:ac4eff391f12 120
mpanetta 1:ac4eff391f12 121 if (debug != NULL)
mpanetta 1:ac4eff391f12 122 {
mpanetta 1:ac4eff391f12 123 debug->printf("IRController: Scan complete.\r\n");
mpanetta 1:ac4eff391f12 124 debug->printf("IRController: Centeroid = %3.2f\r\n", centeroid);
mpanetta 1:ac4eff391f12 125 debug->printf("IRController: Brightness = %3.2f\r\n", brightness);
mpanetta 1:ac4eff391f12 126 }
mpanetta 0:9ac4a91b71fa 127 }