test2.cpp

#include<bits/stdc++.h>
using namespace std;

struct SWheels
{
	double leftV;
	double rightV;
} wheelSpeedGroup[9];

/// All Clockwise (negative for reverse direction)
enum rotationCenter {
	fullyStop,
	straitForward,
	middle,
	nLeft, nRight,
	fLeft, fRight,
	sLeft, sRight
};

enum specialState {
	nothing = 0,
	unchanged = 1 << 8,
	AtCross = 2 << 8,
	LeaveCross = 3 << 8,
	UNEXPECTED_ERROR = 4 << 8,
	REDUNDANT_ERROR = 5 << 8,
	RotateEnd = 6 << 8,
	ROTATE_CONFLICT_ERROR = 7 << 8
};


int advancedFollow(int thisState, int lastState, bool is_moving_forward, int grab_distance, string cmd)
{
	const int L = 1;
	const int R = 2;
	const int M = 4;
	const int T = 8;

	/// 0 change
	const int good = 0b1100;

	/// 1 change
	const int tail_out = 0b0100;
	const int left_in = 0b1101;
	const int right_in = 0b1110;
	// impossible middle_out = 0b1000;

	/// 2 changes
	const int tOut_lIn = 0b0101;
	const int tOut_rIn = 0b0110;
	const int at_cross = 0b1111;
	const int mOut_lIn = 0b1001;
	const int mOut_rIn = 0b1010;
	// impossible mOut_tOut = 0b0000;

	/// 3 changes
	// impossible tail_correct = 0b1011;
	const int middle_correct = 0b0111; // at cross wrong bearing
	const int left_correct = 0b0001; // very bad
	const int right_correct = 0b0010; // very bad

	/// 4 changes
	const int all_wrong = 0b0011; // at cross wrong position

	if (thisState == lastState)
	{
		return unchanged;
	}

	if (cmd == "GOFORWARD")
	{
		if (thisState == good)
			/// it's on the simple strait line
		{
			return straitForward;
		}

		if (lastState == good)
		{
			if (thisState ^ lastState == L)
			{
				/// bearing: almost correct; position: to the right
				return -sLeft;
			}
			else if (thisState ^ lastState == R)
			{
				return sRight;
			}
			else if (thisState ^ lastState == M)
			{
				/// this cannot happen, unless sensors are broken
				return UNEXPECTED_ERROR;
			}
			else if (thisState ^ lastState == T)
			{
				/// bearing: not correct; position: almost correct
				/// move until L / R changes
				return straitForward;
			}
			else
			{
				/// more than one sensor changes
				return REDUNDANT_ERROR;
			}
		}
		else if (lastState == tail_out)
		{
			if (thisState ^ lastState == L)
			{
				/// bearing: to the right; position: a bit to the right
				return -fRight;
			}
			else if (thisState ^ lastState == R)
			{
				return fLeft;
			}
			else if (thisState ^ lastState == M)
			{
				/// cannot go all black with L / R sensors on both sides
				return UNEXPECTED_ERROR;
			}
			else if (thisState ^ lastState == T)
			{
				/// tail in automatically, good
				return straitForward;
			}
			else
			{
				return REDUNDANT_ERROR;
			}
		}
		else if (lastState == left_in)
		{
			if (thisState ^ lastState == L)
			{
				/// left out automatically, good
				return straitForward;
			}
			else if (thisState ^ lastState == R)
			{
				/// must meet the cross
				/// bearing: to the right
				return -sLeft + AtCross;
			}
			else if (thisState ^ lastState == M)
			{
				/// bearing: to the right; position: far to the right
				return -sRight;
			}
			else if (thisState ^ lastState == T)
			{
				/// move forward until middle out / left out / tail in
				return straitForward;
			}
			else
			{
				return REDUNDANT_ERROR;
			}
		}
		else if (lastState == right_in) /// symmetric with above
		{
			if (thisState ^ lastState == L)
			{
				return sRight + AtCross;
			}
			else if (thisState ^ lastState == R)
			{
				return straitForward;
			}
			else if (thisState ^ lastState == M)
			{
				return sLeft;
			}
			else if (thisState ^ lastState == T)
			{
				return straitForward;
			}
			else
			{
				return REDUNDANT_ERROR;
			}
		}
		else if (lastState == tOut_lIn)
		{
			if (thisState ^ lastState == L)
			{
				if (is_moving_forward)
				{
					/// bearing: to the left; position: a bit to the right
					return nRight;
				}
				else
				{
					/// bearing: to the right; position: a bit to the right
					return -nRight;
				}
			}
			else if (thisState ^ lastState == R)
			{
				/// go into a cross and is not strait
				/// bearing: to the right; position: about correct
				return -middle + AtCross;
			}
			else if (thisState ^ lastState == M)
			{
				if (is_moving_forward)
				{
					/// bearing: to the right; position: to the right
					return -fRight;
				}
				else
				{
					/// bearing: to the left; position: to the right
					return nRight;
				}
			}
			else if (thisState ^ lastState == T)
			{
				/// tail in automatically, good
				/// bearing: to the right; position: to the right
				return -sRight;
			}
			else
			{
				return REDUNDANT_ERROR;
			}
		}
		else if (lastState == tOut_rIn) /// symmetric with above
		{
			if (thisState ^ lastState == L)
			{
				return middle + AtCross;
			}
			else if (thisState ^ lastState == R)
			{
				if (is_moving_forward)
				{
					return -nLeft;
				}
				else
				{
					return nLeft;
				}
			}
			else if (thisState ^ lastState == M)
			{
				if (is_moving_forward)
				{
					return fLeft;
				}
				else
				{
					return -nLeft;
				}
			}
			else if (thisState ^ lastState == T)
			{
				return sLeft;
			}
			else
			{
				return REDUNDANT_ERROR;
			}
		}
		else if (lastState == at_cross)
		{
			if (thisState ^ lastState == L)
			{
				/// bearing: to the right; position: unknown
				return -sLeft + LeaveCross;
			}
			else if (thisState ^ lastState == R)
			{
				return sRight + LeaveCross;
			}
			else if (thisState ^ lastState == M)
			{
				return UNEXPECTED_ERROR;
			}
			else if (thisState ^ lastState == T)
			{
				/// bearing: unknown; position: unknown
				return straitForward;
			}
			else
			{
				return REDUNDANT_ERROR;
			}
		}
		else if (lastState == mOut_lIn)
		{
			if (thisState ^ lastState == L)
			{
				/// bearing: to the right; position: far to the right
				return -sRight;
			}
			else if (thisState ^ lastState == R)
			{
				return UNEXPECTED_ERROR;
			}
			else if (thisState ^ lastState == M)
			{
				/// middle enter, good
				/// bearing: to the right; position: a bit to the right
				return -nRight;
			}
			else if (thisState ^ lastState == T)
			{
				if (is_moving_forward)
				{
					/// bearing: almost correct; position: to the right
					return fLeft;
				}
				else
				{
					/// bearing: far to the right; position: to the right
					return -sRight;
				}
			}
			else
			{
				return REDUNDANT_ERROR;
			}
		}
		else if (lastState == mOut_rIn) /// symmetric with above
		{
			if (thisState ^ lastState == L)
			{
				return UNEXPECTED_ERROR;
			}
			else if (thisState ^ lastState == R)
			{
				return sLeft;
			}
			else if (thisState ^ lastState == M)
			{
				return nLeft;
			}
			else if (thisState ^ lastState == T)
			{
				if (is_moving_forward)
				{
					/// bearing: almost correct; position: to the right
					return -fRight;
				}
				else
				{
					/// bearing: far to the right; position: to the right
					return sLeft;
				}
			}
		}
		else if (lastState == middle_correct)
		{
			if (thisState ^ lastState == L)
			{
				/// bearing: to the right; position: unknown
				return -nLeft + LeaveCross;
			}
			else if (thisState ^ lastState == R)
			{
				return nRight + LeaveCross;
			}
			else if (thisState ^ lastState == M)
			{
				return UNEXPECTED_ERROR;
			}
			else if (thisState ^ lastState == T)
			{
				/// tail enters automatically, good
				return straitForward;
			}
			else
			{
				return REDUNDANT_ERROR;
			}
		}
		else if (lastState == left_correct)
		{
			if (thisState ^ lastState == L)
			{
				/// entering cross
				/// bearing: to the left; position: far to the left
				return sLeft;
			}
			else if (thisState ^ lastState == R)
			{
				/// all black, try to go back
				if (is_moving_forward)
				{
					return -straitForward;
				}
				else
				{
					return straitForward;
				}
			}
			else if (thisState ^ lastState == M)
			{
				/// middle enters, good
				/// bearing: to the left; position: a bit to the left
				return nLeft;
			}
			else if (thisState ^ lastState == T)
			{
				/// bearing: to the left; position: to the left
				return sLeft;
			}
			else
			{
				return REDUNDANT_ERROR;
			}
		}
		else if (lastState == right_correct)
		{
			if (thisState ^ lastState == L)
			{
				if (is_moving_forward)
				{
					return -straitForward;
				}
				else
				{
					return straitForward;
				}
			}
			else if (thisState ^ lastState == R)
			{
				return -sRight;
			}
			else if (thisState ^ lastState == M)
			{
				return -nRight;
			}
			else if (thisState ^ lastState == T)
			{
				return -sRight;
			}
			else
			{
				return REDUNDANT_ERROR;
			}
		}
		else if (lastState == all_wrong)
		{
			if (thisState ^ lastState == L)
			{
				if (is_moving_forward)
				{
					return UNEXPECTED_ERROR;
				}
				else
				{
					/// bearing: to the left; position: at cross but unknown
					return nRight;
				}
			}
			else if (thisState ^ lastState == R)
			{
				if (is_moving_forward)
				{
					return UNEXPECTED_ERROR;
				}
				else
				{
					return -nLeft;
				}
			}
			else if (thisState ^ lastState == M)
			{
				/// at cross, details unknown
				return straitForward;
			}
			else if (thisState ^ lastState == T)
			{
				return UNEXPECTED_ERROR;
			}
			else
			{
				return REDUNDANT_ERROR;
			}
		}
		else
		{
			return UNEXPECTED_ERROR;
		}
	}
}

struct Sposition
{
	double x;
	double y;
	double angle;

	Sposition(double defX, double defY, double defA)
	{
		x = defX;
		y = defY;
		angle = defA;
	}

	Sposition operator + (const Sposition& delta) const
	{
		Sposition res(this->x, this->y, this->angle);
		res.x += delta.x;
		res.y += delta.y;
		res.angle += delta.angle;
		return res;
	}
};

class vector2D
{
public:
	double x;
	double y;

	vector2D(double defX, double defY)
	{
		x = defX;
		y = defY;
	}

	vector2D operator + (const vector2D& b) const
	{
	    vector2D res(this->x, this->y);
	    res.x += b.x;
	    res.y += b.y;
	    return res;
	}
};

Sposition disp(double v1, double v2, double delta_t) {
	/* returns the estimated displacement of the robot*/
	/* distance between 2 wheels = 0.237m*/

	double diff, v_centre, d_centre;
	diff = (v1 * delta_t) - (v2 * delta_t);
	v_centre = (v1 + v2) / 2;
	d_centre = v_centre * delta_t;

	Sposition res(d_centre * sin(res.angle), d_centre * cos(res.angle), atan(diff / 0.237));
	return res;
}


int sensor_check(vector2D pos_1){
	/*check that the sensor position has been changed to cross onto the white line*/

	double t = 0.016;
	double a = t / 2;

	if (((pos_1.x + a) * (pos_1.x - a))< 0) {
		return 1;
	}
	else {
		return 0;
	}
	}


	int sensor_sim( vector2D L_i, vector2D R_i, vector2D M_i, vector2D T_i)
		/*sensor position relative to centre of white line (or wheels at t =0) */
	{
		int res = 0;
		/*sensor check returns 0 or 1 value depending on whether white line crossed*/
		res += sensor_check(L_i);
		res += sensor_check(R_i);
		res += sensor_check(M_i);
		res += sensor_check(T_i);
		//cout << "error " << res << bitset<sizeof(char)*4>(res) << endl;
		return res;
	}

	vector2D update_pos(vector2D pos, Sposition displacement){
		double theta = displacement.angle;
		//cout << "displacement ( " << displacement.x << " , " << displacement.y<< " )" << endl;
		double x_pos = pos.x * cos(theta) + pos.y * sin(theta);
		double y_pos = pos.x * -sin(theta) + pos.y * cos(theta);
		vector2D s(x_pos, y_pos);
        return s;}


int main()
	{
	    wheelSpeedGroup[0].leftV = 0;
        wheelSpeedGroup[0].rightV = 0;    // fullyStop
        wheelSpeedGroup[1].leftV = 1;
        wheelSpeedGroup[1].rightV = 1;    // straitForward
        wheelSpeedGroup[2].leftV = 1;
        wheelSpeedGroup[2].rightV = -1;   // middle
        wheelSpeedGroup[3].leftV = 0.5;
        wheelSpeedGroup[3].rightV = -1;   // near left
        wheelSpeedGroup[4].leftV = 1;
        wheelSpeedGroup[4].rightV = -0.5; // near right
        wheelSpeedGroup[5].leftV = 0;
        wheelSpeedGroup[5].rightV = -1;   // far left
        wheelSpeedGroup[6].leftV = 1;
        wheelSpeedGroup[6].rightV = 0;    // far right
        wheelSpeedGroup[7].leftV = -0.5;
        wheelSpeedGroup[7].rightV = -1;   // super far left
        wheelSpeedGroup[8].leftV = 1;
        wheelSpeedGroup[8].rightV = 0.5;  // super far right

		const double delta_t = 0.001;
		/*position of sensors relative to wheel centre*/

		int sensorval_1 = 0011;
		int sensorval_2 = 0011;

		vector2D L_pos(-0.015, 0.0);
		vector2D R_pos(0.017, 0.0);
		vector2D M_pos(0.0, 0.013);
		vector2D T_pos(0.0, 0.198);

		int v1 = 0;
        int v2 = 0;

		while (true)
		{
            bool forwardcheck = false;
			if (v1 + v2 > 0){
			forwardcheck = true;
			}

			int v_num = advancedFollow(sensorval_1, sensorval_2, forwardcheck, 0, "GOFORWARD");
			v1 = wheelSpeedGroup[v_num].leftV;
			v2 = wheelSpeedGroup[v_num].rightV;
			cout << "wheel speed 1" << v1 << endl;
			cout << "wheel speed 2" << v2 << endl;

			Sposition displacement = disp(v1, v2, delta_t);
			cout << "displacement ( " << displacement.x << " , " << displacement.y<< " )" << endl;
			vector2D L_pos = update_pos(L_pos, displacement);
			vector2D R_pos = update_pos(R_pos, displacement);
			vector2D M_pos = update_pos(M_pos, displacement);
			vector2D T_pos = update_pos(T_pos, displacement);

            //cout << "left sensor position: ( " << L_pos.x << " , " << L_pos.y<< " )" << endl;


			sensorval_1 = sensorval_2;
			sensorval_2 = sensor_sim(L_pos, R_pos, M_pos, T_pos);

			double centre_x = L_pos.x + R_pos.x;
			double centre_y = M_pos.y + T_pos.y;

			cout << "current sensor value:" << bitset<sizeof(char)*4>(sensorval_1) << endl;
			cout << "position of wheel centre: ( " << centre_x << " , " << centre_y << " )" << endl;
			cout << "left sensor position: ( " << L_pos.x << " , " << L_pos.y<< " )" << endl;


			system("pause");

		}
}