File content as of revision 4:40fe9bdb34fc:

#include "mbed.h"
#include "Cansat2016.h"
#include "BME280.h"
#include "VNH5019.h"
#include "MBed_Adafruit_GPS.h"

// target GPS
//const double target_x = 139.988727, target_y = 40.142708;
const double target_x = 139.987270, target_y = 40.142732;
//******************************************
//
//  pin setting
//
//******************************************

// Serial communication
Serial pc(USBTX, USBRX);
Serial xbee(p13,p14);

// Compass
DigitalIn compass[4] = {p7, p8, p11, p12};
//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);
DigitalOut nic(p5);


//******************************************
//
//  Member variable
//
//******************************************

// Constructor
Cansat2016 cansat(motor);

/*LocalFileSystem local("local");
FILE *fp;
fp = fopen("/local/out.csv", "w");*/

// now mode
int mode = -1;

// short pin
int short_flag = 0;
// first forward running
int running_flag = 0;
// stuck
int stuck_flag = 0;
// stop
int stop_count = 0;

// previous distance and ronot compass
double d=0.0;
int c=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 stop_Timer;
const int stop_time = 30000;
Timer stuck_Timer;
const int stuck_time = 5000;
Timer back_Timer;
const int back_time = 5000;


//*******************************************
//
//  print of now state
//
//*******************************************
void print_state(void)
{
    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());

    //fprintf(fp, "%f, %f", cansat.get_x(), cansat.get_y());
}


//*******************************************
//
//  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;
        wait_ms(3000);
        mode = 2;
        action_Timer.start();
        xbee.printf("-----------RUNNING MODE-----------\n");
    }
}


//*******************************************
//
//  running mode
//
//*******************************************
void running(void)
{

    // force quick
    if(xbee.readable()) {
        if(xbee.getc() == 'a') {
            cansat.set_action('s');
            action_Timer.stop();
        }
    }

    // decide speed and action of robot
    if(action_Timer.read_ms() >= action_time) {
        running_flag ++;

        d = cansat.get_distance();
        c = cansat.get_compass('r');

        cansat.calc_distance();
        cansat.robot_compass(compass);
        cansat.target_compass();
        cansat.robot_action();

        // forward running (first 10 seconds)
        if(running_flag <= 20) {
            cansat.set_action('f');
            cansat.set_speed(64);
        }

        // finish a forward running
        if(running_flag == 20) {
            cansat.set_speed(128);
            stop_Timer.start();
            stuck_Timer.start();
            back_Timer.start();
        }

        // if stuck
        if(stuck_flag == 1) {
            if(back_Timer.read_ms() <= 10000) {
                if(back_Timer.read_ms()<= back_time) {
                    cansat.set_action('b');
                } else {
                    cansat.set_action('f');
                }
            } else {
                stuck_flag = 0;
                stuck_Timer.reset();
            }
        }
        print_state();
        action_Timer.reset();
    }

    // around the target
    if(cansat.get_distance() < 0.5) {
        cansat.set_action('s');
        //stop_count ++;
    } else {
        stop_Timer.reset();
    }

    if(stop_Timer.read_ms() >= stop_time) {
        mode = 100;
        xbee.printf("-----------STOP MODE-----------\n");
    }

    if(stuck_flag != 1) {
        // stuck check
        if(!(d-0.15 <= cansat.get_distance() && cansat.get_distance() <= d+0.15)) {
            stuck_Timer.reset();
        }

        // stuck
        if(stuck_Timer.read_ms() >= stuck_time) {
            back_Timer.reset();
            stuck_flag = 1;
        }
    }
}


//*******************************************
//
//  stop mode
//
//*******************************************
void stop(void)
{

    cansat.set_action('s');
    //fclose(fp);

}


//*******************************************
//
//  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);

    wait_ms(1000);

    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.getc();
    xbee.printf("start\n");
    xbee.printf("-----------STANDBY MODE-----------\n");
    cansat.set_action('s');
    wait_ms(1000);

    while(true) {

        cansat.motor_control();

        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);
                //xbee.printf("%f %f\n", 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;
        }
    }
}