BeaconAvoid code for AHRC competition.
Dependencies: MODSERIAL PiSlingers m3pi mbed
main.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?
User | Revision | Line number | New contents of line |
---|---|---|---|
mpanetta | 0:9ac4a91b71fa | 1 | #include "mbed.h" |
mpanetta | 0:9ac4a91b71fa | 2 | #include "m3pi.h" |
mpanetta | 0:9ac4a91b71fa | 3 | #include "MODSERIAL.h" |
mpanetta | 0:9ac4a91b71fa | 4 | |
mpanetta | 0:9ac4a91b71fa | 5 | #include "tlc5916.h" |
mpanetta | 0:9ac4a91b71fa | 6 | #include "IRObjDetector.h" |
mpanetta | 0:9ac4a91b71fa | 7 | #include "IRBehaviorController.h" |
mpanetta | 0:9ac4a91b71fa | 8 | #include "Beacon.h" |
mpanetta | 0:9ac4a91b71fa | 9 | #include "PID.h" |
mpanetta | 0:9ac4a91b71fa | 10 | |
mpanetta | 0:9ac4a91b71fa | 11 | DigitalOut led1(LED1); |
mpanetta | 0:9ac4a91b71fa | 12 | DigitalOut led2(LED2); |
mpanetta | 0:9ac4a91b71fa | 13 | |
mpanetta | 0:9ac4a91b71fa | 14 | //Serial blueTooth(p13, p14); |
mpanetta | 2:b789f31e6d94 | 15 | MODSERIAL blueTooth(p13, p14); |
mpanetta | 0:9ac4a91b71fa | 16 | Ticker taskTimer; |
mpanetta | 0:9ac4a91b71fa | 17 | m3pi base; |
mpanetta | 0:9ac4a91b71fa | 18 | |
mpanetta | 0:9ac4a91b71fa | 19 | // PID terms |
mpanetta | 3:aa1a77b080eb | 20 | #define P_TERM 0.10f |
mpanetta | 0:9ac4a91b71fa | 21 | #define I_TERM 0.00f |
mpanetta | 4:369caebdf5dc | 22 | #define D_TERM 0.08f |
mpanetta | 4:369caebdf5dc | 23 | #define THRESHOLD 0.20f |
mpanetta | 3:aa1a77b080eb | 24 | |
mpanetta | 0:9ac4a91b71fa | 25 | //PID pid(P_TERM, I_TERM, D_TERM, &blueTooth); |
mpanetta | 0:9ac4a91b71fa | 26 | PID pid(P_TERM, I_TERM, D_TERM); |
mpanetta | 0:9ac4a91b71fa | 27 | //IRObjDetector ird(&blueTooth); |
mpanetta | 0:9ac4a91b71fa | 28 | //IRBehaviorController irController(&pid, &blueTooth); |
mpanetta | 0:9ac4a91b71fa | 29 | IRBehaviorController irController(&pid); |
mpanetta | 0:9ac4a91b71fa | 30 | Beacon beacon; |
mpanetta | 0:9ac4a91b71fa | 31 | |
mpanetta | 0:9ac4a91b71fa | 32 | |
mpanetta | 2:b789f31e6d94 | 33 | #define USR_POWERDOWN (0x104) |
mpanetta | 2:b789f31e6d94 | 34 | int semihost_powerdown() |
mpanetta | 2:b789f31e6d94 | 35 | { |
mpanetta | 2:b789f31e6d94 | 36 | uint32_t arg; |
mpanetta | 2:b789f31e6d94 | 37 | return __semihost(USR_POWERDOWN, &arg); |
mpanetta | 2:b789f31e6d94 | 38 | } |
mpanetta | 2:b789f31e6d94 | 39 | |
mpanetta | 2:b789f31e6d94 | 40 | |
mpanetta | 0:9ac4a91b71fa | 41 | void |
mpanetta | 0:9ac4a91b71fa | 42 | taskFunc(void) |
mpanetta | 0:9ac4a91b71fa | 43 | { |
mpanetta | 0:9ac4a91b71fa | 44 | // blueTooth.printf("\e[1J"); // Clear terminal screen |
mpanetta | 0:9ac4a91b71fa | 45 | // blueTooth.printf("\e[H"); // Home terminal screen |
mpanetta | 0:9ac4a91b71fa | 46 | // blueTooth.printf("Task exec.\r\n"); |
mpanetta | 0:9ac4a91b71fa | 47 | //irController.runSeeking(); |
mpanetta | 0:9ac4a91b71fa | 48 | irController.runAvoidance(); |
mpanetta | 0:9ac4a91b71fa | 49 | led1 = !led1; |
mpanetta | 0:9ac4a91b71fa | 50 | } |
mpanetta | 0:9ac4a91b71fa | 51 | |
mpanetta | 1:ac4eff391f12 | 52 | void |
mpanetta | 1:ac4eff391f12 | 53 | findBeacon(void) |
mpanetta | 1:ac4eff391f12 | 54 | { |
mpanetta | 1:ac4eff391f12 | 55 | uint8_t check[8] = {0, 0, 0, 0, 0, 0, 0, 0 }; |
mpanetta | 1:ac4eff391f12 | 56 | uint8_t done = 0; |
mpanetta | 3:aa1a77b080eb | 57 | uint8_t threshold = 90; |
mpanetta | 1:ac4eff391f12 | 58 | |
mpanetta | 1:ac4eff391f12 | 59 | base.left_motor(0.2); |
mpanetta | 1:ac4eff391f12 | 60 | base.right_motor(-0.2); |
mpanetta | 1:ac4eff391f12 | 61 | |
mpanetta | 1:ac4eff391f12 | 62 | // Search for the strongest signal, hopefully this will avoid reflections. I need to add some delays in here to fix things a bit. |
mpanetta | 1:ac4eff391f12 | 63 | while (!done) |
mpanetta | 1:ac4eff391f12 | 64 | { |
mpanetta | 1:ac4eff391f12 | 65 | int i; |
mpanetta | 1:ac4eff391f12 | 66 | // Get 8 readings and put them in an array |
mpanetta | 1:ac4eff391f12 | 67 | for (i = 0; i < 8; i++) |
mpanetta | 1:ac4eff391f12 | 68 | { |
mpanetta | 1:ac4eff391f12 | 69 | uint8_t tmp; |
mpanetta | 1:ac4eff391f12 | 70 | beacon.scan(); |
mpanetta | 1:ac4eff391f12 | 71 | |
mpanetta | 1:ac4eff391f12 | 72 | tmp = beacon.get_max_rssi(); |
mpanetta | 1:ac4eff391f12 | 73 | //blueTooth.printf("\r\nBeacon scan %d: rssi = 0x%2.2x\r\n", i, tmp); |
mpanetta | 1:ac4eff391f12 | 74 | check[i] = tmp; |
mpanetta | 3:aa1a77b080eb | 75 | wait(0.1); |
mpanetta | 1:ac4eff391f12 | 76 | } |
mpanetta | 1:ac4eff391f12 | 77 | |
mpanetta | 1:ac4eff391f12 | 78 | // Check all readings against threshold, stop turning and exit search loop when all readings are above threshold. |
mpanetta | 1:ac4eff391f12 | 79 | if ((check[0] > threshold) && (check[1] > threshold) && (check[2] > threshold) && (check[3] > threshold) && (check[4] > threshold)) |
mpanetta | 1:ac4eff391f12 | 80 | { |
mpanetta | 1:ac4eff391f12 | 81 | base.left_motor(0); |
mpanetta | 1:ac4eff391f12 | 82 | base.right_motor(0); |
mpanetta | 1:ac4eff391f12 | 83 | |
mpanetta | 1:ac4eff391f12 | 84 | done = 1; |
mpanetta | 1:ac4eff391f12 | 85 | } |
mpanetta | 1:ac4eff391f12 | 86 | } |
mpanetta | 1:ac4eff391f12 | 87 | } |
mpanetta | 1:ac4eff391f12 | 88 | |
mpanetta | 1:ac4eff391f12 | 89 | // PID terms |
mpanetta | 1:ac4eff391f12 | 90 | #define BEACON_P_TERM 1.0f |
mpanetta | 1:ac4eff391f12 | 91 | #define BEACON_I_TERM 0.0f |
mpanetta | 1:ac4eff391f12 | 92 | #define BEACON_D_TERM 0.0f |
mpanetta | 4:369caebdf5dc | 93 | #define GO_SPEED 0.5f |
mpanetta | 1:ac4eff391f12 | 94 | |
mpanetta | 1:ac4eff391f12 | 95 | PID beaconPID(BEACON_P_TERM, BEACON_I_TERM, BEACON_D_TERM); |
mpanetta | 1:ac4eff391f12 | 96 | |
mpanetta | 0:9ac4a91b71fa | 97 | int |
mpanetta | 0:9ac4a91b71fa | 98 | main() |
mpanetta | 0:9ac4a91b71fa | 99 | { |
mpanetta | 1:ac4eff391f12 | 100 | float power = 0.00f; |
mpanetta | 1:ac4eff391f12 | 101 | float bpower = 0.00f; |
mpanetta | 1:ac4eff391f12 | 102 | float ipower = 0.00f; |
mpanetta | 3:aa1a77b080eb | 103 | float speed = GO_SPEED; |
mpanetta | 1:ac4eff391f12 | 104 | float max = 1.00f; |
mpanetta | 0:9ac4a91b71fa | 105 | float right, left; |
mpanetta | 0:9ac4a91b71fa | 106 | |
mpanetta | 1:ac4eff391f12 | 107 | float centeroid; |
mpanetta | 1:ac4eff391f12 | 108 | |
mpanetta | 3:aa1a77b080eb | 109 | uint16_t rssi = 0x00; |
mpanetta | 3:aa1a77b080eb | 110 | uint16_t prev_rssi = 0x00; |
mpanetta | 1:ac4eff391f12 | 111 | |
mpanetta | 1:ac4eff391f12 | 112 | |
mpanetta | 1:ac4eff391f12 | 113 | |
mpanetta | 0:9ac4a91b71fa | 114 | //FunctionPointer ird_scan(&ird, &IRObjDetector::scan); |
mpanetta | 0:9ac4a91b71fa | 115 | |
mpanetta | 4:369caebdf5dc | 116 | wait(1); |
mpanetta | 0:9ac4a91b71fa | 117 | base.reset(); |
mpanetta | 0:9ac4a91b71fa | 118 | base.locate(0, 0); |
mpanetta | 0:9ac4a91b71fa | 119 | base.printf("IR Track"); |
mpanetta | 4:369caebdf5dc | 120 | wait(4); |
mpanetta | 2:b789f31e6d94 | 121 | |
mpanetta | 2:b789f31e6d94 | 122 | //semihost_powerdown(); // Disable the secondary controller, this reduces both power and noise. |
mpanetta | 2:b789f31e6d94 | 123 | |
mpanetta | 0:9ac4a91b71fa | 124 | blueTooth.baud(115200); |
mpanetta | 0:9ac4a91b71fa | 125 | blueTooth.printf("\r\nSystem start!\r\n"); |
mpanetta | 0:9ac4a91b71fa | 126 | |
mpanetta | 0:9ac4a91b71fa | 127 | irController.setActiveThreshold(THRESHOLD); |
mpanetta | 0:9ac4a91b71fa | 128 | |
mpanetta | 3:aa1a77b080eb | 129 | //beacon.calibrate(&blueTooth); |
mpanetta | 3:aa1a77b080eb | 130 | |
mpanetta | 0:9ac4a91b71fa | 131 | //taskTimer.attach(&irController, &IRBehaviorController::runSeeking, 0.1); |
mpanetta | 1:ac4eff391f12 | 132 | taskTimer.attach(&taskFunc, 0.10); |
mpanetta | 1:ac4eff391f12 | 133 | |
mpanetta | 1:ac4eff391f12 | 134 | blueTooth.printf("Find Beacon\r\n"); |
mpanetta | 2:b789f31e6d94 | 135 | //findBeacon(); |
mpanetta | 1:ac4eff391f12 | 136 | blueTooth.printf("Beacon Found!\r\n"); |
mpanetta | 1:ac4eff391f12 | 137 | wait(1); |
mpanetta | 1:ac4eff391f12 | 138 | blueTooth.printf("Start Main Loop.\r\n"); |
mpanetta | 0:9ac4a91b71fa | 139 | while(1) |
mpanetta | 0:9ac4a91b71fa | 140 | { |
mpanetta | 3:aa1a77b080eb | 141 | uint16_t rssiL; |
mpanetta | 3:aa1a77b080eb | 142 | uint16_t rssiR; |
mpanetta | 4:369caebdf5dc | 143 | float irBrightness; |
mpanetta | 4:369caebdf5dc | 144 | float irCenteroid; |
mpanetta | 3:aa1a77b080eb | 145 | |
mpanetta | 0:9ac4a91b71fa | 146 | blueTooth.printf("\e[1J"); // Clear terminal screen |
mpanetta | 0:9ac4a91b71fa | 147 | blueTooth.printf("\e[H"); // Home terminal screen |
mpanetta | 0:9ac4a91b71fa | 148 | blueTooth.printf("Main Loop Start.\r\n"); |
mpanetta | 2:b789f31e6d94 | 149 | //blueTooth.printf("MAIN: Battery Voltage:\t%1.2f\r\n", base.battery()); |
mpanetta | 0:9ac4a91b71fa | 150 | |
mpanetta | 3:aa1a77b080eb | 151 | //pid.dumpDebug(&blueTooth); |
mpanetta | 3:aa1a77b080eb | 152 | //irController.dumpDebug(&blueTooth); |
mpanetta | 0:9ac4a91b71fa | 153 | |
mpanetta | 4:369caebdf5dc | 154 | blueTooth.printf("Getting power brightness and centeroid from IR.\r\n"); |
mpanetta | 4:369caebdf5dc | 155 | ipower = irController.getPower(); |
mpanetta | 4:369caebdf5dc | 156 | irBrightness = irController.getBrightness(); |
mpanetta | 4:369caebdf5dc | 157 | irCenteroid = irController.getCenteroid(); |
mpanetta | 3:aa1a77b080eb | 158 | blueTooth.printf("IR: power = %3.2f\r\n", ipower); |
mpanetta | 4:369caebdf5dc | 159 | blueTooth.printf("IR: brightness= %3.2f\r\n", irBrightness); |
mpanetta | 4:369caebdf5dc | 160 | blueTooth.printf("IR: centeroid = %3.2f\r\n", irCenteroid); |
mpanetta | 4:369caebdf5dc | 161 | //ipower = 0; // Temp disable. |
mpanetta | 4:369caebdf5dc | 162 | ipower = ipower * irBrightness; // We want more avoidance for closer objects |
mpanetta | 4:369caebdf5dc | 163 | //ipower = -ipower; |
mpanetta | 1:ac4eff391f12 | 164 | |
mpanetta | 1:ac4eff391f12 | 165 | beacon.scan(); |
mpanetta | 1:ac4eff391f12 | 166 | |
mpanetta | 3:aa1a77b080eb | 167 | //NVIC_DisableIRQ(TIMER3_IRQn); // Disable Ticker IRQ for atomicity |
mpanetta | 1:ac4eff391f12 | 168 | centeroid = beacon.get_centeroid(); |
mpanetta | 1:ac4eff391f12 | 169 | rssi = beacon.get_max_rssi(); |
mpanetta | 3:aa1a77b080eb | 170 | rssiR = beacon.getR(); |
mpanetta | 3:aa1a77b080eb | 171 | rssiL = beacon.getL(); |
mpanetta | 3:aa1a77b080eb | 172 | //NVIC_EnableIRQ(TIMER3_IRQn); // Enable Ticker IRQ |
mpanetta | 0:9ac4a91b71fa | 173 | |
mpanetta | 1:ac4eff391f12 | 174 | bpower = beaconPID.run(centeroid); |
mpanetta | 3:aa1a77b080eb | 175 | //bpower = 0; |
mpanetta | 2:b789f31e6d94 | 176 | |
mpanetta | 1:ac4eff391f12 | 177 | blueTooth.printf("Beacon: centeroid = %3.2f\r\n", centeroid); |
mpanetta | 1:ac4eff391f12 | 178 | blueTooth.printf("Beacon: power = %3.2f\r\n", bpower); |
mpanetta | 3:aa1a77b080eb | 179 | blueTooth.printf("Beacon: rssiL = %5.5d\r\n", rssiL); |
mpanetta | 3:aa1a77b080eb | 180 | blueTooth.printf("Beacon: rssiR = %5.5d\r\n", rssiR); |
mpanetta | 3:aa1a77b080eb | 181 | blueTooth.printf("Beacon: rssiD = %5.5d\r\n", rssiR - rssiL); |
mpanetta | 3:aa1a77b080eb | 182 | blueTooth.printf("Beacon: rssiM = %5.5d\r\n", rssi); |
mpanetta | 1:ac4eff391f12 | 183 | |
mpanetta | 1:ac4eff391f12 | 184 | prev_rssi = rssi; |
mpanetta | 1:ac4eff391f12 | 185 | |
mpanetta | 1:ac4eff391f12 | 186 | if (ipower != 0) |
mpanetta | 1:ac4eff391f12 | 187 | power = ipower; |
mpanetta | 1:ac4eff391f12 | 188 | else |
mpanetta | 1:ac4eff391f12 | 189 | power = bpower; |
mpanetta | 2:b789f31e6d94 | 190 | |
mpanetta | 4:369caebdf5dc | 191 | if (rssi > 180) |
mpanetta | 1:ac4eff391f12 | 192 | { |
mpanetta | 1:ac4eff391f12 | 193 | speed = 0; |
mpanetta | 1:ac4eff391f12 | 194 | } |
mpanetta | 3:aa1a77b080eb | 195 | else |
mpanetta | 3:aa1a77b080eb | 196 | speed = GO_SPEED; |
mpanetta | 1:ac4eff391f12 | 197 | |
mpanetta | 0:9ac4a91b71fa | 198 | right = speed - power; |
mpanetta | 0:9ac4a91b71fa | 199 | left = speed + power; |
mpanetta | 0:9ac4a91b71fa | 200 | |
mpanetta | 0:9ac4a91b71fa | 201 | if (right < -max) |
mpanetta | 0:9ac4a91b71fa | 202 | right = -max; |
mpanetta | 0:9ac4a91b71fa | 203 | if (right > max) |
mpanetta | 0:9ac4a91b71fa | 204 | right = max; |
mpanetta | 0:9ac4a91b71fa | 205 | |
mpanetta | 0:9ac4a91b71fa | 206 | if (left < -max) |
mpanetta | 0:9ac4a91b71fa | 207 | left = -max; |
mpanetta | 0:9ac4a91b71fa | 208 | if (left > max) |
mpanetta | 0:9ac4a91b71fa | 209 | left = max; |
mpanetta | 0:9ac4a91b71fa | 210 | |
mpanetta | 0:9ac4a91b71fa | 211 | blueTooth.printf("MAIN: power = %3.2f\r\n", power); |
mpanetta | 0:9ac4a91b71fa | 212 | blueTooth.printf("MAIN: speed = %3.2f\r\n", speed); |
mpanetta | 0:9ac4a91b71fa | 213 | blueTooth.printf("MAIN: left = %3.2f\r\n", left); |
mpanetta | 0:9ac4a91b71fa | 214 | blueTooth.printf("MAIN: right = %3.2f\r\n", right); |
mpanetta | 0:9ac4a91b71fa | 215 | |
mpanetta | 0:9ac4a91b71fa | 216 | base.left_motor(left); |
mpanetta | 0:9ac4a91b71fa | 217 | base.right_motor(right); |
mpanetta | 0:9ac4a91b71fa | 218 | |
mpanetta | 0:9ac4a91b71fa | 219 | blueTooth.printf("Main Loop End.\r\n"); |
mpanetta | 1:ac4eff391f12 | 220 | wait(0.25); |
mpanetta | 0:9ac4a91b71fa | 221 | }; |
mpanetta | 0:9ac4a91b71fa | 222 | |
mpanetta | 0:9ac4a91b71fa | 223 | // testIRObjDet(); |
mpanetta | 0:9ac4a91b71fa | 224 | |
mpanetta | 0:9ac4a91b71fa | 225 | } |