amiro-os / devices / DiWheelDrive / linefollow2.cpp @ 5d138bca
History | View | Annotate | Download (12.626 KB)
| 1 | c76baf23 | Georg Alberding | #include "global.hpp" |
|---|---|---|---|
| 2 | #include "linefollow2.hpp" |
||
| 3 | 2330e415 | Georg Alberding | #include <cmath> |
| 4 | c76baf23 | Georg Alberding | |
| 5 | |||
| 6 | 22b85da1 | galberding | |
| 7 | 12463563 | galberding | LineFollow::LineFollow(Global *global){
|
| 8 | this->global = global;
|
||
| 9 | } |
||
| 10 | 1b3adcdd | galberding | LineFollow::LineFollow(Global *global, LineFollowStrategy strategy){
|
| 11 | this->global = global;
|
||
| 12 | this-> strategy = strategy;
|
||
| 13 | c76baf23 | Georg Alberding | } |
| 14 | 25388c2f | Georg Alberding | |
| 15 | 22b85da1 | galberding | /**
|
| 16 | * Calculate the error from front proxi sensors and fixed threshold values for those sensors.
|
||
| 17 | */
|
||
| 18 | int LineFollow::getError(){
|
||
| 19 | 1b3adcdd | galberding | // Get actual sensor data of both front sensors
|
| 20 | 88afb834 | galberding | int FL = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
|
| 21 | int FR = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
|
||
| 22 | int targetL = global->threshProxyL;
|
||
| 23 | int targetR = global->threshProxyR;
|
||
| 24 | 1b3adcdd | galberding | int error = 0; |
| 25 | switch (this->strategy) |
||
| 26 | {
|
||
| 27 | bfffb0bd | galberding | case LineFollowStrategy::EDGE_RIGHT: case LineFollowStrategy::DOCKING: |
| 28 | 1b3adcdd | galberding | error = -(FL -targetL + FR - targetR); |
| 29 | break;
|
||
| 30 | case LineFollowStrategy::EDGE_LEFT:
|
||
| 31 | error = (FL -targetL + FR - targetR); |
||
| 32 | break;
|
||
| 33 | case LineFollowStrategy::MIDDLE:
|
||
| 34 | // Assume that the smallest value means driving in the middle
|
||
| 35 | targetL = targetR = !(targetL<targetR)?targetR:targetL; |
||
| 36 | error = (FL -targetL + FR - targetR); |
||
| 37 | break;
|
||
| 38 | |||
| 39 | default:
|
||
| 40 | break;
|
||
| 41 | } |
||
| 42 | // Debugging stuff ------
|
||
| 43 | if (global->enableRecord){
|
||
| 44 | global->senseRec[global->sensSamples].error = error; |
||
| 45 | global->senseRec[global->sensSamples].FL = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset(); |
||
| 46 | global->senseRec[global->sensSamples].FR = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset(); |
||
| 47 | global->sensSamples++; |
||
| 48 | } |
||
| 49 | // ----------------------
|
||
| 50 | // Register white values
|
||
| 51 | 22b85da1 | galberding | if (FL+FR > global->threshWhite){
|
| 52 | whiteFlag = 1;
|
||
| 53 | }else{
|
||
| 54 | whiteFlag = 0;
|
||
| 55 | } |
||
| 56 | return error;
|
||
| 57 | } |
||
| 58 | |||
| 59 | 1b3adcdd | galberding | int LineFollow::followLine(int (&rpmSpeed)[2]){ |
| 60 | bfffb0bd | galberding | int correctionSpeed = 0; |
| 61 | 1b3adcdd | galberding | switch (this->strategy) |
| 62 | {
|
||
| 63 | case LineFollowStrategy::FUZZY:
|
||
| 64 | bfffb0bd | galberding | for (int i = 0; i < 4; i++) { |
| 65 | vcnl4020AmbientLight[i] = global->vcnl4020[i].getAmbientLight(); |
||
| 66 | vcnl4020Proximity[i] = global->vcnl4020[i].getProximityScaledWoOffset(); |
||
| 67 | } |
||
| 68 | lineFollowing(vcnl4020Proximity, rpmSpeed); |
||
| 69 | break;
|
||
| 70 | |||
| 71 | case LineFollowStrategy::DOCKING:
|
||
| 72 | correctionSpeed = -getPidCorrectionSpeed(); |
||
| 73 | rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] = -global->forwardSpeed; |
||
| 74 | |||
| 75 | rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = -global->forwardSpeed; |
||
| 76 | |||
| 77 | break;
|
||
| 78 | 22b85da1 | galberding | |
| 79 | 1b3adcdd | galberding | default:
|
| 80 | bfffb0bd | galberding | correctionSpeed = getPidCorrectionSpeed(); |
| 81 | // chprintf((BaseSequentialStream*) &SD1, "Correction: %d, thresh: %d\n",correctionSpeed, global->threshWhite);
|
||
| 82 | 22b85da1 | galberding | |
| 83 | bfffb0bd | galberding | rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] = global->forwardSpeed + correctionSpeed; |
| 84 | 22b85da1 | galberding | |
| 85 | bfffb0bd | galberding | rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = global->forwardSpeed - correctionSpeed; |
| 86 | break;
|
||
| 87 | 1b3adcdd | galberding | } |
| 88 | bfffb0bd | galberding | return whiteFlag;
|
| 89 | 22b85da1 | galberding | } |
| 90 | |||
| 91 | 1b3adcdd | galberding | |
| 92 | 22b85da1 | galberding | /**
|
| 93 | * Pid controller which returns a corrections speed.
|
||
| 94 | */
|
||
| 95 | int LineFollow::getPidCorrectionSpeed(){
|
||
| 96 | int error = getError();
|
||
| 97 | 1b3adcdd | galberding | int sloap = error - oldError;
|
| 98 | int correctionSpeed = (int) (Kp*error + Ki*accumHist + Kd*sloap); |
||
| 99 | oldError = error; |
||
| 100 | // accumHist += (int) (0.01 * error);
|
||
| 101 | 22b85da1 | galberding | if (abs(error) > global->maxDist.error){
|
| 102 | global->maxDist.error = error; |
||
| 103 | } |
||
| 104 | return correctionSpeed;
|
||
| 105 | 88afb834 | galberding | } |
| 106 | 12463563 | galberding | |
| 107 | 1b3adcdd | galberding | |
| 108 | void LineFollow::setStrategy(LineFollowStrategy strategy){
|
||
| 109 | this->strategy = strategy;
|
||
| 110 | } |
||
| 111 | |||
| 112 | LineFollowStrategy LineFollow::getStrategy(){
|
||
| 113 | return this->strategy; |
||
| 114 | } |
||
| 115 | void LineFollow::setGains(float Kp, float Ki, float Kd){ |
||
| 116 | this->Kp = Kp;
|
||
| 117 | this->Ki = Ki;
|
||
| 118 | this->Kd = Kd;
|
||
| 119 | } |
||
| 120 | |||
| 121 | |||
| 122 | |||
| 123 | |||
| 124 | |||
| 125 | // Lagacy code, fuzzy following-----------------------------------------
|
||
| 126 | // Line following by a fuzzy controler
|
||
| 127 | void LineFollow::lineFollowing(int (&proximity)[4], int (&rpmFuzzyCtrl)[2]) { |
||
| 128 | // FUZZYFICATION
|
||
| 129 | // First we need to get the fuzzy value for our 3 values {BLACK, GREY, WHITE}
|
||
| 130 | float leftWheelFuzzyMemberValues[3], leftFrontFuzzyMemberValues[3], rightFrontFuzzyMemberValues[3], rightWheelFuzzyMemberValues[3]; |
||
| 131 | fuzzyfication(proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT], leftWheelFuzzyMemberValues); |
||
| 132 | fuzzyfication(proximity[constants::DiWheelDrive::PROX_FRONT_LEFT], leftFrontFuzzyMemberValues); |
||
| 133 | fuzzyfication(proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT], rightFrontFuzzyMemberValues); |
||
| 134 | fuzzyfication(proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT], rightWheelFuzzyMemberValues); |
||
| 135 | |||
| 136 | // INFERENCE RULE DEFINITION
|
||
| 137 | // Get the member for each sensor
|
||
| 138 | colorMember member[4];
|
||
| 139 | member[constants::DiWheelDrive::PROX_WHEEL_LEFT] = getMember(leftWheelFuzzyMemberValues); |
||
| 140 | member[constants::DiWheelDrive::PROX_FRONT_LEFT] = getMember(leftFrontFuzzyMemberValues); |
||
| 141 | member[constants::DiWheelDrive::PROX_FRONT_RIGHT] = getMember(rightFrontFuzzyMemberValues); |
||
| 142 | member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] = getMember(rightWheelFuzzyMemberValues); |
||
| 143 | |||
| 144 | // visualize sensors via LEDs
|
||
| 145 | global->robot.setLightColor(constants::LightRing::LED_WNW, memberToLed(member[constants::DiWheelDrive::PROX_WHEEL_LEFT])); |
||
| 146 | global->robot.setLightColor(constants::LightRing::LED_NNW, memberToLed(member[constants::DiWheelDrive::PROX_FRONT_LEFT])); |
||
| 147 | global->robot.setLightColor(constants::LightRing::LED_NNE, memberToLed(member[constants::DiWheelDrive::PROX_FRONT_RIGHT])); |
||
| 148 | global->robot.setLightColor(constants::LightRing::LED_ENE, memberToLed(member[constants::DiWheelDrive::PROX_WHEEL_RIGHT])); |
||
| 149 | |||
| 150 | // chprintf((BaseSequentialStream*) &SD1, "Left: BLACK: %f, GREY: %f, WHITE: %f\r\n", leftWheelFuzzyMemberValues[BLACK], leftWheelFuzzyMemberValues[GREY], leftWheelFuzzyMemberValues[WHITE]);
|
||
| 151 | // chprintf((BaseSequentialStream*) &SD1, "Right: BLACK: %f, GREY: %f, WHITE: %f\r\n", rightFuzzyMemberValues[BLACK], rightFuzzyMemberValues[GREY], rightFuzzyMemberValues[WHITE]);
|
||
| 152 | |||
| 153 | // DEFUZZYFICATION
|
||
| 154 | defuzzyfication(member, rpmFuzzyCtrl); |
||
| 155 | // defuzz(member, rpmFuzzyCtrl);
|
||
| 156 | } |
||
| 157 | |||
| 158 | |||
| 159 | Color LineFollow::memberToLed(colorMember member) {
|
||
| 160 | switch (member) {
|
||
| 161 | case BLACK:
|
||
| 162 | return Color(Color::GREEN);
|
||
| 163 | case GREY:
|
||
| 164 | return Color(Color::YELLOW);
|
||
| 165 | case WHITE:
|
||
| 166 | return Color(Color::RED);
|
||
| 167 | default:
|
||
| 168 | return Color(Color::WHITE);
|
||
| 169 | } |
||
| 170 | } |
||
| 171 | |||
| 172 | void LineFollow::defuzzyfication(colorMember (&member)[4], int (&rpmFuzzyCtrl)[2]) { |
||
| 173 | whiteFlag = 0;
|
||
| 174 | // all sensors are equal
|
||
| 175 | if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == member[constants::DiWheelDrive::PROX_FRONT_LEFT] &&
|
||
| 176 | member[constants::DiWheelDrive::PROX_FRONT_LEFT] == member[constants::DiWheelDrive::PROX_FRONT_RIGHT] && |
||
| 177 | member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == member[constants::DiWheelDrive::PROX_WHEEL_RIGHT]) {
|
||
| 178 | // something is wrong -> stop
|
||
| 179 | copyRpmSpeed(rpmHalt, rpmFuzzyCtrl); |
||
| 180 | // both front sensor detect a line
|
||
| 181 | } else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK && |
||
| 182 | member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK) {
|
||
| 183 | // straight
|
||
| 184 | copyRpmSpeed(global->rpmForward, rpmFuzzyCtrl); |
||
| 185 | // exact one front sensor detects a line
|
||
| 186 | } else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK || |
||
| 187 | member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK) {
|
||
| 188 | // soft correction
|
||
| 189 | if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY) {
|
||
| 190 | // soft right
|
||
| 191 | copyRpmSpeed(global->rpmSoftRight, rpmFuzzyCtrl); |
||
| 192 | } else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == WHITE) { |
||
| 193 | // hard right
|
||
| 194 | copyRpmSpeed(global->rpmHardRight, rpmFuzzyCtrl); |
||
| 195 | } else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) { |
||
| 196 | // soft left
|
||
| 197 | copyRpmSpeed(global->rpmSoftLeft, rpmFuzzyCtrl); |
||
| 198 | } else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE) { |
||
| 199 | // hard left
|
||
| 200 | copyRpmSpeed(global->rpmHardLeft, rpmFuzzyCtrl); |
||
| 201 | } |
||
| 202 | // both wheel sensors detect a line
|
||
| 203 | } else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == BLACK && |
||
| 204 | member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == BLACK) {
|
||
| 205 | // something is wrong -> stop
|
||
| 206 | copyRpmSpeed(rpmHalt, rpmFuzzyCtrl); |
||
| 207 | // exactly one wheel sensor detects a line
|
||
| 208 | } else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == BLACK || |
||
| 209 | member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == BLACK) {
|
||
| 210 | if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == BLACK) {
|
||
| 211 | // turn left
|
||
| 212 | copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl); |
||
| 213 | } else if (member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == BLACK) { |
||
| 214 | // turn right
|
||
| 215 | copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl); |
||
| 216 | } |
||
| 217 | // both front sensors may detect a line
|
||
| 218 | } else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY && |
||
| 219 | member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) {
|
||
| 220 | if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY) {
|
||
| 221 | // turn left
|
||
| 222 | copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl); |
||
| 223 | } else if (member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) { |
||
| 224 | // turn right
|
||
| 225 | copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl); |
||
| 226 | } |
||
| 227 | // exactly one front sensor may detect a line
|
||
| 228 | } else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY || |
||
| 229 | member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) {
|
||
| 230 | if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY) {
|
||
| 231 | // turn left
|
||
| 232 | copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl); |
||
| 233 | } else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) { |
||
| 234 | // turn right
|
||
| 235 | copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl); |
||
| 236 | } |
||
| 237 | // both wheel sensors may detect a line
|
||
| 238 | } else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY && |
||
| 239 | member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) {
|
||
| 240 | // something is wrong -> stop
|
||
| 241 | copyRpmSpeed(rpmHalt, rpmFuzzyCtrl); |
||
| 242 | // exactly one wheel sensor may detect a line
|
||
| 243 | } else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY || |
||
| 244 | member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) {
|
||
| 245 | if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY) {
|
||
| 246 | // turn left
|
||
| 247 | copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl); |
||
| 248 | } else if (member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) { |
||
| 249 | // turn right
|
||
| 250 | copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl); |
||
| 251 | } |
||
| 252 | // no sensor detects anything
|
||
| 253 | } else {
|
||
| 254 | // line is lost -> stop
|
||
| 255 | whiteFlag = 1;
|
||
| 256 | copyRpmSpeed(rpmHalt, rpmFuzzyCtrl); |
||
| 257 | } |
||
| 258 | chprintf((BaseSequentialStream*) &SD1, "Fuzzy Speed: Left: %d, Right: %d\n", rpmFuzzyCtrl[0], rpmFuzzyCtrl[1]); |
||
| 259 | return;
|
||
| 260 | } |
||
| 261 | |||
| 262 | colorMember LineFollow::getMember(float (&fuzzyValue)[3]) { |
||
| 263 | colorMember member; |
||
| 264 | |||
| 265 | if (fuzzyValue[BLACK] > fuzzyValue[GREY])
|
||
| 266 | if (fuzzyValue[BLACK] > fuzzyValue[WHITE])
|
||
| 267 | member = BLACK; |
||
| 268 | else
|
||
| 269 | member = WHITE; |
||
| 270 | else
|
||
| 271 | if (fuzzyValue[GREY] > fuzzyValue[WHITE])
|
||
| 272 | member = GREY; |
||
| 273 | else
|
||
| 274 | member = WHITE; |
||
| 275 | |||
| 276 | return member;
|
||
| 277 | } |
||
| 278 | |||
| 279 | // Fuzzyfication of the sensor values
|
||
| 280 | void LineFollow::fuzzyfication(int sensorValue, float (&fuzziedValue)[3]) { |
||
| 281 | if (sensorValue < blackStartFalling ) {
|
||
| 282 | // Only black value
|
||
| 283 | fuzziedValue[BLACK] = 1.0f; |
||
| 284 | fuzziedValue[GREY] = 0.0f; |
||
| 285 | fuzziedValue[WHITE] = 0.0f; |
||
| 286 | } else if (sensorValue > whiteOn ) { |
||
| 287 | // Only white value
|
||
| 288 | fuzziedValue[BLACK] = 0.0f; |
||
| 289 | fuzziedValue[GREY] = 0.0f; |
||
| 290 | fuzziedValue[WHITE] = 1.0f; |
||
| 291 | } else if ( sensorValue < greyMax) { |
||
| 292 | // Some greyisch value between black and grey
|
||
| 293 | |||
| 294 | // Black is going down
|
||
| 295 | if ( sensorValue > blackOff) {
|
||
| 296 | fuzziedValue[BLACK] = 0.0f; |
||
| 297 | } else {
|
||
| 298 | fuzziedValue[BLACK] = static_cast<float>(sensorValue-blackOff) / (blackStartFalling-blackOff); |
||
| 299 | } |
||
| 300 | |||
| 301 | // Grey is going up
|
||
| 302 | if ( sensorValue < greyStartRising) {
|
||
| 303 | fuzziedValue[GREY] = 0.0f; |
||
| 304 | } else {
|
||
| 305 | fuzziedValue[GREY] = static_cast<float>(sensorValue-greyStartRising) / (greyMax-greyStartRising); |
||
| 306 | } |
||
| 307 | |||
| 308 | // White is absent
|
||
| 309 | fuzziedValue[WHITE] = 0.0f; |
||
| 310 | |||
| 311 | } else if ( sensorValue >= greyMax) { |
||
| 312 | // Some greyisch value between grey white
|
||
| 313 | |||
| 314 | // Black is absent
|
||
| 315 | fuzziedValue[BLACK] = 0.0f; |
||
| 316 | |||
| 317 | // Grey is going down
|
||
| 318 | if ( sensorValue < greyOff) {
|
||
| 319 | fuzziedValue[GREY] = static_cast<float>(sensorValue-greyOff) / (greyMax-greyOff); |
||
| 320 | } else {
|
||
| 321 | fuzziedValue[GREY] = 0.0f; |
||
| 322 | } |
||
| 323 | |||
| 324 | // White is going up
|
||
| 325 | if ( sensorValue < whiteStartRising) {
|
||
| 326 | fuzziedValue[WHITE] = 0.0f; |
||
| 327 | } else {
|
||
| 328 | fuzziedValue[WHITE] = static_cast<float>(sensorValue-whiteStartRising) / (whiteOn-whiteStartRising); |
||
| 329 | } |
||
| 330 | } |
||
| 331 | } |
||
| 332 | |||
| 333 | void LineFollow::copyRpmSpeed(const int (&source)[2], int (&target)[2]) { |
||
| 334 | target[constants::DiWheelDrive::LEFT_WHEEL] = source[constants::DiWheelDrive::LEFT_WHEEL]; |
||
| 335 | target[constants::DiWheelDrive::RIGHT_WHEEL] = source[constants::DiWheelDrive::RIGHT_WHEEL]; |
||
| 336 | // chprintf((BaseSequentialStream*) &SD1, "Speed left: %d, Speed right: %d\r\n", target[0], target[1]);
|
||
| 337 | } |