Revision 22b85da1
| devices/DiWheelDrive/global.hpp | ||
|---|---|---|
| 169 | 169 |
int rpmHardLeft[2] = {5,20};
|
| 170 | 170 |
int rpmSoftRight[2] = {rpmSoftLeft[1],rpmSoftLeft[0]};
|
| 171 | 171 |
int rpmHardRight[2] = {rpmHardLeft[1],rpmHardLeft[0]};
|
| 172 |
|
|
| 172 | 173 |
|
| 173 | 174 |
// Line Following thresholds set due to calibration |
| 174 | 175 |
// MaxDelta: 18676, FL: 4289, FR: 22965 |
| 175 |
int threshProxyL = 2168; |
|
| 176 |
int threshProxyR = 24406; |
|
| 176 |
// Thresh FL: 5241, FR: 25528 |
|
| 177 |
int threshProxyL = 5241; |
|
| 178 |
int threshProxyR = 25528; |
|
| 179 |
int threshWhite = 78000; |
|
| 180 |
|
|
| 181 |
// PID for line following: |
|
| 182 |
float K_p = 0.0f; |
|
| 183 |
float K_i = 0.0f; |
|
| 184 |
float K_d = 0.0f; |
|
| 185 |
|
|
| 186 |
// Integral part |
|
| 187 |
int accumHist = 0; |
|
| 188 |
int oldError = 0; |
|
| 189 |
|
|
| 190 |
// Debugging stuff -------------- |
|
| 191 |
int forwardSpeed = 10; |
|
| 192 |
int enableRecord = 0; |
|
| 177 | 193 |
|
| 178 | 194 |
// Buffer for sensor values |
| 179 | 195 |
struct sensorRecord |
| ... | ... | |
| 186 | 202 |
|
| 187 | 203 |
int sensSamples = 0; |
| 188 | 204 |
sensorRecord senseRec[1000]; |
| 205 |
sensorRecord maxDist; |
|
| 189 | 206 |
|
| 190 |
|
|
| 207 |
// ----------------------------- |
|
| 191 | 208 |
public: |
| 192 | 209 |
Global() : |
| 193 | 210 |
HW_I2C1(&I2CD1), HW_I2C2(&I2CD2), |
| devices/DiWheelDrive/linefollow2.cpp | ||
|---|---|---|
| 2 | 2 |
#include "linefollow2.hpp" |
| 3 | 3 |
#include <cmath> |
| 4 | 4 |
|
| 5 |
|
|
| 5 |
// Trash |
|
| 6 | 6 |
void LineFollow::printSensorData(){
|
| 7 | 7 |
chprintf((BaseSequentialStream*) &SD1, "Test!"); |
| 8 | 8 |
} |
| 9 | 9 |
|
| 10 |
|
|
| 10 | 11 |
LineFollow::LineFollow(Global *global){
|
| 11 | 12 |
this->global = global; |
| 12 | 13 |
} |
| 13 | 14 |
|
| 15 |
// trash |
|
| 14 | 16 |
int LineFollow::delta(){
|
| 15 | 17 |
int delta = 0; |
| 16 | 18 |
int FL = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset(); |
| ... | ... | |
| 26 | 28 |
return delta; |
| 27 | 29 |
} |
| 28 | 30 |
|
| 29 |
|
|
| 31 |
// old and trash |
|
| 30 | 32 |
void LineFollow::stableFollow(int vcnl4020Proximity[4], int (&rpmFuzzyCtrl)[2], Global *global){
|
| 31 | 33 |
int targetSensor = 0x38; |
| 32 | 34 |
int actualSensorL = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] ; |
| ... | ... | |
| 53 | 55 |
|
| 54 | 56 |
} |
| 55 | 57 |
|
| 56 |
int error(){
|
|
| 58 |
/** |
|
| 59 |
* Calculate the error from front proxi sensors and fixed threshold values for those sensors. |
|
| 60 |
*/ |
|
| 61 |
int LineFollow::getError(){
|
|
| 62 |
|
|
| 57 | 63 |
int FL = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset(); |
| 58 | 64 |
int FR = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset(); |
| 59 | 65 |
int targetL = global->threshProxyL; |
| 60 | 66 |
int targetR = global->threshProxyR; |
| 61 |
return FL -targetL + FR - targetR; |
|
| 67 |
int error = FL -targetL + FR - targetR; |
|
| 68 |
|
|
| 69 |
if (FL+FR > global->threshWhite){
|
|
| 70 |
whiteFlag = 1; |
|
| 71 |
}else{
|
|
| 72 |
whiteFlag = 0; |
|
| 73 |
} |
|
| 74 |
return error; |
|
| 75 |
} |
|
| 76 |
|
|
| 77 |
/** |
|
| 78 |
* Follow strategy for left edge. |
|
| 79 |
*/ |
|
| 80 |
int LineFollow::followLeftEdge(int rpmSpeed[2]){
|
|
| 81 |
|
|
| 82 |
int correctionSpeed = getPidCorrectionSpeed(); |
|
| 83 |
chprintf((BaseSequentialStream*) &SD1, "Correction: %d, thresh: %d\n",correctionSpeed, global->threshWhite); |
|
| 84 |
|
|
| 85 |
rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] = global->forwardSpeed + correctionSpeed; |
|
| 86 |
|
|
| 87 |
rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = global->forwardSpeed - correctionSpeed; |
|
| 88 |
return whiteFlag; |
|
| 89 |
} |
|
| 90 |
|
|
| 91 |
/** |
|
| 92 |
* Follow strategy for right edge. |
|
| 93 |
*/ |
|
| 94 |
int LineFollow::followRightEdge(int rpmSpeed[2]){
|
|
| 95 |
|
|
| 96 |
int correctionSpeed = getPidCorrectionSpeed(); |
|
| 97 |
chprintf((BaseSequentialStream*) &SD1, "Correction: %d, thresh: %d\n",correctionSpeed, global->threshWhite); |
|
| 98 |
|
|
| 99 |
rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] = global->forwardSpeed - correctionSpeed; |
|
| 100 |
|
|
| 101 |
rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = global->forwardSpeed + correctionSpeed; |
|
| 102 |
return whiteFlag; |
|
| 103 |
} |
|
| 104 |
|
|
| 105 |
/** |
|
| 106 |
* Pid controller which returns a corrections speed. |
|
| 107 |
*/ |
|
| 108 |
int LineFollow::getPidCorrectionSpeed(){
|
|
| 109 |
int error = getError(); |
|
| 110 |
int sloap = error - global->oldError; |
|
| 111 |
int correctionSpeed = (int) (global->K_p*error + global->K_i*global->accumHist + global->K_d*sloap); |
|
| 112 |
global->oldError = error; |
|
| 113 |
global->accumHist += error; |
|
| 114 |
if (abs(error) > global->maxDist.error){
|
|
| 115 |
global->maxDist.error = error; |
|
| 116 |
} |
|
| 117 |
return correctionSpeed; |
|
| 62 | 118 |
} |
| 63 | 119 |
|
| 64 |
void LineFollow::calibrateZiegler(float KCrit, int rpmSpeed[2]){
|
|
| 65 |
int targetSpeedL = 5; |
|
| 66 |
int targetSpeedR = 5; |
|
| 67 |
int delta_ = error(); |
|
| 68 |
int correctionSpeed = (int) (KCrit * delta_); |
|
| 69 |
// global->senseRec[global->sensSamples].FL = FL; |
|
| 70 |
// global->senseRec[global->sensSamples].FR = FR; |
|
| 71 |
global->senseRec[global->sensSamples].error = delta_; |
|
| 72 |
global->senseRec[global->sensSamples].FL = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();; |
|
| 73 |
global->senseRec[global->sensSamples].FR = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();; |
|
| 74 |
global->sensSamples++ |
|
| 75 |
|
|
| 76 |
|
|
| 77 |
rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] = -1*correctionSpeed; |
|
| 78 |
rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = correctionSpeed; |
|
| 79 |
// chprintf((BaseSequentialStream*) &SD1, "CS:%d,LW:%d,RW:%d\n", correctionSpeed, rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL], rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL]); |
|
| 80 |
} |
|
| 120 |
// trash |
|
| 121 |
// void LineFollow::calibrateZiegler(float KCrit, int rpmSpeed[2]){
|
|
| 122 |
// int targetSpeedL = 5; |
|
| 123 |
// int targetSpeedR = 5; |
|
| 124 |
// int delta_ = error(); |
|
| 125 |
// int correctionSpeed = (int) (KCrit * delta_); |
|
| 126 |
// if (global->enableRecord){
|
|
| 127 |
// global->senseRec[global->sensSamples].error = delta_; |
|
| 128 |
// global->senseRec[global->sensSamples].FL = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset(); |
|
| 129 |
// global->senseRec[global->sensSamples].FR = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset(); |
|
| 130 |
// global->sensSamples++; |
|
| 131 |
// } |
|
| 132 |
// if (abs(delta_) > global->maxDist.error){
|
|
| 133 |
// global->maxDist.error = delta_; |
|
| 134 |
// } |
|
| 135 |
|
|
| 136 |
// rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] = global->forwardSpeed + -1*correctionSpeed; |
|
| 137 |
// rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = global->forwardSpeed + correctionSpeed; |
|
| 138 |
// chprintf((BaseSequentialStream*) &SD1, "CS:%d,LW:%d,RW:%d\n", correctionSpeed, rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL], rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL]); |
|
| 139 |
// } |
|
| devices/DiWheelDrive/linefollow2.hpp | ||
|---|---|---|
| 15 | 15 |
float SetPoint = 0x4000; // (0x1800+0x2800) >> 8 |
| 16 | 16 |
float Kp = 0.001; |
| 17 | 17 |
float Ki = 0.00001; |
| 18 |
float Kd = 0.5 |
|
| 19 |
; |
|
| 18 |
float Kd = 0.5; |
|
| 20 | 19 |
int accSum = 0; |
| 21 | 20 |
float oldError = 0; |
| 22 | 21 |
int biggestDiff = 0; |
| 23 | 22 |
Global *global; |
| 24 | 23 |
LineFollow(Global *global); |
| 25 |
void calibrateZiegler(float KCrit, int rpmSpeed[2]); |
|
| 24 |
// void calibrateZiegler(float KCrit, int rpmSpeed[2]); |
|
| 25 |
int followLeftEdge(int rpmSpeed[2]); |
|
| 26 |
int followRightEdge(int rpmSpeed[2]); |
|
| 26 | 27 |
|
| 27 | 28 |
private: |
| 28 | 29 |
int delta(); |
| 30 |
int getError(); |
|
| 31 |
int getPidCorrectionSpeed(); |
|
| 32 |
|
|
| 33 |
char whiteFlag = 0; |
|
| 34 |
|
|
| 29 | 35 |
|
| 30 | 36 |
}; |
| 31 | 37 |
|
| devices/DiWheelDrive/main.cpp | ||
|---|---|---|
| 708 | 708 |
int proxyL = 0; |
| 709 | 709 |
int proxyR = 0; |
| 710 | 710 |
int maxDelta = 0; |
| 711 |
int sensorL = 0; |
|
| 712 |
int sensorR = 0; |
|
| 711 | 713 |
|
| 712 | 714 |
if (argc == 1){
|
| 713 | 715 |
chprintf(chp, "Test %i rounds \n", atoi(argv[0])); |
| ... | ... | |
| 739 | 741 |
proxyL = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT]; |
| 740 | 742 |
proxyR = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]; |
| 741 | 743 |
} |
| 744 |
sensorL += global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset(); |
|
| 745 |
sensorR += global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset(); |
|
| 742 | 746 |
|
| 743 | 747 |
// if (vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT] > proxyR && vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] > proxyL ){
|
| 744 | 748 |
// delta *= -1; |
| ... | ... | |
| 754 | 758 |
// sleep(CAN::UPDATE_PERIOD); |
| 755 | 759 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
| 756 | 760 |
} |
| 757 |
chprintf(chp,"Sensors Calibrated: MaxDelta: %d, FL: %d, FR: %d\n", maxDelta, proxyL, proxyR); |
|
| 761 |
|
|
| 758 | 762 |
|
| 759 |
global.threshProxyL = proxyL; |
|
| 760 |
global.threshProxyR = proxyR; |
|
| 763 |
global.threshProxyL = sensorL / rounds; |
|
| 764 |
global.threshProxyR = sensorR / rounds; |
|
| 765 |
chprintf(chp,"Thresh FL: %d, FR: %d\n", global.threshProxyL, global.threshProxyR); |
|
| 761 | 766 |
return; |
| 762 | 767 |
} |
| 763 | 768 |
|
| ... | ... | |
| 770 | 775 |
uint16_t proxyL = global.threshProxyL; |
| 771 | 776 |
uint16_t proxyR = global.threshProxyR; |
| 772 | 777 |
uint16_t maxDelta = 0; |
| 773 |
|
|
| 778 |
|
|
| 779 |
int sensorL = 0; |
|
| 780 |
int sensorR = 0; |
|
| 774 | 781 |
if (argc == 1){
|
| 775 | 782 |
chprintf(chp, "Test %i rounds \n", atoi(argv[0])); |
| 776 | 783 |
rounds = atoi(argv[0]); |
| ... | ... | |
| 783 | 790 |
vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight(); |
| 784 | 791 |
vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset(); |
| 785 | 792 |
} |
| 786 |
|
|
| 793 |
|
|
| 787 | 794 |
uint16_t delta = (vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] |
| 788 | 795 |
- vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]); |
| 789 | 796 |
// // Update proximity thresh |
| ... | ... | |
| 810 | 817 |
return; |
| 811 | 818 |
} |
| 812 | 819 |
|
| 820 |
// Either 0 to disable record or > 0 to enable it |
|
| 821 |
void setRecord(BaseSequentialStream *chp, int argc, char *argv[]){
|
|
| 822 |
if (argc == 1){
|
|
| 823 |
chprintf(chp, "Set recording to %d\n", atoi(argv[0])); |
|
| 824 |
global.enableRecord = atoi(argv[0]); |
|
| 825 |
} |
|
| 826 |
} |
|
| 813 | 827 |
|
| 814 | 828 |
|
| 815 | 829 |
void zieglerMeth2(BaseSequentialStream *chp, int argc, char *argv[]) {
|
| ... | ... | |
| 822 | 836 |
int maxDelta = 0; |
| 823 | 837 |
float KCrit = 0.0f; |
| 824 | 838 |
global.sensSamples = 0; |
| 839 |
global.maxDist.error = 0; |
|
| 825 | 840 |
LineFollow lf(&global); |
| 841 |
int led = 0; |
|
| 826 | 842 |
|
| 827 | 843 |
if (argc == 2){
|
| 828 | 844 |
chprintf(chp, "KCrti %f\n", atof(argv[0])); |
| 829 | 845 |
chprintf(chp, "Steps %i\n", atoi(argv[1])); |
| 830 | 846 |
KCrit = atof(argv[0]); |
| 831 | 847 |
steps = atoi(argv[1]); |
| 832 |
} else{
|
|
| 833 |
chprintf(chp, "Usage: dev_ziegler2 <K_crit> <steps>"); |
|
| 848 |
} else if (argc == 3){
|
|
| 849 |
chprintf(chp, "KCrti %f\n", atof(argv[0])); |
|
| 850 |
chprintf(chp, "Steps %i\n", atoi(argv[1])); |
|
| 851 |
KCrit = atof(argv[0]); |
|
| 852 |
steps = atoi(argv[1]); |
|
| 853 |
global.forwardSpeed = atoi(argv[2]); |
|
| 854 |
|
|
| 855 |
}else{
|
|
| 856 |
chprintf(chp, "Usage: dev_ziegler2 <K_crit> <steps> (<speed>)"); |
|
| 834 | 857 |
return; |
| 835 | 858 |
} |
| 859 |
global.K_p = KCrit; |
|
| 860 |
for(led=0; led<8; led++){
|
|
| 861 |
global.robot.setLightColor(led, Color(Color::BLACK)); |
|
| 862 |
} |
|
| 836 | 863 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "Recodring starts in five seconds...\n"); |
| 837 | 864 |
BaseThread::sleep(MS2ST(5000)); |
| 838 | 865 |
// global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000); |
| 839 |
|
|
| 866 |
int checkWhite = 0; |
|
| 867 |
int it_switch = steps / 2; |
|
| 840 | 868 |
for(int s=0; s < steps; s++){
|
| 841 |
chprintf(chp,"S:%d,",s); |
|
| 842 |
lf.calibrateZiegler(KCrit, rpmSpeed); |
|
| 869 |
// chprintf(chp,"S:%d,",s); |
|
| 870 |
if(global.threshWhite) |
|
| 871 |
if(s < it_switch){
|
|
| 843 | 872 |
|
| 844 |
global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000); |
|
| 873 |
checkWhite = lf.followRightEdge(rpmSpeed); |
|
| 874 |
}else{
|
|
| 875 |
checkWhite = lf.followLeftEdge(rpmSpeed); |
|
| 876 |
} |
|
| 877 |
if(checkWhite){
|
|
| 878 |
for(led=0; led<8; led++){
|
|
| 879 |
global.robot.setLightColor(led, Color(Color::RED)); |
|
| 880 |
} |
|
| 881 |
global.motorcontrol.setTargetRPM(0,0); |
|
| 882 |
}else{
|
|
| 883 |
global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000); |
|
| 884 |
} |
|
| 885 |
|
|
| 886 |
|
|
| 845 | 887 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
| 846 | 888 |
} |
| 889 |
|
|
| 847 | 890 |
global.motorcontrol.setTargetRPM(0,0); |
| 848 | 891 |
} |
| 849 | 892 |
|
| ... | ... | |
| 884 | 927 |
|
| 885 | 928 |
global.senseRec[j].FL = FL; |
| 886 | 929 |
global.senseRec[j].FR = FR; |
| 887 |
global.senseRec[j].delta = delta; |
|
| 888 | 930 |
// chprintf(chp,"FL: 0x%x, FR: 0x%x, Delta: %d, ProxyL: %x, ProxyR: %x, MaxDelta: %d\n", |
| 889 | 931 |
// vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT], |
| 890 | 932 |
// vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT], |
| ... | ... | |
| 905 | 947 |
void printMove(BaseSequentialStream *chp, int argc, char *argv[]){
|
| 906 | 948 |
|
| 907 | 949 |
for (int j=0; j<global.sensSamples;j++){
|
| 908 |
chprintf(chp,"FL:%d,FR:%d,Delta:%d\n",global.senseRec[j].FL, global.senseRec[j].FR, global.senseRec[j].delta);
|
|
| 950 |
chprintf(chp,"FL:%d,FR:%d,Delta:%d,Error:%d\n",global.senseRec[j].FL, global.senseRec[j].FR, global.senseRec[j].delta, global.senseRec[j].error);
|
|
| 909 | 951 |
} |
| 952 |
chprintf(chp,"MaxDist: FL:%d,FR:%d,Delta:%d,Error:%d\n",global.maxDist.FL, global.maxDist.FR, global.maxDist.delta, global.maxDist.error); |
|
| 953 |
|
|
| 910 | 954 |
|
| 911 | 955 |
} |
| 912 | 956 |
|
| ... | ... | |
| 943 | 987 |
{"calibrate_line", calibrateLineSensores},
|
| 944 | 988 |
{"record_move_l", recordMove},
|
| 945 | 989 |
{"print_record", printMove},
|
| 990 |
{"setRecord", setRecord},
|
|
| 946 | 991 |
{NULL, NULL}
|
| 947 | 992 |
}; |
| 948 | 993 |
|
Also available in: Unified diff