Diff: main.cpp

Revision:

0:c867a0a05fe8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Jul 23 11:11:04 2016 +0000
@@ -0,0 +1,220 @@
+#include "mbed.h"
+#include "Cansat2016.h"
+#include "BME280.h"
+#include "VNH5019.h"
+#include "MBed_Adafruit_GPS.h"
+
+// target GPS
+const double target_x = 139.939017, target_y = 37.522390;
+
+//******************************************
+//
+//  pin setting
+//
+//******************************************
+
+// Serial communication
+Serial pc(USBTX, USBRX);
+Serial xbee(p13,p14);
+
+// Compass
+DigitalIn compass[4] = {p12, p11, p8, p7};
+// Pressure
+BME280 pressure(p9, p10);
+// MOtor
+VNH5019 motor(p23,p22,p25,p21,p24,p26);
+
+DigitalIn short_in(p19);
+DigitalOut short_out(p20);
+DigitalInOut nic(p5);
+
+
+//******************************************
+//
+//  Member variable
+//
+//******************************************
+
+// Constructor
+Cansat2016 cansat(motor);
+
+// now mode
+int mode = -1;
+
+int short_flag = 0;
+
+// time interval
+Timer GPS_Timer;
+const int GPS_time = 1000;
+Timer fall_Timer;
+const int fall_time = 30000;
+Timer action_Timer;
+const int action_time = 500;
+Timer stack_Timer;
+const int stack_time = 5000;
+
+
+//*******************************************
+//
+//  standby mode
+//
+//*******************************************
+void standby()
+{
+    cansat.set_action('s');
+
+    if(!short_in) {
+        short_flag ++;
+    }
+
+    if(short_flag >= 5) {
+        mode = 1;
+        fall_Timer.start();
+        xbee.printf("-----------FALLING MODE-----------\n");
+    }
+}
+
+//*******************************************
+//
+//  falling mode
+//
+//*******************************************
+void falling(void)
+{
+    if(fall_Timer.read_ms() >= fall_time) {
+        // nichrome wire is high
+        nic = 1;
+        xbee.printf("remove the parachute\n");
+        wait_ms(3000);
+        nic = 0;
+        mode = 2;
+        action_Timer.start();
+        xbee.printf("forward running\n");
+
+        // running on parachute (10 seconds)
+        while(action_Timer.read_ms() <= 10000) {
+            cansat.set_action('f');
+            cansat.motor_control();
+        }
+        action_Timer.reset();
+        xbee.printf("-----------RUNNING MODE-----------\n");
+    }
+}
+
+//*******************************************
+//
+//  running mode
+//
+//*******************************************
+void running(void)
+{
+
+    cansat.motor_control();
+
+    // decide speed and action of robot
+    if(action_Timer.read_ms() >= action_time) {
+        cansat.calc_distance();
+        cansat.robot_compass(compass);
+        cansat.target_compass();
+        cansat.robot_action();
+        xbee.printf("(%f, %f) distance:%f, target:%d, robot:%d, action:%c\n",
+                    cansat.get_x(), cansat.get_y(), cansat.get_distance(), cansat.get_compass('t'), cansat.get_compass('r'), cansat.get_action());
+        action_Timer.reset();
+    }
+
+    // around the target
+    if(cansat.get_distance() < 0.5) {
+        mode = 100;
+        xbee.printf("-----------STOP MODE-----------\n");
+    }
+
+}
+
+//*******************************************
+//
+//  stop mode
+//
+//*******************************************
+void stop(void)
+{
+
+    cansat.set_action('s');
+
+}
+
+
+//*******************************************
+//
+//  main processing
+//
+//*******************************************
+
+int main()
+{
+
+    wait(3);
+
+    pc.baud(57600);
+    xbee.baud(57600);
+
+    //GPS setting
+    Serial* gps_Serial = new Serial(p28,p27);
+    Adafruit_GPS myGPS(gps_Serial);
+    myGPS.begin(9600);
+
+    myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
+    myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
+    myGPS.sendCommand(PGCMD_ANTENNA);
+
+    GPS_Timer.start();
+
+    // target GPS
+    cansat.set_targetGPS(target_x, target_y);
+    // robot GPS
+    double robot_x=0.0, robot_y=0.0;
+
+    short_out = 1;
+    nic = 0;
+
+    xbee.printf("start\n");
+    xbee.printf("-----------STANDBY MODE-----------\n");
+    wait_ms(1000);
+
+    while(true) {
+
+        myGPS.read();
+        if ( myGPS.newNMEAreceived() ) {
+            if ( !myGPS.parse(myGPS.lastNMEA()) ) {
+                continue;
+            }
+        }
+
+        // get robot GPS
+        if (GPS_Timer.read_ms() >= GPS_time) {
+            if(myGPS.fix) {
+                robot_x = (double)myGPS.longitudeH +(double)(myGPS.longitudeL / 10000.0 / 60.0);
+                robot_y = (double)myGPS.latitudeH + (double)(myGPS.latitudeL / 10000.0 / 60.0);
+
+                cansat.set_robotGPS(robot_x, robot_y);
+                
+            }
+            GPS_Timer.reset();
+        }
+
+
+        switch(mode) {
+            case -1:    // stanby mode
+                standby();
+                break;
+            case 1:     // falling mode
+                falling();
+                break;
+            case 2:     // running mode
+                running();
+                break;
+            case 100:   // stop mode
+                stop();
+                break;
+        }
+    }
+}