Revision 70:0da6ca845762, committed 2013-04-14

Comitter:

rsavitski

Date:

Sun Apr 14 22:02:21 2013 +0000

Parent:

69:4b7bb92916da

Child:

71:eb1956c2d316

Child:

76:532d9bc1d2aa

Commit message:

top flipper pushing

Changed in this revision

--- a/Processes/AI/ai.cpp	Sun Apr 14 20:23:07 2013 +0000
+++ b/Processes/AI/ai.cpp	Sun Apr 14 22:02:21 2013 +0000
@@ -19,6 +19,8 @@
     Waypoint *wpptr;
 };
 
+void arm_upper(const void *dummy); //TODO: kill
+
 void delayed_setter(const void *tid_wpptr); //TODO: kill the hack
 
 void ailayer(void const *dummy)
@@ -42,6 +44,8 @@
 
     bool firstavoidstop = 1;
     delayed_struct ds = {Thread::gettid(),&current_waypoint};
+    RtosTimer delayed_wp_set(delayed_setter, osTimerOnce, (void *)&ds);
+    RtosTimer delayed_armer(arm_upper, osTimerOnce);
     
     for (float phi=(180-11.25)/180*PI; phi > 11.25/180*PI;)
     {
@@ -52,7 +56,8 @@
             //Thread::wait(1000);
             arm::upper_arm.go_down();
             delayed_done = false;
-            RtosTimer delayed_wp_set(delayed_setter, osTimerOnce, (void *)&ds);
+            delayed_armer.start(1200);
+            delayed_wp_set.start(2400);
             //Thread::wait(2000);
             //arm::upper_arm.go_up();
             
@@ -83,10 +88,15 @@
     }
 }
 
+void arm_upper(const void *dummy)
+{
+    arm::upper_arm.go_up();
+}
+
 void delayed_setter(const void *tid_wpptr) //TODO: kill the hack
 {
     delayed_struct *dsptr = (delayed_struct *)tid_wpptr;
-    arm::upper_arm.go_up();
+    //arm::upper_arm.go_up();
     motion::setNewWaypoint(dsptr->tid,dsptr->wpptr);
     delayed_done = true;
 }

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