commit c76baf23888c28a0190f2f4e21783e2d424da760
Author: Georg Alberding <galberding@techfak.uni-bielefeld.de>
Date:   Tue Sep 24 15:54:26 2019 +0200

    Create rudimentary line following with front proxi sensors

diff --git a/devices/DiWheelDrive/userthread.cpp b/devices/DiWheelDrive/userthread.cpp
index dbf8517..0663117 100644
--- a/devices/DiWheelDrive/userthread.cpp
+++ b/devices/DiWheelDrive/userthread.cpp
@@ -2,6 +2,8 @@
 
 #include "global.hpp"
 
+#include "linefollow2.hpp" 
+
 using namespace amiro;
 
 extern Global global;
@@ -55,11 +57,6 @@ 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 rpmForward[2] = {25,25};
-const int rpmSoftLeft[2] = {15,25};
-const int rpmHardLeft[2] = {10,25};
-const int rpmSoftRight[2] = {rpmSoftLeft[1],rpmSoftLeft[0]};
-const int rpmHardRight[2] = {rpmHardLeft[1],rpmHardLeft[0]};
 const int rpmTurnLeft[2] = {-10, 10};
 const int rpmTurnRight[2] = {rpmTurnLeft[1],rpmTurnLeft[0]};
 const int rpmHalt[2] = {0, 0};
@@ -111,6 +108,7 @@ colorMember discrimination(int value) {
 void copyRpmSpeed(const int (&source)[2], int (&target)[2]) {
 	target[constants::DiWheelDrive::LEFT_WHEEL] = source[constants::DiWheelDrive::LEFT_WHEEL];
 	target[constants::DiWheelDrive::RIGHT_WHEEL] = source[constants::DiWheelDrive::RIGHT_WHEEL];
+	chprintf((BaseSequentialStream*) &SD1, "Speed left: %d, Speed right: %d\r\n", target[0], target[1]);
 }
 
 // Fuzzyfication of the sensor values
@@ -198,23 +196,23 @@ void defuzzyfication(colorMember (&member)[4], int (&rpmFuzzyCtrl)[2]) {
 	} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == BLACK &&
 	    member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK) {
 		// straight
-		copyRpmSpeed(rpmForward, rpmFuzzyCtrl);
+		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(rpmSoftRight, rpmFuzzyCtrl);
+			copyRpmSpeed(global.rpmSoftRight, rpmFuzzyCtrl);
 		} else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == WHITE) {
 			// hard right
-			copyRpmSpeed(rpmHardRight, rpmFuzzyCtrl);
+			copyRpmSpeed(global.rpmHardRight, rpmFuzzyCtrl);
 		} else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == GREY) {
 			// soft left
-			copyRpmSpeed(rpmSoftLeft, rpmFuzzyCtrl);
+			copyRpmSpeed(global.rpmSoftLeft, rpmFuzzyCtrl);
 		} else if (member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE) {
 			// hard left
-			copyRpmSpeed(rpmHardLeft, rpmFuzzyCtrl);
+			copyRpmSpeed(global.rpmHardLeft, rpmFuzzyCtrl);
 		}
 	// both wheel sensors detect a line
 	} else if (member[constants::DiWheelDrive::PROX_WHEEL_LEFT] == BLACK &&
@@ -288,6 +286,43 @@ Color memberToLed(colorMember member) {
 	}
 }
 
+//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(global.rpmHardLeft, rpmFuzzyCtrl);
+	// Hard deviation to left
+	}else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY
+		&& member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK){
+			copyRpmSpeed(global.rpmHardRight, rpmFuzzyCtrl);
+	// Turn if white
+	}else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == WHITE
+		&& member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE ){
+			copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);
+	}
+}
+
 // Line following by a fuzzy controler
 void lineFollowing(int (&proximity)[4], int (&rpmFuzzyCtrl)[2]) {
 	// FUZZYFICATION
@@ -312,13 +347,17 @@ void lineFollowing(int (&proximity)[4], int (&rpmFuzzyCtrl)[2]) {
 	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", leftFuzzyMemberValues[BLACK], leftFuzzyMemberValues[GREY], leftFuzzyMemberValues[WHITE]);
-//	chprintf((BaseSequentialStream*) &SD1, "Right: BLACK: %f, GREY: %f, WHITE: %f\r\n", rightFuzzyMemberValues[BLACK], rightFuzzyMemberValues[GREY], rightFuzzyMemberValues[WHITE]);
+	// 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);
+	// defuzzyfication(member, rpmFuzzyCtrl);
+	defuzz(member, rpmFuzzyCtrl);
 }
 
+
+
+
 // Set the speed by the array
 void setRpmSpeed(const int (&rpmSpeed)[2]) {
 	global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);
@@ -355,7 +394,7 @@ UserThread::main()
 		global.robot.setLightColor(led, Color(Color::BLACK));
     }
     running = false;
-
+	LineFollow lf;
 	/*
 	 * LOOP
 	 */
@@ -369,7 +408,20 @@ UserThread::main()
         /*
          * evaluate the accelerometer
          */
-        if (accel_z < -900 /*-0.9g*/) {
+
+	//Features over can: (Line Following)
+	//SetKP
+	//SetKI
+	//SetKd ?
+	//SetSleepTime
+	//SeForwardSpeed? or SetMaxSpeed
+	//DriveOnLeftLine
+	//DriveOnRightLine
+	
+	//if accel_z<-900
+		//send can event (one time)
+		// Line following is started if amiro roteted
+        if (accel_z < -900 /*-0.9g*/) { //new: (can.allowLineFollowOnTurn && accel_z < 900) || can.startLineFollow optional feature
             if (running) {
                 // stop the robot
                 running = false;
@@ -400,13 +452,15 @@ UserThread::main()
                 vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight();
                 vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset();
             }
-
-//            chprintf((BaseSequentialStream*) &SD1, "0x%04X 0x%04X 0x%04X 0x%04X\n",
-//                     vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT],
-//                     vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT],
-//                     vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT],
-//                     vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT]);
-
+			lf.followLine(vcnl4020Proximity, &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],
+            //         vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT],
+            //         vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT]);
+	//if (configLineFollowing==2)
+	//   lineFollownew
+	//else
             lineFollowing(vcnl4020Proximity, rpmFuzzyCtrl);
             setRpmSpeed(rpmFuzzyCtrl);
         }
