Mike Panetta / Mbed 2 deprecated BeaconAvoid

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?

UserRevisionLine numberNew 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 }