Diff: main.cpp

Revision:

59:972b4eb0df94

Parent:

58:ea73523cf04b

Child:

60:bf851bafc807

--- a/main.cpp	Sat Apr 26 21:01:57 2014 +0000
+++ b/main.cpp	Sat Apr 26 21:12:09 2014 +0000
@@ -113,19 +113,13 @@
 {
     
     // Start Mav test
-    DH::Locs().add(LHType_targ,DataLocation(27.175015,78.042155,5.0));
+    //DH::Locs().add(LHType_targ,DataLocation(27.175015,78.042155,5.0));
     USB::getSerial().printf("Wait %d seconds\n",DELAYBOOT);
     wait(DELAYBOOT);    
     while(true){
         MavCmd::get().run();
+        wait(1);
     }
-    
-    // End mav test
-    
-    //handlers
-    //ImageHandle imageHand;
-    //GPSHandle gpsHand;
-    //CommandHandle commHand;
 
     USB::getSerial().printf("Starting %d\n",sizeof(PacketStruct));
     USB::getSerial().printf("Check GPS\n");
@@ -138,65 +132,27 @@
     int count = 0;
         while(1){
             //USB::getSerial().printf("Running GPS...\r\n");
-            GPSHandle::getGPSHand().run();
+            //GPSHandle::getGPSHand().run();
+            MavCmd::get().run();
             //USB::getSerial().printf("Running Compass...\r\n");
             compassHandle::getCompassHand().run();
-            if(count % 100 == 0){
+            if(count % 10 == 0){
                 USB::getSerial().printf("Requesting commands from egg...\r\n");
                 wait_us(100000);
                 CommandHandle::getCommandHand().run();
                 wait_us(100000);
-                if(GPSHandle::getGPSHand().if_image_location()){
-                    USB::getSerial().printf("Taking picture and sending...\r\n");
-                    wait_us(100000);
-                    ImageHandle::getImageHand().run();
-                    USB::getSerial().printf("sent all data\r\n");
-                    wait_us(100000);
-                    GPSHandle::getGPSHand().next_waypoint();
-                    wait_us(100000);
-                }else{
-                    //USB::getSerial().printf("Not close enough to waypoint for image\r\n");
-                }
+            }
+            if(GPSHandle::getGPSHand().if_image_location()){
+                USB::getSerial().printf("Taking picture and sending...\r\n");
+                wait_us(100000);
+                ImageHandle::getImageHand().run();
+                USB::getSerial().printf("sent all data\r\n");
+                wait_us(100000);
+                GPSHandle::getGPSHand().next_waypoint();
+                wait_us(100000);
+            }else{
+                //USB::getSerial().printf("Not close enough to waypoint for image\r\n");
             }
             count++;
-        //}
-
-        // Run image handler
-        //USB::getSerial().printf("Check Image\n");
-       // imageHand.run();
-        // Run GPS handler
-       // USB::getSerial().printf("Check GPS\n");
-        //gpsHand.run();
-        //USB::getSerial().printf("GPS Time: %f\n",DH::Locs().getC().getTime());
-        // Read packet
-        //USB::getSerial().printf("Read Input\n");
-        //PacketStruct* pack=getPS().lastValid;//getPS().getNextPacket();
-        //if(pack!=NULL){
-        //    USB::getSerial().printf("Received Type: %d\n",pack->type);
-        //    if(pack->type==PT_REQLOC){
-        //if(getPS().outputDevice.readable()>0){
-//                char input=getPS().outputDevice.getc();
-//                //if(getPS().outputDevice.getc()=='A'){
-//                // Send location
-//                unsigned int sID=getPS().getSuperID();
-//                getPS().sendPacket(0,NULL,0,PT_EMPTY);
-//                getPS().sendPacket(sID,NULL,0,PT_SENDLOC);
-//                getPS().sendPacket(sID,(char*)(&DH::Locs().getC().getLoc()),sizeof(DataLocation));
-//                getPS().sendPacket(sID,NULL,0,PT_END);
-//                //}
-//        }
     }
-    /// Main Loop
-   // while(true) {
-//        gps.baud(57600);
-//        xbee.baud(9600);
-//        pc.baud(57600);
-//
-//        sendGpsCommand("PMTK301,1");
-//        while(true) {
-//            pc.putc(gps.getc());
-//        }
-        //gps.attach(&handleGpsData, Serial::RxIrq);
-        //xbee.attach(&handleXbeeGps, Serial::RxIrq)//;
-    //}
 }