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
01_DriveTrain/pidBearing.cpp@30:44676e1b38f8, 2018-03-20 (annotated)
- Committer:
- ahmed_lv
- Date:
- Tue Mar 20 05:56:22 2018 +0000
- Revision:
- 30:44676e1b38f8
- Parent:
- 29:a6a812ee83ea
Editted Input Variables to PID
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ahmed_lv | 26:32eaf3c3ac2e | 1 | #include "pidBearing.h" |
ahmed_lv | 26:32eaf3c3ac2e | 2 | |
ahmed_lv | 26:32eaf3c3ac2e | 3 | pidBearing::pidBearing() |
ahmed_lv | 26:32eaf3c3ac2e | 4 | { |
ahmed_lv | 26:32eaf3c3ac2e | 5 | kp = 1; |
ahmed_lv | 26:32eaf3c3ac2e | 6 | kd = 50; |
ahmed_lv | 26:32eaf3c3ac2e | 7 | ki = 0.0; |
ahmed_lv | 26:32eaf3c3ac2e | 8 | } |
ahmed_lv | 26:32eaf3c3ac2e | 9 | |
ahmed_lv | 29:a6a812ee83ea | 10 | void pidBearing::setSpeedChange(float* wl, float* wr, float* speedChange, float* Error, |
ahmed_lv | 29:a6a812ee83ea | 11 | float target[2], |
ahmed_lv | 29:a6a812ee83ea | 12 | float local[2], |
ahmed_lv | 29:a6a812ee83ea | 13 | float yaw, |
ahmed_lv | 29:a6a812ee83ea | 14 | float* robotFrame, |
ahmed_lv | 29:a6a812ee83ea | 15 | float xx, float yy, |
ahmed_lv | 29:a6a812ee83ea | 16 | float* k3, |
ahmed_lv | 29:a6a812ee83ea | 17 | float lGain) |
ahmed_lv | 26:32eaf3c3ac2e | 18 | { |
ahmed_lv | 29:a6a812ee83ea | 19 | bearingToWaypoint = 90 - 180/3.142 * atan2((target[1]-local[1]),(target[0]-local[0])); //angle due X-axis of IMU |
ahmed_lv | 26:32eaf3c3ac2e | 20 | |
ahmed_lv | 26:32eaf3c3ac2e | 21 | if(bearingToWaypoint>=0.0) |
ahmed_lv | 26:32eaf3c3ac2e | 22 | imuToAchieve = 360 - bearingToWaypoint; //Since YAW decreases in the clockwise direction, if target is to the right IMU to achieve should be 360 minus the bearing to the waypoint |
ahmed_lv | 26:32eaf3c3ac2e | 23 | else |
ahmed_lv | 26:32eaf3c3ac2e | 24 | imuToAchieve = 0 - bearingToWaypoint; //Since YAW decreases in the anticlockwise direction, if target is to the left IMU to acieve should 0 minus (negative sign) bearing to the waypoint |
ahmed_lv | 26:32eaf3c3ac2e | 25 | |
ahmed_lv | 26:32eaf3c3ac2e | 26 | offset = yaw - imuToAchieve; //this is to calculate how far off the bot is from it's current IMU yaw to the one it needs to be. |
ahmed_lv | 26:32eaf3c3ac2e | 27 | |
ahmed_lv | 26:32eaf3c3ac2e | 28 | if (offset >=180) |
ahmed_lv | 26:32eaf3c3ac2e | 29 | angleToRotate = offset - 360; //this is to determine the shortest angle to rotate. |
ahmed_lv | 26:32eaf3c3ac2e | 30 | else |
ahmed_lv | 26:32eaf3c3ac2e | 31 | angleToRotate = offset - 0; |
ahmed_lv | 26:32eaf3c3ac2e | 32 | |
ahmed_lv | 26:32eaf3c3ac2e | 33 | if(generalFunctions::abs_f(angleToRotate)>180) |
ahmed_lv | 29:a6a812ee83ea | 34 | *Error = 360 - generalFunctions::abs_f(angleToRotate); |
ahmed_lv | 26:32eaf3c3ac2e | 35 | else |
ahmed_lv | 29:a6a812ee83ea | 36 | *Error = angleToRotate; |
ahmed_lv | 26:32eaf3c3ac2e | 37 | |
ahmed_lv | 29:a6a812ee83ea | 38 | sumError = sumError + *Error; |
ahmed_lv | 29:a6a812ee83ea | 39 | *speedChange = kd * (*Error - prevaError) + kp * (*Error) + ki * (sumError); |
ahmed_lv | 29:a6a812ee83ea | 40 | prevaError = *Error; |
ahmed_lv | 29:a6a812ee83ea | 41 | |
ahmed_lv | 29:a6a812ee83ea | 42 | *robotFrame= sqrt(pow((target[0] - local[0]),2) + pow((target[1] - local[1]),2)); |
ahmed_lv | 29:a6a812ee83ea | 43 | // robotFrame_targetDistance= sqrt(pow((target_waypoint[0] - current_position[0]),2) + pow((target_waypoint[1] - current_position[1]),2)); |
ahmed_lv | 26:32eaf3c3ac2e | 44 | |
ahmed_lv | 29:a6a812ee83ea | 45 | max_in = sqrt(pow((target[0] - xx *10),2) + pow((target[1] - yy*10),2)); |
ahmed_lv | 29:a6a812ee83ea | 46 | minDist = *robotFrame - generalFunctions::abs_f(*Error*8); |
ahmed_lv | 29:a6a812ee83ea | 47 | if (*robotFrame < 120) |
ahmed_lv | 29:a6a812ee83ea | 48 | *k3 = 0.0; |
ahmed_lv | 29:a6a812ee83ea | 49 | else |
ahmed_lv | 29:a6a812ee83ea | 50 | *k3= generalFunctions::abs_f(generalFunctions::map_f(generalFunctions::abs_f(minDist), 0, max_in, 0.0, lGain)); |
ahmed_lv | 30:44676e1b38f8 | 51 | *k3 = generalFunctions::constrain_f(*k3, 0, lGain); |
ahmed_lv | 26:32eaf3c3ac2e | 52 | |
ahmed_lv | 26:32eaf3c3ac2e | 53 | |
ahmed_lv | 26:32eaf3c3ac2e | 54 | *wl = -(10 + *speedChange)*(*k3); |
ahmed_lv | 26:32eaf3c3ac2e | 55 | *wr = -(10 - *speedChange)*(*k3); |
ahmed_lv | 29:a6a812ee83ea | 56 | } |
ahmed_lv | 29:a6a812ee83ea | 57 | |
ahmed_lv | 29:a6a812ee83ea | 58 | void pidBearing::findRobotFrameDistance(float target[2], float local[2], float* robotFrame) |
ahmed_lv | 29:a6a812ee83ea | 59 | { |
ahmed_lv | 29:a6a812ee83ea | 60 | *robotFrame= sqrt(pow((target[0] - local[0]),2) + pow((target[1] - local[1]),2)); |
ahmed_lv | 26:32eaf3c3ac2e | 61 | } |