Implement new controller
Dependencies: mbed-rtos mbed QEI BNO055 MPU6050_DMP_Nucleo-I2Cdev virgo3_imuHandler_Orion_PCB MAX17048 Servo
Fork of Orion_newPCB_test by
Diff: orion_main.cpp
- Revision:
- 29:a6a812ee83ea
- Parent:
- 28:39d694b0e998
- Child:
- 30:44676e1b38f8
diff -r 39d694b0e998 -r a6a812ee83ea orion_main.cpp --- 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);