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