Haruna Nakazawa / Mbed 2 deprecated Cansat2016_v1

cansat program (version 1)

Dependencies:   ADXL345_I2C BME280 Cansat2016 MBed_Adafruit-GPS-Library VNH5019 mbed

main.cpp@0:c867a0a05fe8, 2016-07-23 (annotated)

Committer:
s1210160
Date:
Sat Jul 23 11:11:04 2016 +0000
Revision:
0:c867a0a05fe8
pressure

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s1210160 0:c867a0a05fe8 1 #include "mbed.h"
s1210160 0:c867a0a05fe8 2 #include "Cansat2016.h"
s1210160 0:c867a0a05fe8 3 #include "BME280.h"
s1210160 0:c867a0a05fe8 4 #include "VNH5019.h"
s1210160 0:c867a0a05fe8 5 #include "MBed_Adafruit_GPS.h"
s1210160 0:c867a0a05fe8 6
s1210160 0:c867a0a05fe8 7 // target GPS
s1210160 0:c867a0a05fe8 8 const double target_x = 139.939017, target_y = 37.522390;
s1210160 0:c867a0a05fe8 9
s1210160 0:c867a0a05fe8 10 //******************************************
s1210160 0:c867a0a05fe8 11 //
s1210160 0:c867a0a05fe8 12 // pin setting
s1210160 0:c867a0a05fe8 13 //
s1210160 0:c867a0a05fe8 14 //******************************************
s1210160 0:c867a0a05fe8 15
s1210160 0:c867a0a05fe8 16 // Serial communication
s1210160 0:c867a0a05fe8 17 Serial pc(USBTX, USBRX);
s1210160 0:c867a0a05fe8 18 Serial xbee(p13,p14);
s1210160 0:c867a0a05fe8 19
s1210160 0:c867a0a05fe8 20 // Compass
s1210160 0:c867a0a05fe8 21 DigitalIn compass[4] = {p12, p11, p8, p7};
s1210160 0:c867a0a05fe8 22 // Pressure
s1210160 0:c867a0a05fe8 23 BME280 pressure(p9, p10);
s1210160 0:c867a0a05fe8 24 // MOtor
s1210160 0:c867a0a05fe8 25 VNH5019 motor(p23,p22,p25,p21,p24,p26);
s1210160 0:c867a0a05fe8 26
s1210160 0:c867a0a05fe8 27 DigitalIn short_in(p19);
s1210160 0:c867a0a05fe8 28 DigitalOut short_out(p20);
s1210160 0:c867a0a05fe8 29 DigitalInOut nic(p5);
s1210160 0:c867a0a05fe8 30
s1210160 0:c867a0a05fe8 31
s1210160 0:c867a0a05fe8 32 //******************************************
s1210160 0:c867a0a05fe8 33 //
s1210160 0:c867a0a05fe8 34 // Member variable
s1210160 0:c867a0a05fe8 35 //
s1210160 0:c867a0a05fe8 36 //******************************************
s1210160 0:c867a0a05fe8 37
s1210160 0:c867a0a05fe8 38 // Constructor
s1210160 0:c867a0a05fe8 39 Cansat2016 cansat(motor);
s1210160 0:c867a0a05fe8 40
s1210160 0:c867a0a05fe8 41 // now mode
s1210160 0:c867a0a05fe8 42 int mode = -1;
s1210160 0:c867a0a05fe8 43
s1210160 0:c867a0a05fe8 44 int short_flag = 0;
s1210160 0:c867a0a05fe8 45
s1210160 0:c867a0a05fe8 46 // time interval
s1210160 0:c867a0a05fe8 47 Timer GPS_Timer;
s1210160 0:c867a0a05fe8 48 const int GPS_time = 1000;
s1210160 0:c867a0a05fe8 49 Timer fall_Timer;
s1210160 0:c867a0a05fe8 50 const int fall_time = 30000;
s1210160 0:c867a0a05fe8 51 Timer action_Timer;
s1210160 0:c867a0a05fe8 52 const int action_time = 500;
s1210160 0:c867a0a05fe8 53 Timer stack_Timer;
s1210160 0:c867a0a05fe8 54 const int stack_time = 5000;
s1210160 0:c867a0a05fe8 55
s1210160 0:c867a0a05fe8 56
s1210160 0:c867a0a05fe8 57 //*******************************************
s1210160 0:c867a0a05fe8 58 //
s1210160 0:c867a0a05fe8 59 // standby mode
s1210160 0:c867a0a05fe8 60 //
s1210160 0:c867a0a05fe8 61 //*******************************************
s1210160 0:c867a0a05fe8 62 void standby()
s1210160 0:c867a0a05fe8 63 {
s1210160 0:c867a0a05fe8 64 cansat.set_action('s');
s1210160 0:c867a0a05fe8 65
s1210160 0:c867a0a05fe8 66 if(!short_in) {
s1210160 0:c867a0a05fe8 67 short_flag ++;
s1210160 0:c867a0a05fe8 68 }
s1210160 0:c867a0a05fe8 69
s1210160 0:c867a0a05fe8 70 if(short_flag >= 5) {
s1210160 0:c867a0a05fe8 71 mode = 1;
s1210160 0:c867a0a05fe8 72 fall_Timer.start();
s1210160 0:c867a0a05fe8 73 xbee.printf("-----------FALLING MODE-----------\n");
s1210160 0:c867a0a05fe8 74 }
s1210160 0:c867a0a05fe8 75 }
s1210160 0:c867a0a05fe8 76
s1210160 0:c867a0a05fe8 77 //*******************************************
s1210160 0:c867a0a05fe8 78 //
s1210160 0:c867a0a05fe8 79 // falling mode
s1210160 0:c867a0a05fe8 80 //
s1210160 0:c867a0a05fe8 81 //*******************************************
s1210160 0:c867a0a05fe8 82 void falling(void)
s1210160 0:c867a0a05fe8 83 {
s1210160 0:c867a0a05fe8 84 if(fall_Timer.read_ms() >= fall_time) {
s1210160 0:c867a0a05fe8 85 // nichrome wire is high
s1210160 0:c867a0a05fe8 86 nic = 1;
s1210160 0:c867a0a05fe8 87 xbee.printf("remove the parachute\n");
s1210160 0:c867a0a05fe8 88 wait_ms(3000);
s1210160 0:c867a0a05fe8 89 nic = 0;
s1210160 0:c867a0a05fe8 90 mode = 2;
s1210160 0:c867a0a05fe8 91 action_Timer.start();
s1210160 0:c867a0a05fe8 92 xbee.printf("forward running\n");
s1210160 0:c867a0a05fe8 93
s1210160 0:c867a0a05fe8 94 // running on parachute (10 seconds)
s1210160 0:c867a0a05fe8 95 while(action_Timer.read_ms() <= 10000) {
s1210160 0:c867a0a05fe8 96 cansat.set_action('f');
s1210160 0:c867a0a05fe8 97 cansat.motor_control();
s1210160 0:c867a0a05fe8 98 }
s1210160 0:c867a0a05fe8 99 action_Timer.reset();
s1210160 0:c867a0a05fe8 100 xbee.printf("-----------RUNNING MODE-----------\n");
s1210160 0:c867a0a05fe8 101 }
s1210160 0:c867a0a05fe8 102 }
s1210160 0:c867a0a05fe8 103
s1210160 0:c867a0a05fe8 104 //*******************************************
s1210160 0:c867a0a05fe8 105 //
s1210160 0:c867a0a05fe8 106 // running mode
s1210160 0:c867a0a05fe8 107 //
s1210160 0:c867a0a05fe8 108 //*******************************************
s1210160 0:c867a0a05fe8 109 void running(void)
s1210160 0:c867a0a05fe8 110 {
s1210160 0:c867a0a05fe8 111
s1210160 0:c867a0a05fe8 112 cansat.motor_control();
s1210160 0:c867a0a05fe8 113
s1210160 0:c867a0a05fe8 114 // decide speed and action of robot
s1210160 0:c867a0a05fe8 115 if(action_Timer.read_ms() >= action_time) {
s1210160 0:c867a0a05fe8 116 cansat.calc_distance();
s1210160 0:c867a0a05fe8 117 cansat.robot_compass(compass);
s1210160 0:c867a0a05fe8 118 cansat.target_compass();
s1210160 0:c867a0a05fe8 119 cansat.robot_action();
s1210160 0:c867a0a05fe8 120 xbee.printf("(%f, %f) distance:%f, target:%d, robot:%d, action:%c\n",
s1210160 0:c867a0a05fe8 121 cansat.get_x(), cansat.get_y(), cansat.get_distance(), cansat.get_compass('t'), cansat.get_compass('r'), cansat.get_action());
s1210160 0:c867a0a05fe8 122 action_Timer.reset();
s1210160 0:c867a0a05fe8 123 }
s1210160 0:c867a0a05fe8 124
s1210160 0:c867a0a05fe8 125 // around the target
s1210160 0:c867a0a05fe8 126 if(cansat.get_distance() < 0.5) {
s1210160 0:c867a0a05fe8 127 mode = 100;
s1210160 0:c867a0a05fe8 128 xbee.printf("-----------STOP MODE-----------\n");
s1210160 0:c867a0a05fe8 129 }
s1210160 0:c867a0a05fe8 130
s1210160 0:c867a0a05fe8 131 }
s1210160 0:c867a0a05fe8 132
s1210160 0:c867a0a05fe8 133 //*******************************************
s1210160 0:c867a0a05fe8 134 //
s1210160 0:c867a0a05fe8 135 // stop mode
s1210160 0:c867a0a05fe8 136 //
s1210160 0:c867a0a05fe8 137 //*******************************************
s1210160 0:c867a0a05fe8 138 void stop(void)
s1210160 0:c867a0a05fe8 139 {
s1210160 0:c867a0a05fe8 140
s1210160 0:c867a0a05fe8 141 cansat.set_action('s');
s1210160 0:c867a0a05fe8 142
s1210160 0:c867a0a05fe8 143 }
s1210160 0:c867a0a05fe8 144
s1210160 0:c867a0a05fe8 145
s1210160 0:c867a0a05fe8 146 //*******************************************
s1210160 0:c867a0a05fe8 147 //
s1210160 0:c867a0a05fe8 148 // main processing
s1210160 0:c867a0a05fe8 149 //
s1210160 0:c867a0a05fe8 150 //*******************************************
s1210160 0:c867a0a05fe8 151
s1210160 0:c867a0a05fe8 152 int main()
s1210160 0:c867a0a05fe8 153 {
s1210160 0:c867a0a05fe8 154
s1210160 0:c867a0a05fe8 155 wait(3);
s1210160 0:c867a0a05fe8 156
s1210160 0:c867a0a05fe8 157 pc.baud(57600);
s1210160 0:c867a0a05fe8 158 xbee.baud(57600);
s1210160 0:c867a0a05fe8 159
s1210160 0:c867a0a05fe8 160 //GPS setting
s1210160 0:c867a0a05fe8 161 Serial* gps_Serial = new Serial(p28,p27);
s1210160 0:c867a0a05fe8 162 Adafruit_GPS myGPS(gps_Serial);
s1210160 0:c867a0a05fe8 163 myGPS.begin(9600);
s1210160 0:c867a0a05fe8 164
s1210160 0:c867a0a05fe8 165 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
s1210160 0:c867a0a05fe8 166 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
s1210160 0:c867a0a05fe8 167 myGPS.sendCommand(PGCMD_ANTENNA);
s1210160 0:c867a0a05fe8 168
s1210160 0:c867a0a05fe8 169 GPS_Timer.start();
s1210160 0:c867a0a05fe8 170
s1210160 0:c867a0a05fe8 171 // target GPS
s1210160 0:c867a0a05fe8 172 cansat.set_targetGPS(target_x, target_y);
s1210160 0:c867a0a05fe8 173 // robot GPS
s1210160 0:c867a0a05fe8 174 double robot_x=0.0, robot_y=0.0;
s1210160 0:c867a0a05fe8 175
s1210160 0:c867a0a05fe8 176 short_out = 1;
s1210160 0:c867a0a05fe8 177 nic = 0;
s1210160 0:c867a0a05fe8 178
s1210160 0:c867a0a05fe8 179 xbee.printf("start\n");
s1210160 0:c867a0a05fe8 180 xbee.printf("-----------STANDBY MODE-----------\n");
s1210160 0:c867a0a05fe8 181 wait_ms(1000);
s1210160 0:c867a0a05fe8 182
s1210160 0:c867a0a05fe8 183 while(true) {
s1210160 0:c867a0a05fe8 184
s1210160 0:c867a0a05fe8 185 myGPS.read();
s1210160 0:c867a0a05fe8 186 if ( myGPS.newNMEAreceived() ) {
s1210160 0:c867a0a05fe8 187 if ( !myGPS.parse(myGPS.lastNMEA()) ) {
s1210160 0:c867a0a05fe8 188 continue;
s1210160 0:c867a0a05fe8 189 }
s1210160 0:c867a0a05fe8 190 }
s1210160 0:c867a0a05fe8 191
s1210160 0:c867a0a05fe8 192 // get robot GPS
s1210160 0:c867a0a05fe8 193 if (GPS_Timer.read_ms() >= GPS_time) {
s1210160 0:c867a0a05fe8 194 if(myGPS.fix) {
s1210160 0:c867a0a05fe8 195 robot_x = (double)myGPS.longitudeH +(double)(myGPS.longitudeL / 10000.0 / 60.0);
s1210160 0:c867a0a05fe8 196 robot_y = (double)myGPS.latitudeH + (double)(myGPS.latitudeL / 10000.0 / 60.0);
s1210160 0:c867a0a05fe8 197
s1210160 0:c867a0a05fe8 198 cansat.set_robotGPS(robot_x, robot_y);
s1210160 0:c867a0a05fe8 199
s1210160 0:c867a0a05fe8 200 }
s1210160 0:c867a0a05fe8 201 GPS_Timer.reset();
s1210160 0:c867a0a05fe8 202 }
s1210160 0:c867a0a05fe8 203
s1210160 0:c867a0a05fe8 204
s1210160 0:c867a0a05fe8 205 switch(mode) {
s1210160 0:c867a0a05fe8 206 case -1: // stanby mode
s1210160 0:c867a0a05fe8 207 standby();
s1210160 0:c867a0a05fe8 208 break;
s1210160 0:c867a0a05fe8 209 case 1: // falling mode
s1210160 0:c867a0a05fe8 210 falling();
s1210160 0:c867a0a05fe8 211 break;
s1210160 0:c867a0a05fe8 212 case 2: // running mode
s1210160 0:c867a0a05fe8 213 running();
s1210160 0:c867a0a05fe8 214 break;
s1210160 0:c867a0a05fe8 215 case 100: // stop mode
s1210160 0:c867a0a05fe8 216 stop();
s1210160 0:c867a0a05fe8 217 break;
s1210160 0:c867a0a05fe8 218 }
s1210160 0:c867a0a05fe8 219 }
s1210160 0:c867a0a05fe8 220 }