cansat program1
Dependencies: ADXL345 BME280 HMC5883L ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST cansat mbed
Fork of Cansat_program4_1 by
Diff: main.cpp
- Revision:
- 11:19091694455e
- Parent:
- 10:ce253d8a5f2c
- Child:
- 12:5724d4a57a4c
diff -r ce253d8a5f2c -r 19091694455e main.cpp --- a/main.cpp Mon Aug 10 10:01:55 2015 +0000 +++ b/main.cpp Mon Aug 10 18:22:40 2015 +0000 @@ -105,7 +105,8 @@ void Kalman(double Latitude,double Longitude); int change = 0; -int mode = -1; //ロボットのモード +int mode = 2; //ロボットのモード +double target_x = 37.52260177176629,target_y = 139.93881346945034; double goal_Pressure, goal_Temperature,goal_Humidity; //地表地点の気圧、気温、湿度 Timer sep_Timer; @@ -192,9 +193,9 @@ } //ロボットの動き方 -char robot_Action(double robot_angle, double target_angle) { +char robot_Action(int robot_angle, int target_angle) { - double n, t, r; + int n, t, r; t = target_angle; r = robot_angle; n = r - t; @@ -277,7 +278,7 @@ } int calc_angle(double c){ - if(c > 337.5 ||(c >=0 && c < 22.5) return 0; + if(c > 337.5 ||(c >=0 && c < 22.5)) return 0; else if(c >= 22.5 && c < 67.5) return 1; else if(c >= 67.5 && c < 112.5) return 2; else if(c >= 112.5 && c < 157.5) return 3; @@ -425,6 +426,11 @@ //interrupt start refresh_Timer.start(); + + cansat.set_target(target_x, target_y); + wait_ms(10000); + + compass_interrupt.attach(&Compass_intrpt, 0.5); printf("start\n"); @@ -455,8 +461,7 @@ break; } - compass_interrupt.attach(&Compass_intrpt, 0.5); - while(1){ + /* while(1){ printf("compass x : %i, compass y : %i, compass z : %i\n", raw[0], raw[1], raw[2]); printf("set compass x : %i, set compass y : %i, set compass z : %i\n", cansat.get_compass_x(), cansat.get_compass_y(), cansat.get_compass_z()); @@ -465,7 +470,7 @@ printf("robot angle : %d\n", calc_angle(headingLPF)); 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, @@ -483,8 +488,9 @@ //print_gps(count); Get_GPS(&myGPS); log++; - // pc.printf("%d times, x:%f, y:%f, speed:%d\n", log, cansat.get_robot_x(), cansat.get_robot_y(), cansat.get_speed()); - // pc.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_z(), cansat.get_pressure()); + pc.printf("%d times, x:%f, y:%f, speed:%d\n", log, cansat.get_robot_x(), cansat.get_robot_y(), cansat.get_speed()); + pc.printf("moter command: %c\n", cansat.motor_command); + pc.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_z(), cansat.get_pressure()); } }