Diff: orion_main.cpp

Revision:

26:32eaf3c3ac2e

Parent:

25:3a82f868c101

Child:

27:2db168d9fb18

--- a/orion_main.cpp	Tue Mar 13 12:32:04 2018 +0000
+++ b/orion_main.cpp	Thu Mar 15 09:14:43 2018 +0000
@@ -42,8 +42,22 @@
 //** Drivetrain **
 motorDriver drive;  //motor drive train
 odometer odometry;  //odometer function
+pidBearing PID_B; //PID control for bearing
 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]; 
+
+
+//**end of new control **
+
 Timer motorControl_t;
 float rpm_cmd[2]; //drive motor rpm command
 float rpm_compensated[2]; //rpm command compensated by acc limit
@@ -329,13 +343,17 @@
             ref_gamma = purePursuit_gamma;
             ref_dgamma = purePursuit_omega;
 
-            attitudeControl.GenWheelVelocities(&W_l, &W_r, 0, motorControl_t.read(),
-                                               pitch_th, pitch_om, yaw_th , yaw_om,
-                                               wheelTh_l, wheelTh_r, wheelOm_l, wheelOm_r,
-                                               0, ref_dbeta, ref_beta,
-                                               0, ref_dtheta, ref_theta,
-                                               0, ref_dgamma, ref_gamma,
-                                               &u1, &u2);
+//            attitudeControl.GenWheelVelocities(&W_l, &W_r, 0, motorControl_t.read(),
+//                                               pitch_th, pitch_om, yaw_th , yaw_om,
+//                                               wheelTh_l, wheelTh_r, wheelOm_l, wheelOm_r,
+//                                               0, ref_dbeta, ref_beta,
+//                                               0, ref_dtheta, ref_theta,
+//                                               0, ref_dgamma, ref_gamma,
+//                                               &u1, &u2);
+            
+            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 ); //new controller
 
 
 
@@ -374,7 +392,7 @@
 
             drive.setPWM_L(pwm_cmd[0]);
             drive.setPWM_R(pwm_cmd[1]);
-            
+                     
             
         }
 
@@ -462,15 +480,12 @@
         Debug.printf("Calib Status (M-A-G-S-O): (%d , %d , %d , %d , %d)\n\e[K", imu.calib_stat[0], imu.calib_stat[1], imu.calib_stat[2], imu.calib_stat[3], imu.calib_stat[4]);
 
         //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, purePursuit_headingE %.1f \n\e[K", purePursuit.robotFrame_targetDistance, RAD_TO_DEG(purePursuit.purePursuit_headingE));
+        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 being tracked (X,Y): %.2f, %.2f\n\e[K", target_waypoint[0], target_waypoint[1]);
 
-        Debug.printf("SMC: ref_beta %.2f, ref_dbeta %.3f\n\e[K", RAD_TO_DEG(ref_beta), RAD_TO_DEG(ref_dbeta));
-        Debug.printf("SMC: ref_gamma %.2f, ref_dgamma %.3f\n\e[K", RAD_TO_DEG(ref_gamma), RAD_TO_DEG(ref_dgamma));
-        Debug.printf("SMC: ref_theta %.2f, ref_dtheta %.3f\n\e[K", RAD_TO_DEG(ref_theta), RAD_TO_DEG(ref_dtheta));
-        Debug.printf("SMC: u1*tc %.2f rpm, u2*tc %.2f rpm\n\e[K", u1*0.005*60/(2*M_PI), u2*0.005*60/(2*M_PI));
+
 
         Debug.printf("Compensated RPM (L,R): %.1f, %.1f\n\e[K", rpm_compensated[0], rpm_compensated[1]);
         //Debug.printf("Computed PWM (L,R): %.1f, %.1f\n\e[K", pwm_cmd[0]*100.0, pwm_cmd[1]*100.0);