Mike Panetta / Mbed 2 deprecated BeaconAvoid

BeaconAvoid code for AHRC competition.

Dependencies:   MODSERIAL PiSlingers m3pi mbed

main.cpp@0:9ac4a91b71fa, 2012-04-04 (annotated)

Committer:
mpanetta
Date:
Wed Apr 04 17:07:49 2012 +0000
Revision:
0:9ac4a91b71fa
Child:
1:ac4eff391f12
Initial commit.  Avoidance code complete, beacon code still needs to be added.

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 0:9ac4a91b71fa 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 0:9ac4a91b71fa 20 #define P_TERM 0.04f
mpanetta 0:9ac4a91b71fa 21 #define I_TERM 0.00f
mpanetta 0:9ac4a91b71fa 22 #define D_TERM 0.00f
mpanetta 0:9ac4a91b71fa 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 0:9ac4a91b71fa 32 void
mpanetta 0:9ac4a91b71fa 33 taskFunc(void)
mpanetta 0:9ac4a91b71fa 34 {
mpanetta 0:9ac4a91b71fa 35 // blueTooth.printf("\e[1J"); // Clear terminal screen
mpanetta 0:9ac4a91b71fa 36 // blueTooth.printf("\e[H"); // Home terminal screen
mpanetta 0:9ac4a91b71fa 37 // blueTooth.printf("Task exec.\r\n");
mpanetta 0:9ac4a91b71fa 38 //irController.runSeeking();
mpanetta 0:9ac4a91b71fa 39 irController.runAvoidance();
mpanetta 0:9ac4a91b71fa 40 led1 = !led1;
mpanetta 0:9ac4a91b71fa 41 }
mpanetta 0:9ac4a91b71fa 42
mpanetta 0:9ac4a91b71fa 43 int
mpanetta 0:9ac4a91b71fa 44 main()
mpanetta 0:9ac4a91b71fa 45 {
mpanetta 0:9ac4a91b71fa 46 float power = 0.00f;
mpanetta 0:9ac4a91b71fa 47 float speed = 0.20f;
mpanetta 0:9ac4a91b71fa 48 float max = 1.00f;
mpanetta 0:9ac4a91b71fa 49 float right, left;
mpanetta 0:9ac4a91b71fa 50
mpanetta 0:9ac4a91b71fa 51 //FunctionPointer ird_scan(&ird, &IRObjDetector::scan);
mpanetta 0:9ac4a91b71fa 52
mpanetta 0:9ac4a91b71fa 53
mpanetta 0:9ac4a91b71fa 54 base.reset();
mpanetta 0:9ac4a91b71fa 55 base.locate(0, 0);
mpanetta 0:9ac4a91b71fa 56 base.printf("IR Track");
mpanetta 0:9ac4a91b71fa 57 wait(10);
mpanetta 0:9ac4a91b71fa 58
mpanetta 0:9ac4a91b71fa 59 blueTooth.baud(115200);
mpanetta 0:9ac4a91b71fa 60 blueTooth.printf("\r\nSystem start!\r\n");
mpanetta 0:9ac4a91b71fa 61
mpanetta 0:9ac4a91b71fa 62 irController.setActiveThreshold(THRESHOLD);
mpanetta 0:9ac4a91b71fa 63
mpanetta 0:9ac4a91b71fa 64 //taskTimer.attach(&irController, &IRBehaviorController::runSeeking, 0.1);
mpanetta 0:9ac4a91b71fa 65 taskTimer.attach(&taskFunc, 0.01);
mpanetta 0:9ac4a91b71fa 66
mpanetta 0:9ac4a91b71fa 67 while(1)
mpanetta 0:9ac4a91b71fa 68 {
mpanetta 0:9ac4a91b71fa 69 blueTooth.printf("\e[1J"); // Clear terminal screen
mpanetta 0:9ac4a91b71fa 70 blueTooth.printf("\e[H"); // Home terminal screen
mpanetta 0:9ac4a91b71fa 71 blueTooth.printf("Main Loop Start.\r\n");
mpanetta 0:9ac4a91b71fa 72 blueTooth.printf("MAIN: Battery Voltage:\t%1.2f\r\n", base.battery());
mpanetta 0:9ac4a91b71fa 73
mpanetta 0:9ac4a91b71fa 74 pid.dumpDebug(&blueTooth);
mpanetta 0:9ac4a91b71fa 75 irController.dumpDebug(&blueTooth);
mpanetta 0:9ac4a91b71fa 76
mpanetta 0:9ac4a91b71fa 77 power = irController.getPower();
mpanetta 0:9ac4a91b71fa 78
mpanetta 0:9ac4a91b71fa 79
mpanetta 0:9ac4a91b71fa 80 right = speed - power;
mpanetta 0:9ac4a91b71fa 81 left = speed + power;
mpanetta 0:9ac4a91b71fa 82
mpanetta 0:9ac4a91b71fa 83 if (right < -max)
mpanetta 0:9ac4a91b71fa 84 right = -max;
mpanetta 0:9ac4a91b71fa 85 if (right > max)
mpanetta 0:9ac4a91b71fa 86 right = max;
mpanetta 0:9ac4a91b71fa 87
mpanetta 0:9ac4a91b71fa 88 if (left < -max)
mpanetta 0:9ac4a91b71fa 89 left = -max;
mpanetta 0:9ac4a91b71fa 90 if (left > max)
mpanetta 0:9ac4a91b71fa 91 left = max;
mpanetta 0:9ac4a91b71fa 92
mpanetta 0:9ac4a91b71fa 93 blueTooth.printf("MAIN: power = %3.2f\r\n", power);
mpanetta 0:9ac4a91b71fa 94 blueTooth.printf("MAIN: speed = %3.2f\r\n", speed);
mpanetta 0:9ac4a91b71fa 95 blueTooth.printf("MAIN: left = %3.2f\r\n", left);
mpanetta 0:9ac4a91b71fa 96 blueTooth.printf("MAIN: right = %3.2f\r\n", right);
mpanetta 0:9ac4a91b71fa 97
mpanetta 0:9ac4a91b71fa 98 base.left_motor(left);
mpanetta 0:9ac4a91b71fa 99 base.right_motor(right);
mpanetta 0:9ac4a91b71fa 100
mpanetta 0:9ac4a91b71fa 101 blueTooth.printf("Main Loop End.\r\n");
mpanetta 0:9ac4a91b71fa 102 wait(0.1);
mpanetta 0:9ac4a91b71fa 103 };
mpanetta 0:9ac4a91b71fa 104
mpanetta 0:9ac4a91b71fa 105 // testIRObjDet();
mpanetta 0:9ac4a91b71fa 106
mpanetta 0:9ac4a91b71fa 107 }