cansat program (version 1)
Dependencies: ADXL345_I2C BME280 Cansat2016 MBed_Adafruit-GPS-Library VNH5019 mbed
main.cpp
- Committer:
- s1210160
- Date:
- 2016-07-23
- Revision:
- 0:c867a0a05fe8
File content as of revision 0:c867a0a05fe8:
#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; } } }