commit 1b3adcdd278abb4e6daba0478081040b9fbbcb86
Author: galberding <galberding@techfak.uni-bielefeld.de>
Date:   Thu Oct 10 13:35:37 2019 +0200

    Implement differnet line following strategies

diff --git a/devices/DiWheelDrive/userthread.cpp b/devices/DiWheelDrive/userthread.cpp
index 4339c53..5be3fb8 100644
--- a/devices/DiWheelDrive/userthread.cpp
+++ b/devices/DiWheelDrive/userthread.cpp
@@ -42,11 +42,11 @@ states policy[] = {
 // BLACK is the line itselfe
 // GREY is the boarder between the line and the surface
 // WHITE is the common surface
-enum colorMember : uint8_t {
-	BLACK=0,
-	GREY=1,
-	WHITE=2
-};
+// enum colorMember : uint8_t {
+// 	BLACK=0,
+// 	GREY=1,
+// 	WHITE=2
+// };
 
 // a buffer for the z-value of the accelerometer
 int16_t accel_z;
@@ -57,9 +57,9 @@ const int sizeOfPolicy = sizeof(policy) / sizeof(states);
 int policyCounter = 0; // Do not change this, it points to the beginning of the policy
 
 // Different speed settings (all values in "rounds per minute")
-const int rpmTurnLeft[2] = {-10, 10};
-const int rpmTurnRight[2] = {rpmTurnLeft[1],rpmTurnLeft[0]};
-const int rpmHalt[2] = {0, 0};
+// const int rpmTurnLeft[2] = {-10, 10};
+// const int rpmTurnRight[2] = {rpmTurnLeft[1],rpmTurnLeft[0]};
+// const int rpmHalt[2] = {0, 0};
 
 // Definition of the fuzzyfication function
 //  | Membership
@@ -71,13 +71,13 @@ const int rpmHalt[2] = {0, 0};
 // All values are "raw sensor values"
 /* Use these values for white ground surface (e.g. paper) */
 
-const int blackStartFalling = 0x1000; // Where the black curve starts falling
-const int blackOff = 0x1800; // Where no more black is detected
-const int whiteStartRising = 0x2800; // Where the white curve starts rising
-const int whiteOn = 0x6000; // Where the white curve has reached the maximum value
-const int greyMax = (whiteOn + blackStartFalling) / 2; // Where grey has its maximum
-const int greyStartRising = blackStartFalling; // Where grey starts rising
-const int greyOff = whiteOn; // Where grey is completely off again
+// const int blackStartFalling = 0x1000; // Where the black curve starts falling
+// const int blackOff = 0x1800; // Where no more black is detected
+// const int whiteStartRising = 0x2800; // Where the white curve starts rising
+// const int whiteOn = 0x6000; // Where the white curve has reached the maximum value
+// const int greyMax = (whiteOn + blackStartFalling) / 2; // Where grey has its maximum
+// const int greyStartRising = blackStartFalling; // Where grey starts rising
+// const int greyOff = whiteOn; // Where grey is completely off again
 
 /* Use these values for gray ground surfaces */
 /*
@@ -111,252 +111,6 @@ void copyRpmSpeed(const int (&source)[2], int (&target)[2]) {
 	// chprintf((BaseSequentialStream*) &SD1, "Speed left: %d, Speed right: %d\r\n", target[0], target[1]);
 }
 
-// Fuzzyfication of the sensor values
-void fuzzyfication(int sensorValue, float (&fuzziedValue)[3]) {
-	if (sensorValue < blackStartFalling ) {
-		// Only black value
-		fuzziedValue[BLACK] = 1.0f;
-		fuzziedValue[GREY] = 0.0f;
-		fuzziedValue[WHITE] = 0.0f;
-	} else if (sensorValue > whiteOn ) {
-		// Only white value
-		fuzziedValue[BLACK] = 0.0f;
-		fuzziedValue[GREY] = 0.0f;
-		fuzziedValue[WHITE] = 1.0f;
-	} else if ( sensorValue < greyMax) {
-		// Some greyisch value between black and grey
-
-		// Black is going down
-		if ( sensorValue > blackOff) {
-			fuzziedValue[BLACK] = 0.0f;
-		} else {
-			fuzziedValue[BLACK] = static_cast<float>(sensorValue-blackOff) / (blackStartFalling-blackOff);
-		}
-
-		// Grey is going up
-		if ( sensorValue < greyStartRising) {
-			fuzziedValue[GREY] = 0.0f;
-		} else {
-			fuzziedValue[GREY] = static_cast<float>(sensorValue-greyStartRising) / (greyMax-greyStartRising);
-		}
-
-		// White is absent
-		fuzziedValue[WHITE] = 0.0f;
-
-	} else if ( sensorValue >= greyMax) {
-		// Some greyisch value between grey white
-
-		// Black is absent
-		fuzziedValue[BLACK] = 0.0f;
-
-		// Grey is going down
-		if ( sensorValue < greyOff) {
-			fuzziedValue[GREY] = static_cast<float>(sensorValue-greyOff) / (greyMax-greyOff);
-		} else {
-			fuzziedValue[GREY] = 0.0f;
-		}
-
-		// White is going up
-		if ( sensorValue < whiteStartRising) {
-			fuzziedValue[WHITE] = 0.0f;
-		} else {
-			fuzziedValue[WHITE] = static_cast<float>(sensorValue-whiteStartRising) / (whiteOn-whiteStartRising);
-		}
-	}
-}
-
-// Return the color, which has the highest fuzzy value
-colorMember getMember(float (&fuzzyValue)[3]) {
-	colorMember member;
-
-	if (fuzzyValue[BLACK] > fuzzyValue[GREY])
-		if (fuzzyValue[BLACK] > fuzzyValue[WHITE])
-			member = BLACK;
-		else
-			member = WHITE;
-	else
-		if (fuzzyValue[GREY] > fuzzyValue[WHITE])
-			member = GREY;
-		else
-			member = WHITE;
-
-	return member;
-}
-
-// Get a crisp output for the steering commands
-void defuzzyfication(colorMember (&member)[4], int (&rpmFuzzyCtrl)[2]) {
-
-	// all sensors are equal
-	if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == member[constants::DiWheelDrive::PROX_FRONT_LEFT] &&
-	    member[constants::DiWheelDrive::PROX_FRONT_LEFT] == member[constants::DiWheelDrive::PROX_FRONT_RIGHT] &&
-	    member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == member[constants::DiWheelDrive::PROX_WHEEL_RIGHT]) {
-		// something is wrong -> stop
-		copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);
-	// both front sensor detect a line
-	} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK &&
-	    member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK) {
-		// straight
-		copyRpmSpeed(global.rpmForward, rpmFuzzyCtrl);
-	// exact one front sensor detects a line
-	} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK ||
-	           member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK) {
-		// soft correction
-		if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY) {
-			// soft right
-			copyRpmSpeed(global.rpmSoftRight, rpmFuzzyCtrl);
-		} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == WHITE) {
-			// hard right
-			copyRpmSpeed(global.rpmHardRight, rpmFuzzyCtrl);
-		} else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) {
-			// soft left
-			copyRpmSpeed(global.rpmSoftLeft, rpmFuzzyCtrl);
-		} else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE) {
-			// hard left
-			copyRpmSpeed(global.rpmHardLeft, rpmFuzzyCtrl);
-		}
-	// both wheel sensors detect a line
-	} else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == BLACK &&
-	           member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == BLACK) {
-		// something is wrong -> stop
-		copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);
-	// exactly one wheel sensor detects a line
-	} else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == BLACK ||
-	           member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == BLACK) {
-		if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == BLACK) {
-			// turn left
-			copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl);
-		} else if (member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == BLACK) {
-			// turn right
-			copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);
-		}
-	// both front sensors may detect a line
-	} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY &&
-	           member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) {
-		if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY) {
-			// turn left
-			copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl);
-		} else if (member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) {
-			// turn right
-			copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);
-		}
-	// exactly one front sensor may detect a line
-	} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY ||
-	           member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) {
-		if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY) {
-			// turn left
-			copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl);
-		} else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) {
-			// turn right
-			copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);
-		}
-	// both wheel sensors may detect a line
-	} else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY &&
-	           member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) {
-		// something is wrong -> stop
-		copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);
-	// exactly one wheel sensor may detect a line
-	} else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY ||
-	           member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) {
-		if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == GREY) {
-			// turn left
-			copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl);
-		} else if (member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] == GREY) {
-			// turn right
-			copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);
-		}
-	// no sensor detects anything 
-	} else {
-		// line is lost -> stop
-		copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);
-	}
-
-	return;
-}
-
-Color memberToLed(colorMember member) {
-	switch (member) {
-		case BLACK:
-			return Color(Color::GREEN);
-		case GREY:
-			return Color(Color::YELLOW);
-		case WHITE:
-			return Color(Color::RED);
-		default:
-			return Color(Color::WHITE);
-	}
-}
-
-//void lineFollowing_new(xyz) {}
-
-void defuzz(colorMember (&member)[4], int (&rpmFuzzyCtrl)[2]){
-	// all sensors are equal
-	// if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == member[constants::DiWheelDrive::PROX_FRONT_LEFT] &&
-	//     member[constants::DiWheelDrive::PROX_FRONT_LEFT] == member[constants::DiWheelDrive::PROX_FRONT_RIGHT] &&
-	//     member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == member[constants::DiWheelDrive::PROX_WHEEL_RIGHT]) {
-	// 	// something is wrong -> stop
-	// 	copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);
-	// // both front sensor detect a line
-	if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK &&
-	    (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY)) {
-		// straight
-		copyRpmSpeed(global.rpmForward, rpmFuzzyCtrl);
-	// Deviation to right
-	} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK
-		&& member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE){
-			copyRpmSpeed(global.rpmSoftLeft, rpmFuzzyCtrl);
-	// Deviation to left
-	}else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK
-		&& member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK){
-			copyRpmSpeed(global.rpmSoftRight, rpmFuzzyCtrl);
-	// Hard deviatio to right
-	}else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY
-		&& member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE){
-			copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl);
-	// Hard deviation to left
-	}else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY
-		&& member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK){
-			copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);
-	// stop if white
-	}else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == WHITE
-		&& member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE ){
-			copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);
-	}
-}
-
-// Line following by a fuzzy controler
-void lineFollowing(int (&proximity)[4], int (&rpmFuzzyCtrl)[2]) {
-	// FUZZYFICATION
-	// First we need to get the fuzzy value for our 3 values {BLACK, GREY, WHITE}
-	float leftWheelFuzzyMemberValues[3], leftFrontFuzzyMemberValues[3], rightFrontFuzzyMemberValues[3], rightWheelFuzzyMemberValues[3];
-	fuzzyfication(proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT], leftWheelFuzzyMemberValues);
-	fuzzyfication(proximity[constants::DiWheelDrive::PROX_FRONT_LEFT], leftFrontFuzzyMemberValues);
-	fuzzyfication(proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT], rightFrontFuzzyMemberValues);
-	fuzzyfication(proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT], rightWheelFuzzyMemberValues);
-
-	// INFERENCE RULE DEFINITION
-	// Get the member for each sensor
-	colorMember member[4];
-	member[constants::DiWheelDrive::PROX_WHEEL_LEFT] = getMember(leftWheelFuzzyMemberValues);
-	member[constants::DiWheelDrive::PROX_FRONT_LEFT] = getMember(leftFrontFuzzyMemberValues);
-	member[constants::DiWheelDrive::PROX_FRONT_RIGHT] = getMember(rightFrontFuzzyMemberValues);
-	member[constants::DiWheelDrive::PROX_WHEEL_RIGHT] = getMember(rightWheelFuzzyMemberValues);
-
-	// visualize sensors via LEDs
-	global.robot.setLightColor(constants::LightRing::LED_WNW, memberToLed(member[constants::DiWheelDrive::PROX_WHEEL_LEFT]));
-	global.robot.setLightColor(constants::LightRing::LED_NNW, memberToLed(member[constants::DiWheelDrive::PROX_FRONT_LEFT]));
-	global.robot.setLightColor(constants::LightRing::LED_NNE, memberToLed(member[constants::DiWheelDrive::PROX_FRONT_RIGHT]));
-	global.robot.setLightColor(constants::LightRing::LED_ENE, memberToLed(member[constants::DiWheelDrive::PROX_WHEEL_RIGHT]));
-
-	// chprintf((BaseSequentialStream*) &SD1, "Left: BLACK: %f, GREY: %f, WHITE: %f\r\n", leftWheelFuzzyMemberValues[BLACK], leftWheelFuzzyMemberValues[GREY], leftWheelFuzzyMemberValues[WHITE]);
-	// chprintf((BaseSequentialStream*) &SD1, "Right: BLACK: %f, GREY: %f, WHITE: %f\r\n", rightFuzzyMemberValues[BLACK], rightFuzzyMemberValues[GREY], rightFuzzyMemberValues[WHITE]);
-
-	// DEFUZZYFICATION
-	// defuzzyfication(member, rpmFuzzyCtrl);
-	defuzz(member, rpmFuzzyCtrl);
-}
-
-
-
 
 // Set the speed by the array
 void setRpmSpeed(const int (&rpmSpeed)[2]) {
@@ -452,7 +206,7 @@ UserThread::main()
                 vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight();
                 vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset();
             }
-			lf.stableFollow(vcnl4020Proximity, rpmFuzzyCtrl, &global);
+			// lf.stableFollow(vcnl4020Proximity, rpmFuzzyCtrl, &global);
             // chprintf((BaseSequentialStream*) &SD1, "0x%04X 0x%04X 0x%04X 0x%04X\n",
             //         vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT],
             //         vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT],
