Diff: Processes/Motion/motion.cpp

Revision:

70:0da6ca845762

Parent:

69:4b7bb92916da

Child:

72:7996aa8286ae

Child:

76:532d9bc1d2aa

--- 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;
 }