Diff: orion_main.cpp

Revision:

29:a6a812ee83ea

Parent:

28:39d694b0e998

Child:

30:44676e1b38f8

--- a/orion_main.cpp	Mon Mar 19 04:06:32 2018 +0000
+++ b/orion_main.cpp	Tue Mar 20 03:47:56 2018 +0000
@@ -72,16 +72,8 @@
 pidControl PID_L, PID_R; //pidcontroller for left and right motors
 
 //**new PID control**
-
-float bearingToWaypoint;
-float normalDistanceError;
-float prevError;
-float speedChange;
-
-float k3, cmdL, cmdR, aError;
-float dist[3]; 
-
-
+float bearingToWaypoint, robotFrame, prevError, speedChange;
+float k3, cmdL, cmdR, Error;
 //**end of new control **
 
 Timer motorControl_t;
@@ -419,9 +411,14 @@
         if(imu.imu_stabilized[1] ==1) {
 
             
-            PID_B.setSpeedChange(   &W_l, &W_r, &speedChange, &aError, target_waypoint[0], target_waypoint[1], 
-                        localization.position[0], localization.position[1], imu.Pose[2], purePursuit.robotFrame_targetDistance,
-                        waypoints_set[waypoint_index - 2][0], waypoints_set[waypoint_index - 2][1], &k3, 3.5); //new controller
+            PID_B.setSpeedChange(   &W_l, &W_r, &speedChange, &Error, 
+                                    target_waypoint,
+                                    localization.position,
+                                    imu.Pose[2], 
+                                    &robotFrame,
+                                    waypoints_set[waypoint_index - 2][0], waypoints_set[waypoint_index - 2][1], 
+                                    &k3,
+                                    3.5 ); //new controller
 
 
 
@@ -478,12 +475,20 @@
     while(true) {
         if(imu.imu_stabilized[0] ==1) {
             //purePursuit.GenVGW(&purePursuit_velocity, &purePursuit_gamma, &purePursuit_omega, target_waypoint, 400.0, localization.position, DEG_TO_RAD(imu.Pose[2]));
-            purePursuit.GenVGW(&purePursuit_velocity, &purePursuit_gamma, &purePursuit_omega, target_waypoint, target_velocity, localization.position, DEG_TO_RAD(imu.Pose[2]));
+//            purePursuit.GenVGW(&purePursuit_velocity, &purePursuit_gamma, &purePursuit_omega, target_waypoint, target_velocity, localization.position, DEG_TO_RAD(imu.Pose[2]));
+//
+//            if(purePursuit.robotFrame_targetDistance <= waypointZone)
+//                waypointReached_flag = 1;
+//            else
+//                waypointReached_flag = 0;
 
-            if(purePursuit.robotFrame_targetDistance <= waypointZone)
+        PID_B.findRobotFrameDistance(target_waypoint, localization.position, &robotFrame);   
+                  
+            if(robotFrame <= waypointZone)
                 waypointReached_flag = 1;
             else
-                waypointReached_flag = 0;
+                waypointReached_flag = 0; 
+
         }
         Thread::wait(imu_UpdatePeriodMS);
     }
@@ -550,7 +555,7 @@
 //        Debug.printf("Battery Status: %3.2f%%, %1.2fV\n\e[K", battery.getSOC(), battery.getVcell());
         Debug.printf("Waypoint Tracking: waypointReached %d, waypointSetFinish %d waypointIndex %d\n\e[K", waypointReached_flag, waypointSetFinish_flag, waypoint_index);
         Debug.printf("Waypoint Tracking: distanceToWaypoint %.1f, BearingToWaypoint %.4f \n\e[K", purePursuit.robotFrame_targetDistance, bearingToWaypoint);
-        Debug.printf("Waypoint Tracking: Rotate %.3f, Change in Speed %.3f, k3 %.3f \n\e[K", aError, speedChange, k3);
+        Debug.printf("Waypoint Tracking: Rotate %.3f, Change in Speed %.3f, k3 %.3f \n\e[K", Error, speedChange, k3);
         Debug.printf("Waypoint being tracked (X,Y): %.2f, %.2f\n\e[K", target_waypoint[0], target_waypoint[1]);
 //        Debug.printf("Ranging: %u, %u, %u, %u, %u \n\e[K", RangeSensor.exleft, RangeSensor.left, RangeSensor.front, RangeSensor.right, RangeSensor.exright);