![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
GPS and IMU reading works
Dependencies: mbed Servo SDFileSystem
Diff: main.cpp
- Revision:
- 46:305112d73c69
- Parent:
- 44:4d90645d15ec
- Child:
- 47:4b490931e75f
diff -r dfa689f159ce -r 305112d73c69 main.cpp --- a/main.cpp Tue Nov 03 16:31:44 2015 +0000 +++ b/main.cpp Wed Nov 04 21:45:34 2015 +0000 @@ -60,14 +60,17 @@ double D_GPS_VelocityKph=0; double D_temp=0; int asterisk_idx; +int current_task=0; double Longitude_Path[MAX_TASK_SIZE]; double Latitude_Path[MAX_TASK_SIZE]; +int IF_Path_Complete[MAX_TASK_SIZE]; double dis[MAX_TASK_SIZE]; double ang[MAX_TASK_SIZE]; int autonomous_mode=0; -int arrived_destination=0; +int arrived_destination=0; // fot test purpose + double angle_to_path_point,distance_to_path_point,desired_speed,distance_to_route; double rudder_ctrl_parameters[3]; double rudder_variables[5];//,,,prev,out @@ -99,8 +102,10 @@ void initialize() { fill(Longitude_Path, Longitude_Path+MAX_TASK_SIZE, 181); fill(Latitude_Path, Latitude_Path+MAX_TASK_SIZE, 181); + fill(IF_Path_Complete, IF_Path_Complete+MAX_TASK_SIZE, 0); fill(dis, dis+MAX_TASK_SIZE, -1); fill(ang, ang+MAX_TASK_SIZE, -361); + current_task = 1; } void updateIMU(string IMU_data) { @@ -381,6 +386,7 @@ rudder_ctrl_parameters[2]=0; } + void update_controller_tmr_ISR() { /*Input: angle(deg) difference between heading from IMU and path point航向与目的点之间的夹角 angle to path point @@ -454,6 +460,7 @@ }else{ if (autonomous_mode){set_servo_position(wingServo,0,-45,0,45,1);} arrived_destination=1; + IF_Path_Complete[current_task]=1; } //actuator saturation @@ -496,10 +503,10 @@ initialize(); //float angle=20; while (1) { - - angle_to_path_point=getAngle(1); //positive if pathpoint is on the right of the boat - distance_to_path_point=getCTE(1); //positive if trace is on the right of the boat - distance_to_route=getDistance(1); //no sign + current_task = get_current_task(); + angle_to_path_point=getAngle(current_task); //positive if pathpoint is on the right of the boat + distance_to_path_point=getCTE(current_task); //positive if trace is on the right of the boat + distance_to_route=getDistance(current_task); //no sign arrived_destination=0; //will be 1 if arrived destination autonomous_mode=0; //set 1 if in autonomous mode, set 0 bypass the controller