CanSat2015aizu
08/13
Dependencies: ADXL345 BME280 HMC5883L ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST cansat2 mbed
Fork of
Cansat_program4
by 
CanSat2015aizu
Diff: main.cpp
- Revision:
- 19:cb3a4b4c3526
- Parent:
- 18:c86872baed44
- Child:
- 20:f92bdcda5a60
--- a/main.cpp Wed Aug 12 23:32:49 2015 +0000
+++ b/main.cpp Thu Aug 13 07:11:57 2015 +0000
@@ -66,6 +66,7 @@
DigitalInOut nic(p5);
int short_flag = 0;
+ int running_flag = 0;
Timer compass_Timer;
const int compass_Time = 500;
@@ -89,8 +90,8 @@
double preHeading = 0.0;
int maxX, minX, maxY, minY;
-const int ofsX = 46; //calibration x
-const int ofsY = -950; //calibration y
+const int ofsX = 36; //calibration x
+const int ofsY = -425; //calibration y
int16_t raw[3];
@@ -114,8 +115,8 @@
int change = 0;
int mode = -1; //ロボットのモード
-double target_x = 40.085592,target_y = 139.592383;
-double goal_Pressure = 1003.5, goal_Temperature,goal_Humidity; //地表地点の気圧、気温、湿度
+double target_x = 139.987305,target_y = 40.142655;
+double goal_Pressure = 1002.20, goal_Temperature,goal_Humidity; //地表地点の気圧、気温、湿度
Timer sep_Timer;
const int sep_Time = 3000; //seperate time in ms
@@ -172,8 +173,8 @@
if (myGPS->fix) {
cansat.nowStatus = GPS_AVAIL;
- cansat.set_robot_x((double)myGPS->latitudeH + (double)(myGPS->latitudeL / 1000000.0 / 60.0));
- cansat.set_robot_y((double)myGPS->longitudeH +(double)(myGPS->longitudeL / 1000000.0 / 60.0));
+ cansat.set_robot_y((double)myGPS->latitudeH + (double)(myGPS->latitudeL / 10000.0 / 60.0));
+ cansat.set_robot_x((double)myGPS->longitudeH +(double)(myGPS->longitudeL / 10000.0 / 60.0));
if(flag < COUNTER_MAX){
@@ -309,7 +310,7 @@
heading2 = heading1;
heading1 = heading0;
heading0 = heading;
- headingLPF = (heading0 + heading1 + heading2 + heading3)/4; //low pass filter
+ headingLPF = (heading0 + heading1 + heading2 + heading3)/4.0; //low pass filter
headingLPF = headingLPF * 180.0 / M_PI;
// pc.printf("heading=%f\r\n",headingLPF);
@@ -337,6 +338,7 @@
}
if(short_flag >= 5){
mode = 1;
+ parachute_Timer.start();
}
}
@@ -346,7 +348,7 @@
******************************/
void falling_print(){
- xbee.printf("%d times, pressure:%04.2f hPa\n", log_number, cansat.get_pressure());
+ xbee.printf("%d: %04.2f hPa\n", log_number, cansat.get_pressure());
log_number++;
@@ -391,15 +393,19 @@
void running_print(){
- xbee.printf("%d times, x:%f, y:%f, speed:%d\n", log_number, cansat.get_robot_x(), cansat.get_robot_y(), cansat.get_speed());
- // xbee.printf("moter command: %c\n", cansat.motor_command);
- // xbee.printf("robot_angle:%d, target_angle:%d, compass_angle:%f\n",cansat.get_robot_angle(), cansat.get_target_angle(),cansat.get_compass_angle());
-xbee.printf("robot_x:%f, robot_y:%f, target_x:%f, target_y:%f", cansat.get_robot_x(), cansat.get_robot_y(), cansat.get_target_x(), cansat.get_target_y());
- double y1 = cansat.get_target_y();
- double y2 = cansat.get_robot_y();
- double x1 = cansat.get_target_x();
- double x2 = cansat.get_robot_x();
- xbee.printf("taret_angle:%f",robot_compass(x1-x2, y1-y2));
+ if(running_flag <= 20){
+ xbee.printf("%d: %f, %f, %d\n", log_number, cansat.get_robot_x(), cansat.get_robot_y(), cansat.get_speed());
+ }
+ else{
+ xbee.printf("%d: %f, %f, %d\n", log_number, cansat.get_robotKalman_x(), cansat.get_robotKalman_y(), cansat.get_speed());
+ }
+ xbee.printf("%c, %f\n", cansat.motor_command, cansat.get_compass_angle());
+ xbee.printf("%d, %d\n",cansat.get_robot_angle(), cansat.get_target_angle());
+ // double y1 = cansat.get_target_y();
+ // double y2 = cansat.get_robot_y();
+ // double x1 = cansat.get_target_x();
+ // double x2 = cansat.get_robot_x();
+ // xbee.printf("taret_angle:%f",robot_compass(x1-x2, y1-y2));
log_number++;
@@ -412,17 +418,32 @@
double y2 = cansat.get_robot_y();
double x1 = cansat.get_target_x();
double x2 = cansat.get_robot_x();
+ double x_k2 = cansat.get_robotKalman_x();
+ double y_k2 = cansat.get_robotKalman_y();
double dx = x2 - x1;
double dy = y2 - y1;
- double y_sub = dy*111135;
- double x_sub = dx*91191;
+ double dx_k = x_k2 - x1;
+ double dy_k = y_k2 - y1;
+ double y_sub;
+ double x_sub;
+
+ if(running_flag < 20){
+ y_sub = dy*111135.0;
+ x_sub = dx*91191.0;
+ cansat.set_target_distance(sqrt(y_sub*y_sub + x_sub*x_sub));
+ cansat.set_target_angle(calc_angle(robot_compass(x1-x2, y1-y2)));
+ running_flag++;
+ }
+ else{
+ y_sub = dy_k*111135.0;
+ x_sub = dx_k*91191.0;
+ cansat.set_target_distance(sqrt(y_sub*y_sub + x_sub*x_sub));
+ cansat.set_target_angle(calc_angle(robot_compass(x1-x_k2, y1-y_k2)));
+ }
- cansat.set_target_distance(sqrt(y_sub*y_sub + x_sub*x_sub));
- cansat.set_target_angle(calc_angle(robot_compass(x1-x2, y1-y2)));
-
- if(cansat.get_target_distance() < 10) cansat.set_speed(32);
- else if(cansat.get_target_distance() < 20 && cansat.get_target_distance() > 10) cansat.set_speed(64);
- else cansat.set_speed(128);
+ if(cansat.get_target_distance() < 10) cansat.set_speed(64);
+ else if(cansat.get_target_distance() < 20 && cansat.get_target_distance() > 10) cansat.set_speed(128);
+ else cansat.set_speed(255);
if(fall_flag == 1 && sep_Timer.read_ms() >= sep_Time){
nic = 0;
@@ -481,7 +502,7 @@
pc.baud(PC_BAUD_RATE);
//set xbee frequency to 57600bps
//xbee.begin(XBEE_BAUD_RATE);
- xbee.baud(9600);
+ xbee.baud(57600);
//Compass setting
compass.init();
@@ -491,6 +512,8 @@
Timer refresh_Timer;
const int refresh_Time = 1000; //refresh time in ms
int count = 0;
+ Timer compass_refresh_Timer;
+ const int compass_refresh_Time = 500;
myGPS.begin(GPS_BAUD_RATE);
@@ -506,24 +529,53 @@
compass_Timer.start();
running_Timer.start();
sep_Timer.start();
- parachute_Timer.start();
+ compass_refresh_Timer.start();
cansat.set_target(target_x, target_y);
// wait_ms(10000);
- compass_interrupt.attach(&Compass_intrpt, 0.5);
+ //compass_interrupt.attach(&Compass_intrpt, 0.5);
+
printf("start\n");
+ xbee.printf("target: %f, %f\n",cansat.get_target_x(), cansat.get_target_y());
//int mode = -1;
nic.output();
nic = 0;
short_out = 1;
wait_ms(1000);
- if(short_out){
- xbee.printf("HIGH\n");
- }
+// if(short_out){
+// xbee.printf("HIGH\n");
+// }
while (true) {
+
+ myGPS.read();
+ //recive gps module
+ //check if we recieved a new message from GPS, if so, attempt to parse it,
+ if ( myGPS.newNMEAreceived() ) {
+ if ( !myGPS.parse(myGPS.lastNMEA()) ) {
+ continue;
+ }
+ else{
+ count++;
+ }
+ }
+ //一定時間ごとに自分のGPSデータを取得し、AigamozuControlPacketsないのagzPointとagzPointKalmanに格納する
+ if (refresh_Timer.read_ms() >= refresh_Time) {
+ refresh_Timer.reset();
+ //print_gps(count);
+ Get_GPS(&myGPS);
+ //log++;
+ /* xbee.printf("%d times, x:%f, y:%f, speed:%d\n", log, cansat.get_robot_x(), cansat.get_robot_y(), cansat.get_speed());
+ xbee.printf("moter command: %c\n", cansat.motor_command);
+ xbee.printf("robot_angle:%d, target_angle:%d, robot_compass:%f, %04.2f hPa\n",cansat.get_robot_angle(), cansat.get_target_angle(), cansat.get_compass_angle(), cansat.get_pressure());
+ */
+ }
+ if(compass_refresh_Timer.read_ms() >= compass_refresh_Time){
+ Compass_intrpt();
+ compass_refresh_Timer.reset();
+ }
switch(mode){
//スタートモード：パラシュートが開くまではこのモードを実行
@@ -557,28 +609,7 @@
printf("set robot angle : %d\n", cansat.get_robot_angle());
}
*/
- myGPS.read();
- //recive gps module
- //check if we recieved a new message from GPS, if so, attempt to parse it,
- if ( myGPS.newNMEAreceived() ) {
- if ( !myGPS.parse(myGPS.lastNMEA()) ) {
- continue;
- }
- else{
- count++;
- }
- }
- //一定時間ごとに自分のGPSデータを取得し、AigamozuControlPacketsないのagzPointとagzPointKalmanに格納する
- if (refresh_Timer.read_ms() >= refresh_Time) {
- refresh_Timer.reset();
- //print_gps(count);
- Get_GPS(&myGPS);
- //log++;
- /* xbee.printf("%d times, x:%f, y:%f, speed:%d\n", log, cansat.get_robot_x(), cansat.get_robot_y(), cansat.get_speed());
- xbee.printf("moter command: %c\n", cansat.motor_command);
- xbee.printf("robot_angle:%d, target_angle:%d, robot_compass:%f, %04.2f hPa\n",cansat.get_robot_angle(), cansat.get_target_angle(), cansat.get_compass_angle(), cansat.get_pressure());
- */
- }
+
}
}
\ No newline at end of file