Diff: orion_main.cpp

Revision:

30:44676e1b38f8

Parent:

29:a6a812ee83ea

--- a/orion_main.cpp	Tue Mar 20 03:47:56 2018 +0000
+++ b/orion_main.cpp	Tue Mar 20 05:56:22 2018 +0000
@@ -71,10 +71,10 @@
 pidBearing PID_B; //PID control for bearing
 pidControl PID_L, PID_R; //pidcontroller for left and right motors
 
-//**new PID control**
+//**new PID control**/*#LV*/
 float bearingToWaypoint, robotFrame, prevError, speedChange;
 float k3, cmdL, cmdR, Error;
-//**end of new control **
+//**end of new control **/*#LV*/
 
 Timer motorControl_t;
 float rpm_cmd[2]; //drive motor rpm command
@@ -410,7 +410,7 @@
         //if((imu.imu_stabilized[1] ==1) && (go_cmd == 1)) {
         if(imu.imu_stabilized[1] ==1) {
 
-            
+/*#LV*/            
             PID_B.setSpeedChange(   &W_l, &W_r, &speedChange, &Error, 
                                     target_waypoint,
                                     localization.position,
@@ -419,7 +419,7 @@
                                     waypoints_set[waypoint_index - 2][0], waypoints_set[waypoint_index - 2][1], 
                                     &k3,
                                     3.5 ); //new controller
-
+/*#LV*/
 
 
             if(waypointSetFinish_flag == 0) {
@@ -482,12 +482,14 @@
 //            else
 //                waypointReached_flag = 0;
 
+/*#LV*/
         PID_B.findRobotFrameDistance(target_waypoint, localization.position, &robotFrame);   
                   
             if(robotFrame <= waypointZone)
                 waypointReached_flag = 1;
             else
                 waypointReached_flag = 0; 
+/*#LV*/                
 
         }
         Thread::wait(imu_UpdatePeriodMS);