main.cpp

00001 #include "mbed.h"
00002 #include "m3pi.h"
00003 #include "MODSERIAL.h"
00004 
00005 #include "tlc5916.h"
00006 #include "IRObjDetector.h"
00007 #include "IRBehaviorController.h"
00008 #include "Beacon.h"
00009 #include "PID.h"
00010 
00011 DigitalOut led1(LED1);
00012 DigitalOut led2(LED2);
00013 
00014 //Serial          blueTooth(p13, p14);
00015 MODSERIAL       blueTooth(p13, p14); 
00016 Ticker          taskTimer;
00017 m3pi            base;
00018 
00019 // PID terms
00020 #define P_TERM    0.10f
00021 #define I_TERM    0.00f
00022 #define D_TERM    0.08f
00023 #define THRESHOLD 0.20f
00024 
00025 //PID                  pid(P_TERM, I_TERM, D_TERM, &blueTooth);
00026 PID                  pid(P_TERM, I_TERM, D_TERM);
00027 //IRObjDetector        ird(&blueTooth);
00028 //IRBehaviorController irController(&pid, &blueTooth);
00029 IRBehaviorController irController(&pid);
00030 Beacon               beacon;
00031 
00032 
00033 #define USR_POWERDOWN    (0x104)
00034 int semihost_powerdown()
00035 {
00036     uint32_t arg;
00037     return __semihost(USR_POWERDOWN, &arg);
00038 }
00039 
00040 
00041 void 
00042 taskFunc(void)
00043 {
00044 //    blueTooth.printf("\e[1J");      // Clear terminal screen
00045 //    blueTooth.printf("\e[H");       // Home terminal screen
00046 //    blueTooth.printf("Task exec.\r\n");
00047     //irController.runSeeking();
00048     irController.runAvoidance();
00049     led1 = !led1;
00050 }
00051 
00052 void
00053 findBeacon(void)
00054 {
00055     uint8_t check[8]  = {0, 0, 0, 0, 0, 0, 0, 0 };
00056     uint8_t done      = 0;
00057     uint8_t threshold = 90;
00058     
00059     base.left_motor(0.2);
00060     base.right_motor(-0.2);
00061 
00062     // Search for the strongest signal, hopefully this will avoid reflections.  I need to add some delays in here to fix things a bit.
00063     while (!done)
00064     {
00065         int i;
00066         // Get 8 readings and put them in an array
00067         for (i = 0; i < 8; i++)
00068         {
00069             uint8_t tmp;
00070             beacon.scan();
00071         
00072             tmp = beacon.get_max_rssi();
00073             //blueTooth.printf("\r\nBeacon scan %d: rssi = 0x%2.2x\r\n", i, tmp);    
00074             check[i] = tmp;
00075             wait(0.1);
00076         }
00077         
00078         // Check all readings against threshold, stop turning and exit search loop when all readings are above threshold.
00079         if ((check[0] > threshold) && (check[1] > threshold) && (check[2] > threshold) && (check[3] > threshold) && (check[4] > threshold))
00080         {
00081             base.left_motor(0);
00082             base.right_motor(0);
00083             
00084             done = 1;
00085         }
00086     }
00087 }
00088 
00089 // PID terms
00090 #define BEACON_P_TERM 1.0f
00091 #define BEACON_I_TERM 0.0f
00092 #define BEACON_D_TERM 0.0f
00093 #define GO_SPEED 0.5f
00094 
00095 PID beaconPID(BEACON_P_TERM, BEACON_I_TERM, BEACON_D_TERM);
00096 
00097 int 
00098 main() 
00099 {
00100     float power  = 0.00f;
00101     float bpower = 0.00f;
00102     float ipower = 0.00f;
00103     float speed  = GO_SPEED;
00104     float max    = 1.00f;
00105     float right, left;
00106    
00107     float centeroid;
00108             
00109     uint16_t rssi      = 0x00;
00110     uint16_t prev_rssi = 0x00;
00111     
00112 
00113     
00114     //FunctionPointer ird_scan(&ird, &IRObjDetector::scan);
00115     
00116     wait(1);    
00117     base.reset();
00118     base.locate(0, 0);
00119     base.printf("IR Track");
00120     wait(4);
00121 
00122     //semihost_powerdown(); // Disable the secondary controller, this reduces both power and noise.
00123         
00124     blueTooth.baud(115200);
00125     blueTooth.printf("\r\nSystem start!\r\n");
00126  
00127     irController.setActiveThreshold(THRESHOLD);
00128     
00129     //beacon.calibrate(&blueTooth);
00130     
00131     //taskTimer.attach(&irController, &IRBehaviorController::runSeeking, 0.1);
00132     taskTimer.attach(&taskFunc, 0.10);
00133     
00134     blueTooth.printf("Find Beacon\r\n");
00135     //findBeacon();    
00136     blueTooth.printf("Beacon Found!\r\n");
00137     wait(1);
00138     blueTooth.printf("Start Main Loop.\r\n");        
00139     while(1)
00140     {
00141         uint16_t rssiL;
00142         uint16_t rssiR;
00143         float    irBrightness;
00144         float    irCenteroid;
00145         
00146         blueTooth.printf("\e[1J");      // Clear terminal screen
00147         blueTooth.printf("\e[H");       // Home terminal screen
00148         blueTooth.printf("Main Loop Start.\r\n");
00149         //blueTooth.printf("MAIN: Battery Voltage:\t%1.2f\r\n", base.battery());    
00150         
00151         //pid.dumpDebug(&blueTooth);
00152         //irController.dumpDebug(&blueTooth);
00153         
00154         blueTooth.printf("Getting power brightness and centeroid from IR.\r\n");
00155         ipower       = irController.getPower();
00156         irBrightness = irController.getBrightness();
00157         irCenteroid  = irController.getCenteroid();
00158         blueTooth.printf("IR:     power     = %3.2f\r\n", ipower);
00159         blueTooth.printf("IR:     brightness= %3.2f\r\n", irBrightness);
00160         blueTooth.printf("IR:     centeroid = %3.2f\r\n", irCenteroid);
00161         //ipower    = 0; // Temp disable.
00162         ipower = ipower * irBrightness; // We want more avoidance for closer objects
00163         //ipower = -ipower;
00164         
00165         beacon.scan();
00166         
00167         //NVIC_DisableIRQ(TIMER3_IRQn);   // Disable Ticker IRQ for atomicity
00168         centeroid = beacon.get_centeroid();
00169         rssi      = beacon.get_max_rssi();
00170         rssiR     = beacon.getR();
00171         rssiL     = beacon.getL();
00172         //NVIC_EnableIRQ(TIMER3_IRQn);    // Enable Ticker IRQ
00173         
00174         bpower    = beaconPID.run(centeroid);
00175         //bpower = 0;
00176         
00177         blueTooth.printf("Beacon: centeroid = %3.2f\r\n", centeroid);
00178         blueTooth.printf("Beacon: power     = %3.2f\r\n", bpower);
00179         blueTooth.printf("Beacon: rssiL     = %5.5d\r\n", rssiL);
00180         blueTooth.printf("Beacon: rssiR     = %5.5d\r\n", rssiR);
00181         blueTooth.printf("Beacon: rssiD     = %5.5d\r\n", rssiR - rssiL);
00182         blueTooth.printf("Beacon: rssiM     = %5.5d\r\n", rssi);
00183                 
00184         prev_rssi = rssi;
00185 
00186         if (ipower != 0)
00187             power = ipower;
00188         else
00189             power = bpower;
00190             
00191         if (rssi > 180)
00192         {
00193             speed = 0;
00194         }
00195         else
00196             speed = GO_SPEED;
00197                         
00198         right = speed - power;
00199         left  = speed + power;
00200         
00201         if (right < -max)
00202             right = -max;
00203         if (right > max)
00204             right = max;
00205             
00206         if (left < -max)
00207             left = -max;
00208         if (left > max)
00209             left = max;
00210         
00211         blueTooth.printf("MAIN: power = %3.2f\r\n", power);
00212         blueTooth.printf("MAIN: speed = %3.2f\r\n", speed);
00213         blueTooth.printf("MAIN: left  = %3.2f\r\n", left);
00214         blueTooth.printf("MAIN: right = %3.2f\r\n", right);
00215         
00216         base.left_motor(left);
00217         base.right_motor(right);
00218         
00219         blueTooth.printf("Main Loop End.\r\n");
00220         wait(0.25);
00221     };
00222     
00223 //    testIRObjDet();
00224             
00225 }