![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
GPS and IMU reading works
Dependencies: mbed Servo SDFileSystem
Diff: main.cpp
- Revision:
- 26:353a80179346
- Parent:
- 24:8e38cc14150c
- Child:
- 27:1be1f25be449
diff -r 30966ed7f7e8 -r 353a80179346 main.cpp --- a/main.cpp Thu Sep 10 17:40:48 2015 +0000 +++ b/main.cpp Thu Sep 10 18:15:16 2015 +0000 @@ -11,6 +11,7 @@ Servo rudderServo(p25); Servo wingServo(p26); SDFileSystem sd(p5, p6, p7, p8, "sd");// mosi, miso, sck, cs +Ticker ctrl_updt_timer; //timer to refresh controller char IMU_message[256]; int IMU_message_counter=0; @@ -62,6 +63,12 @@ double Longtitude_Path[MAX_TASK_SIZE]; double Latitude_Path[MAX_TASK_SIZE]; +double angle_to_path_point,distance_to_path_point,desired_speed; +double rudder_ctrl_parameters[3]; +double rudder_variables[5];//,,,prev,out +double T=0.2; //controller update period=0.2sec, 5Hz + + vector<string> split(const string &s, char delim) { stringstream ss(s); string item; @@ -276,16 +283,6 @@ } - -void update_controller() { - /*Input: angle(deg) difference between heading from IMU and path point航向与目的点之间的夹角 - distance(meter) to the next path point - - Function: drive two servos to navigate the sailboat to the desired path point - */ - -} - void set_servo_position(Servo targetServo, float angleDeg, float minDeg, float minNorVal, float maxDeg, float maxNorVal){ /*angleDeg: desired angle minDeg: minimum angle in degrees @@ -314,6 +311,39 @@ targetServo=pos; } + +void initialize_controller(){ + rudder_variables[0]=0; + rudder_variables[1]=0; + rudder_variables[2]=0; + rudder_variables[3]=0; + rudder_variables[4]=0; + rudder_ctrl_parameters[0]=1; + rudder_ctrl_parameters[1]=0; + rudder_ctrl_parameters[2]=0; +} + +void update_controller_tmr_ISR() { + /*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; + + 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]; + + + //Drive servos + set_servo_position(rudderServo,rudder_variables[4],-45,0,45,1); +} + + + int log_data_SD(){ FILE *fp = fopen("/sd/dataLog/dataLog.txt", "w"); if(fp == NULL) { @@ -334,6 +364,9 @@ GPS.attach(&GPS_serial_ISR); pc.baud(115200); pc.attach(&PC_serial_ISR); + ctrl_updt_timer.attach(&update_controller_tmr_ISR, T); // Update controller at 1/T Hz + + initialize_controller(); //float angle=20; while (1) { @@ -341,6 +374,8 @@ // set_servo_position(rudderServo, angle, 0, 0, 180, 1); // set_servo_position(wingServo, angle, 0, 0, 180, 1); // angle=angle+10; + + wait(0.4); printStateIMU(); printStateGPS();