Team Virgo v3 / Orion_newPCB_test_LV

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 Team Virgo v3

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?

UserRevisionLine numberNew 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 }