BeaconAvoid code for AHRC competition.
Dependencies: MODSERIAL PiSlingers m3pi mbed
Diff: main.cpp
- Revision:
- 3:aa1a77b080eb
- Parent:
- 2:b789f31e6d94
- Child:
- 4:369caebdf5dc
diff -r b789f31e6d94 -r aa1a77b080eb main.cpp --- a/main.cpp Fri Nov 02 03:00:54 2012 +0000 +++ b/main.cpp Sat Nov 03 01:03:33 2012 +0000 @@ -17,10 +17,11 @@ m3pi base; // PID terms -#define P_TERM 0.08f +#define P_TERM 0.10f #define I_TERM 0.00f #define D_TERM 0.05f -#define THRESHOLD 0.30f +#define THRESHOLD 0.40f + //PID pid(P_TERM, I_TERM, D_TERM, &blueTooth); PID pid(P_TERM, I_TERM, D_TERM); //IRObjDetector ird(&blueTooth); @@ -53,7 +54,7 @@ { uint8_t check[8] = {0, 0, 0, 0, 0, 0, 0, 0 }; uint8_t done = 0; - uint8_t threshold = 0x60; + uint8_t threshold = 90; base.left_motor(0.2); base.right_motor(-0.2); @@ -71,7 +72,7 @@ tmp = beacon.get_max_rssi(); //blueTooth.printf("\r\nBeacon scan %d: rssi = 0x%2.2x\r\n", i, tmp); check[i] = tmp; - //wait(0.1); + wait(0.1); } // Check all readings against threshold, stop turning and exit search loop when all readings are above threshold. @@ -89,6 +90,7 @@ #define BEACON_P_TERM 1.0f #define BEACON_I_TERM 0.0f #define BEACON_D_TERM 0.0f +#define GO_SPEED 0.8f PID beaconPID(BEACON_P_TERM, BEACON_I_TERM, BEACON_D_TERM); @@ -98,14 +100,14 @@ float power = 0.00f; float bpower = 0.00f; float ipower = 0.00f; - float speed = 0.50f; + float speed = GO_SPEED; float max = 1.00f; float right, left; float centeroid; - uint8_t rssi = 0x00; - uint8_t prev_rssi = 0x00; + uint16_t rssi = 0x00; + uint16_t prev_rssi = 0x00; @@ -124,6 +126,8 @@ irController.setActiveThreshold(THRESHOLD); + //beacon.calibrate(&blueTooth); + //taskTimer.attach(&irController, &IRBehaviorController::runSeeking, 0.1); taskTimer.attach(&taskFunc, 0.10); @@ -134,29 +138,40 @@ blueTooth.printf("Start Main Loop.\r\n"); while(1) { + uint16_t rssiL; + uint16_t rssiR; + blueTooth.printf("\e[1J"); // Clear terminal screen blueTooth.printf("\e[H"); // Home terminal screen blueTooth.printf("Main Loop Start.\r\n"); //blueTooth.printf("MAIN: Battery Voltage:\t%1.2f\r\n", base.battery()); - pid.dumpDebug(&blueTooth); - irController.dumpDebug(&blueTooth); + //pid.dumpDebug(&blueTooth); + //irController.dumpDebug(&blueTooth); + blueTooth.printf("Getting power from IR.\r\n"); ipower = irController.getPower(); + blueTooth.printf("IR: power = %3.2f\r\n", ipower); + ipower = 0; // Temp disable. beacon.scan(); + //NVIC_DisableIRQ(TIMER3_IRQn); // Disable Ticker IRQ for atomicity centeroid = beacon.get_centeroid(); rssi = beacon.get_max_rssi(); + rssiR = beacon.getR(); + rssiL = beacon.getL(); + //NVIC_EnableIRQ(TIMER3_IRQn); // Enable Ticker IRQ bpower = beaconPID.run(centeroid); - bpower = 0; + //bpower = 0; blueTooth.printf("Beacon: centeroid = %3.2f\r\n", centeroid); blueTooth.printf("Beacon: power = %3.2f\r\n", bpower); - blueTooth.printf("Beacon: rssiL = 0x%2.2x\r\n", beacon.getL()); - blueTooth.printf("Beacon: rssiR = 0x%2.2x\r\n", beacon.getR()); - blueTooth.printf("Beacon: rssi = 0x%2.2x\r\n", rssi); + blueTooth.printf("Beacon: rssiL = %5.5d\r\n", rssiL); + blueTooth.printf("Beacon: rssiR = %5.5d\r\n", rssiR); + blueTooth.printf("Beacon: rssiD = %5.5d\r\n", rssiR - rssiL); + blueTooth.printf("Beacon: rssiM = %5.5d\r\n", rssi); prev_rssi = rssi; @@ -165,10 +180,12 @@ else power = bpower; - if (rssi > 0xFF) + if (rssi > 140) { speed = 0; } + else + speed = GO_SPEED; right = speed - power; left = speed + power;