2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Diff: Processes/Motion/motion.cpp
- Revision:
- 70:0da6ca845762
- Parent:
- 69:4b7bb92916da
- Child:
- 72:7996aa8286ae
- Child:
- 76:532d9bc1d2aa
diff -r 4b7bb92916da -r 0da6ca845762 Processes/Motion/motion.cpp --- a/Processes/Motion/motion.cpp Sun Apr 14 20:23:07 2013 +0000 +++ b/Processes/Motion/motion.cpp Sun Apr 14 22:02:21 2013 +0000 @@ -23,8 +23,8 @@ namespace motion { -volatile int collavoiden = 0; -AvoidDstSensor ADS(P_FWD_DISTANCE_SENSOR); +volatile int collavoiden = 0; // TODO: kill oskar's code +AvoidDstSensor ADS(P_FWD_DISTANCE_SENSOR); //TODO: kill oskar's hack // motion commands supported enum motion_type_t { motion_waypoint }; @@ -46,6 +46,8 @@ // local copy of the current active motion motion_cmd current_motion; +Waypoint target_waypoint = {0,0,0,0,0}; //local wp copy, TODO: fix and make a shared local memory pool for any movement cmd to be copied to + Mutex waypoint_flag_mutex; // local to waypoint motion handler @@ -68,7 +70,8 @@ void waypoint_motion_handler() { // save target waypoint - Waypoint target_waypoint = *current_motion.wp_ptr; + //Waypoint target_waypoint = *current_motion.wp_ptr; + // get current state from Kalman State current_state = Kalman::getState(); @@ -114,9 +117,9 @@ waypoint_flag_mutex.unlock(); // angular velocity controller - const float p_gain_av = 0.7; //TODO: tune + const float p_gain_av = 1;//0.7; //TODO: tune - const float max_av = 0.5; // radians per sec //TODO: tune + const float max_av = 1;//0.5; // radians per sec //TODO: tune // angle error [-pi, pi] float angular_v = p_gain_av * angle_err; @@ -129,9 +132,9 @@ // forward velocity controller - const float p_gain_fv = 0.7; //TODO: tune + const float p_gain_fv = 1;//0.7; //TODO: tune - float max_fv = 0.2; // meters per sec //TODO: tune + float max_fv = 0.3;//0.2; // meters per sec //TODO: tune float max_fv_reverse = 0.05; //TODO: tune const float angle_envelope_exponent = 32;//512; //8.0; //TODO: tune @@ -183,7 +186,8 @@ current_motion.setter_tid = setter_tid_in; current_motion.motion_type = motion_waypoint; - current_motion.wp_ptr = new_wp; + //current_motion.wp_ptr = new_wp; //TODO: need to make local copy or edits to mem ptr contents screw motion over + target_waypoint = *new_wp; }