![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
GPS and IMU reading works
Dependencies: mbed Servo SDFileSystem
Diff: main.cpp
- Revision:
- 42:e1a2a6daf70b
- Parent:
- 41:d207b407c8bc
- Child:
- 43:a430d5e4f33d
diff -r d207b407c8bc -r e1a2a6daf70b main.cpp --- a/main.cpp Sun Nov 01 01:43:23 2015 +0000 +++ b/main.cpp Sun Nov 01 22:20:08 2015 +0000 @@ -5,8 +5,8 @@ DigitalOut led3(LED3); DigitalOut led4(LED4); -//Serial pc(p9, p10); -Serial pc(USBTX, USBRX); +Serial pc(p9, p10); +//Serial pc(USBTX, USBRX); Serial IMU(p28, p27); // tx, rx Serial GPS(p13, p14); // tx, rx Servo rudderServo(p25); @@ -422,6 +422,51 @@ + +void update_controller_tmr_ISR2() { + /*Input: angle(deg) difference between heading from IMU and path point航向与目的点之间的夹角 + distance(meter) to the next path point + Global Variables: angle_to_path_point,distance_to_path_point,distance_to_route; + + Function: drive two servos to navigate the sailboat to the desired path point + */ + + rudder_variables[0]=rudder_ctrl_parameters[0]*angle_to_path_point; + rudder_variables[1]=rudder_variables[1]+rudder_ctrl_parameters[1]*angle_to_path_point*T; + rudder_variables[2]=(rudder_variables[3]-angle_to_path_point)/T; + rudder_variables[3]=angle_to_path_point; + rudder_variables[4]=rudder_variables[0]+rudder_variables[1]+rudder_variables[2]; + + /*if (distance_to_route>0){ + rudder_variables[4]=rudder_variables[4]+10; + }else{ + rudder_variables[4]=rudder_variables[4]-10; + }*/ + + + //bang-bang controller for vehicle velocity + //Our sailboat is a slow moving vehicle and GPS cannot provide + //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);} + }else{ + if (autonomous_mode){set_servo_position(wingServo,0,-45,0,45,1);} + } + + //actuator saturation + if (rudder_variables[4]> 45){rudder_variables[4]= 45;} + if (rudder_variables[4]<-45){rudder_variables[4]=-45;} + + //Drive servos + if (autonomous_mode){set_servo_position(rudderServo,rudder_variables[4],-45,0,45,1);} + + pc.printf("%f\n",rudder_variables[4]); +} + + + + + int log_data_SD(){ FILE *fp = fopen("/sd/dataLog/dataLog.txt", "w"); if(fp == NULL) { @@ -442,7 +487,8 @@ GPS.attach(&GPS_serial_ISR); pc.baud(9600); pc.attach(&PC_serial_ISR); - ctrl_updt_timer.attach(&update_controller_tmr_ISR, T); // Update controller at 1/T Hz + //ctrl_updt_timer.attach(&update_controller_tmr_ISR, T); // Update controller at 1/T Hz + ctrl_updt_timer.attach(&update_controller_tmr_ISR2, T); // Update controller at 1/T Hz initialize_controller(); initialize(); //float angle=20; @@ -450,9 +496,9 @@ //Enmao, please put your function below: - angle_to_path_point=0; - distance_to_path_point=0; - distance_to_route=0; + 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 autonomous_mode=0; //set 1 if in autonomous mode