Mike Panetta / Mbed 2 deprecated BeaconAvoid

BeaconAvoid code for AHRC competition.

Dependencies:   MODSERIAL PiSlingers m3pi mbed

main.cpp@2:b789f31e6d94, 2012-11-02 (annotated)

Committer:
mpanetta
Date:
Fri Nov 02 03:00:54 2012 +0000
Revision:
2:b789f31e6d94
Parent:
1:ac4eff391f12
Child:
3:aa1a77b080eb
Cleaned up code, fixed PID values for object avoidance routine.

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 2:b789f31e6d94 20 #define P_TERM 0.08f
mpanetta 0:9ac4a91b71fa 21 #define I_TERM 0.00f
mpanetta 2:b789f31e6d94 22 #define D_TERM 0.05f
mpanetta 2:b789f31e6d94 23 #define THRESHOLD 0.30f
mpanetta 0:9ac4a91b71fa 24 //PID pid(P_TERM, I_TERM, D_TERM, &blueTooth);
mpanetta 0:9ac4a91b71fa 25 PID pid(P_TERM, I_TERM, D_TERM);
mpanetta 0:9ac4a91b71fa 26 //IRObjDetector ird(&blueTooth);
mpanetta 0:9ac4a91b71fa 27 //IRBehaviorController irController(&pid, &blueTooth);
mpanetta 0:9ac4a91b71fa 28 IRBehaviorController irController(&pid);
mpanetta 0:9ac4a91b71fa 29 Beacon beacon;
mpanetta 0:9ac4a91b71fa 30
mpanetta 0:9ac4a91b71fa 31
mpanetta 2:b789f31e6d94 32 #define USR_POWERDOWN (0x104)
mpanetta 2:b789f31e6d94 33 int semihost_powerdown()
mpanetta 2:b789f31e6d94 34 {
mpanetta 2:b789f31e6d94 35 uint32_t arg;
mpanetta 2:b789f31e6d94 36 return __semihost(USR_POWERDOWN, &arg);
mpanetta 2:b789f31e6d94 37 }
mpanetta 2:b789f31e6d94 38
mpanetta 2:b789f31e6d94 39
mpanetta 0:9ac4a91b71fa 40 void
mpanetta 0:9ac4a91b71fa 41 taskFunc(void)
mpanetta 0:9ac4a91b71fa 42 {
mpanetta 0:9ac4a91b71fa 43 // blueTooth.printf("\e[1J"); // Clear terminal screen
mpanetta 0:9ac4a91b71fa 44 // blueTooth.printf("\e[H"); // Home terminal screen
mpanetta 0:9ac4a91b71fa 45 // blueTooth.printf("Task exec.\r\n");
mpanetta 0:9ac4a91b71fa 46 //irController.runSeeking();
mpanetta 0:9ac4a91b71fa 47 irController.runAvoidance();
mpanetta 0:9ac4a91b71fa 48 led1 = !led1;
mpanetta 0:9ac4a91b71fa 49 }
mpanetta 0:9ac4a91b71fa 50
mpanetta 1:ac4eff391f12 51 void
mpanetta 1:ac4eff391f12 52 findBeacon(void)
mpanetta 1:ac4eff391f12 53 {
mpanetta 1:ac4eff391f12 54 uint8_t check[8] = {0, 0, 0, 0, 0, 0, 0, 0 };
mpanetta 1:ac4eff391f12 55 uint8_t done = 0;
mpanetta 1:ac4eff391f12 56 uint8_t threshold = 0x60;
mpanetta 1:ac4eff391f12 57
mpanetta 1:ac4eff391f12 58 base.left_motor(0.2);
mpanetta 1:ac4eff391f12 59 base.right_motor(-0.2);
mpanetta 1:ac4eff391f12 60
mpanetta 1:ac4eff391f12 61 // 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 62 while (!done)
mpanetta 1:ac4eff391f12 63 {
mpanetta 1:ac4eff391f12 64 int i;
mpanetta 1:ac4eff391f12 65 // Get 8 readings and put them in an array
mpanetta 1:ac4eff391f12 66 for (i = 0; i < 8; i++)
mpanetta 1:ac4eff391f12 67 {
mpanetta 1:ac4eff391f12 68 uint8_t tmp;
mpanetta 1:ac4eff391f12 69 beacon.scan();
mpanetta 1:ac4eff391f12 70
mpanetta 1:ac4eff391f12 71 tmp = beacon.get_max_rssi();
mpanetta 1:ac4eff391f12 72 //blueTooth.printf("\r\nBeacon scan %d: rssi = 0x%2.2x\r\n", i, tmp);
mpanetta 1:ac4eff391f12 73 check[i] = tmp;
mpanetta 1:ac4eff391f12 74 //wait(0.1);
mpanetta 1:ac4eff391f12 75 }
mpanetta 1:ac4eff391f12 76
mpanetta 1:ac4eff391f12 77 // Check all readings against threshold, stop turning and exit search loop when all readings are above threshold.
mpanetta 1:ac4eff391f12 78 if ((check[0] > threshold) && (check[1] > threshold) && (check[2] > threshold) && (check[3] > threshold) && (check[4] > threshold))
mpanetta 1:ac4eff391f12 79 {
mpanetta 1:ac4eff391f12 80 base.left_motor(0);
mpanetta 1:ac4eff391f12 81 base.right_motor(0);
mpanetta 1:ac4eff391f12 82
mpanetta 1:ac4eff391f12 83 done = 1;
mpanetta 1:ac4eff391f12 84 }
mpanetta 1:ac4eff391f12 85 }
mpanetta 1:ac4eff391f12 86 }
mpanetta 1:ac4eff391f12 87
mpanetta 1:ac4eff391f12 88 // PID terms
mpanetta 1:ac4eff391f12 89 #define BEACON_P_TERM 1.0f
mpanetta 1:ac4eff391f12 90 #define BEACON_I_TERM 0.0f
mpanetta 1:ac4eff391f12 91 #define BEACON_D_TERM 0.0f
mpanetta 1:ac4eff391f12 92
mpanetta 1:ac4eff391f12 93 PID beaconPID(BEACON_P_TERM, BEACON_I_TERM, BEACON_D_TERM);
mpanetta 1:ac4eff391f12 94
mpanetta 0:9ac4a91b71fa 95 int
mpanetta 0:9ac4a91b71fa 96 main()
mpanetta 0:9ac4a91b71fa 97 {
mpanetta 1:ac4eff391f12 98 float power = 0.00f;
mpanetta 1:ac4eff391f12 99 float bpower = 0.00f;
mpanetta 1:ac4eff391f12 100 float ipower = 0.00f;
mpanetta 2:b789f31e6d94 101 float speed = 0.50f;
mpanetta 1:ac4eff391f12 102 float max = 1.00f;
mpanetta 0:9ac4a91b71fa 103 float right, left;
mpanetta 0:9ac4a91b71fa 104
mpanetta 1:ac4eff391f12 105 float centeroid;
mpanetta 1:ac4eff391f12 106
mpanetta 1:ac4eff391f12 107 uint8_t rssi = 0x00;
mpanetta 1:ac4eff391f12 108 uint8_t prev_rssi = 0x00;
mpanetta 1:ac4eff391f12 109
mpanetta 1:ac4eff391f12 110
mpanetta 1:ac4eff391f12 111
mpanetta 0:9ac4a91b71fa 112 //FunctionPointer ird_scan(&ird, &IRObjDetector::scan);
mpanetta 0:9ac4a91b71fa 113
mpanetta 0:9ac4a91b71fa 114
mpanetta 0:9ac4a91b71fa 115 base.reset();
mpanetta 0:9ac4a91b71fa 116 base.locate(0, 0);
mpanetta 0:9ac4a91b71fa 117 base.printf("IR Track");
mpanetta 2:b789f31e6d94 118 wait(5);
mpanetta 2:b789f31e6d94 119
mpanetta 2:b789f31e6d94 120 //semihost_powerdown(); // Disable the secondary controller, this reduces both power and noise.
mpanetta 2:b789f31e6d94 121
mpanetta 0:9ac4a91b71fa 122 blueTooth.baud(115200);
mpanetta 0:9ac4a91b71fa 123 blueTooth.printf("\r\nSystem start!\r\n");
mpanetta 0:9ac4a91b71fa 124
mpanetta 0:9ac4a91b71fa 125 irController.setActiveThreshold(THRESHOLD);
mpanetta 0:9ac4a91b71fa 126
mpanetta 0:9ac4a91b71fa 127 //taskTimer.attach(&irController, &IRBehaviorController::runSeeking, 0.1);
mpanetta 1:ac4eff391f12 128 taskTimer.attach(&taskFunc, 0.10);
mpanetta 1:ac4eff391f12 129
mpanetta 1:ac4eff391f12 130 blueTooth.printf("Find Beacon\r\n");
mpanetta 2:b789f31e6d94 131 //findBeacon();
mpanetta 1:ac4eff391f12 132 blueTooth.printf("Beacon Found!\r\n");
mpanetta 1:ac4eff391f12 133 wait(1);
mpanetta 1:ac4eff391f12 134 blueTooth.printf("Start Main Loop.\r\n");
mpanetta 0:9ac4a91b71fa 135 while(1)
mpanetta 0:9ac4a91b71fa 136 {
mpanetta 0:9ac4a91b71fa 137 blueTooth.printf("\e[1J"); // Clear terminal screen
mpanetta 0:9ac4a91b71fa 138 blueTooth.printf("\e[H"); // Home terminal screen
mpanetta 0:9ac4a91b71fa 139 blueTooth.printf("Main Loop Start.\r\n");
mpanetta 2:b789f31e6d94 140 //blueTooth.printf("MAIN: Battery Voltage:\t%1.2f\r\n", base.battery());
mpanetta 0:9ac4a91b71fa 141
mpanetta 0:9ac4a91b71fa 142 pid.dumpDebug(&blueTooth);
mpanetta 0:9ac4a91b71fa 143 irController.dumpDebug(&blueTooth);
mpanetta 0:9ac4a91b71fa 144
mpanetta 1:ac4eff391f12 145 ipower = irController.getPower();
mpanetta 1:ac4eff391f12 146
mpanetta 1:ac4eff391f12 147 beacon.scan();
mpanetta 1:ac4eff391f12 148
mpanetta 1:ac4eff391f12 149 centeroid = beacon.get_centeroid();
mpanetta 1:ac4eff391f12 150 rssi = beacon.get_max_rssi();
mpanetta 0:9ac4a91b71fa 151
mpanetta 1:ac4eff391f12 152 bpower = beaconPID.run(centeroid);
mpanetta 2:b789f31e6d94 153 bpower = 0;
mpanetta 2:b789f31e6d94 154
mpanetta 1:ac4eff391f12 155 blueTooth.printf("Beacon: centeroid = %3.2f\r\n", centeroid);
mpanetta 1:ac4eff391f12 156 blueTooth.printf("Beacon: power = %3.2f\r\n", bpower);
mpanetta 2:b789f31e6d94 157 blueTooth.printf("Beacon: rssiL = 0x%2.2x\r\n", beacon.getL());
mpanetta 2:b789f31e6d94 158 blueTooth.printf("Beacon: rssiR = 0x%2.2x\r\n", beacon.getR());
mpanetta 1:ac4eff391f12 159 blueTooth.printf("Beacon: rssi = 0x%2.2x\r\n", rssi);
mpanetta 1:ac4eff391f12 160
mpanetta 1:ac4eff391f12 161 prev_rssi = rssi;
mpanetta 1:ac4eff391f12 162
mpanetta 1:ac4eff391f12 163 if (ipower != 0)
mpanetta 1:ac4eff391f12 164 power = ipower;
mpanetta 1:ac4eff391f12 165 else
mpanetta 1:ac4eff391f12 166 power = bpower;
mpanetta 2:b789f31e6d94 167
mpanetta 2:b789f31e6d94 168 if (rssi > 0xFF)
mpanetta 1:ac4eff391f12 169 {
mpanetta 1:ac4eff391f12 170 speed = 0;
mpanetta 1:ac4eff391f12 171 }
mpanetta 1:ac4eff391f12 172
mpanetta 0:9ac4a91b71fa 173 right = speed - power;
mpanetta 0:9ac4a91b71fa 174 left = speed + power;
mpanetta 0:9ac4a91b71fa 175
mpanetta 0:9ac4a91b71fa 176 if (right < -max)
mpanetta 0:9ac4a91b71fa 177 right = -max;
mpanetta 0:9ac4a91b71fa 178 if (right > max)
mpanetta 0:9ac4a91b71fa 179 right = max;
mpanetta 0:9ac4a91b71fa 180
mpanetta 0:9ac4a91b71fa 181 if (left < -max)
mpanetta 0:9ac4a91b71fa 182 left = -max;
mpanetta 0:9ac4a91b71fa 183 if (left > max)
mpanetta 0:9ac4a91b71fa 184 left = max;
mpanetta 0:9ac4a91b71fa 185
mpanetta 0:9ac4a91b71fa 186 blueTooth.printf("MAIN: power = %3.2f\r\n", power);
mpanetta 0:9ac4a91b71fa 187 blueTooth.printf("MAIN: speed = %3.2f\r\n", speed);
mpanetta 0:9ac4a91b71fa 188 blueTooth.printf("MAIN: left = %3.2f\r\n", left);
mpanetta 0:9ac4a91b71fa 189 blueTooth.printf("MAIN: right = %3.2f\r\n", right);
mpanetta 0:9ac4a91b71fa 190
mpanetta 0:9ac4a91b71fa 191 base.left_motor(left);
mpanetta 0:9ac4a91b71fa 192 base.right_motor(right);
mpanetta 0:9ac4a91b71fa 193
mpanetta 0:9ac4a91b71fa 194 blueTooth.printf("Main Loop End.\r\n");
mpanetta 1:ac4eff391f12 195 wait(0.25);
mpanetta 0:9ac4a91b71fa 196 };
mpanetta 0:9ac4a91b71fa 197
mpanetta 0:9ac4a91b71fa 198 // testIRObjDet();
mpanetta 0:9ac4a91b71fa 199
mpanetta 0:9ac4a91b71fa 200 }