![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
GPS and IMU reading works
Dependencies: mbed Servo SDFileSystem
Diff: main.cpp
- Revision:
- 43:a430d5e4f33d
- Parent:
- 42:e1a2a6daf70b
- Child:
- 44:4d90645d15ec
diff -r e1a2a6daf70b -r a430d5e4f33d main.cpp --- a/main.cpp Sun Nov 01 22:20:08 2015 +0000 +++ b/main.cpp Tue Nov 03 15:54:29 2015 +0000 @@ -67,6 +67,7 @@ double ang[MAX_TASK_SIZE]; int autonomous_mode=0; +int arrived_destination=0; 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 @@ -449,8 +450,10 @@ //very accurate speed reading in our application if (distance_to_path_point>30){ if (autonomous_mode){set_servo_position(wingServo,45,-45,0,45,1);} + arrived_destination=0; }else{ if (autonomous_mode){set_servo_position(wingServo,0,-45,0,45,1);} + arrived_destination=1; } //actuator saturation @@ -496,11 +499,12 @@ //Enmao, please put your function below: - angle_to_path_point=0; //positive if pathpoint is on the right of the boat - distance_to_path_point=0; //positive if trace is on the right of the boat - distance_to_route=0; //no sign + 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 - autonomous_mode=0; //set 1 if in autonomous mode + arrived_destination=0; //will be 1 if arrived destination + autonomous_mode=0; //set 1 if in autonomous mode, set 0 bypass the controller @@ -509,7 +513,7 @@ // set_servo_position(wingServo, angle, 0, 0, 180, 1); // angle=angle+10; - wait(0.2); + wait(2); //printStateIMU(); //printStateGPS(); //printPath();