AMiRo-OS

// State machine states

enum states : uint8_t {

	IDLE,

	GO_RIGHT,

	GO_STRAIGHT,

	PARKING,

	PARKING_RIGHT,

	PARKING_LEFT,

	GO_LEFT,

	SPINNING_PARKING,

	SPINNING

// Policy

states policy[] = {

  GO_STRAIGHT,

  GO_RIGHT,

  GO_RIGHT,

  GO_STRAIGHT,

  GO_RIGHT,

  GO_STRAIGHT,

  GO_RIGHT,

  GO_STRAIGHT,

  GO_STRAIGHT,

  GO_RIGHT,

  GO_STRAIGHT,

  GO_RIGHT,

  GO_STRAIGHT

};

// The different classes (or members) of color discrimination

// 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

// };

bool running;

// Get some information about the policy

const int sizeOfPolicy = sizeof(policy) / sizeof(states);

int policyCounter = 0; // Do not change this, it points to the beginning of the policy

// Border for the discrimination between black and white

const int discrBlackWhite = 16000; // border in "raw sensor values"

// Discrimination between black and white (returns BLACK or WHITE)

// The border was calculated by a MAP-decider

colorMember discrimination(int value) {

	if (value < discrBlackWhite)

		return BLACK;

	else

		return WHITE;

}

// Copy the speed from the source to the target array

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]);

}

	global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);

// Get the next policy rule

states getNextPolicy() {

	// If the policy is over, start again

	if (policyCounter >= sizeOfPolicy)

		policyCounter = 3;

	return policy[policyCounter++];

	/*

	 * SETUP

	 */

	int rpmFuzzyCtrl[2] = {0};

    for (uint8_t led = 0; led < 8; ++led) {

		global.robot.setLightColor(led, Color(Color::BLACK));

    }

    running = false;

	LineFollow lf(&global);

	/*

	 * LOOP

	 */

	while (!this->shouldTerminate())

	{

	//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;

                global.motorcontrol.setTargetRPM(0, 0);

            } else {

                // start the robot

                running = true;

            }

            // set the front LEDs to blue for one second

            global.robot.setLightColor(constants::LightRing::LED_SSW, Color(Color::BLACK));

            global.robot.setLightColor(constants::LightRing::LED_WSW, Color(Color::BLACK));

            global.robot.setLightColor(constants::LightRing::LED_WNW, Color(Color::WHITE));

            global.robot.setLightColor(constants::LightRing::LED_NNW, Color(Color::WHITE));

            global.robot.setLightColor(constants::LightRing::LED_NNE, Color(Color::WHITE));

            global.robot.setLightColor(constants::LightRing::LED_ENE, Color(Color::WHITE));

            global.robot.setLightColor(constants::LightRing::LED_ESE, Color(Color::BLACK));

            global.robot.setLightColor(constants::LightRing::LED_SSE, Color(Color::BLACK));

            this->sleep(MS2ST(1000));

            global.robot.setLightColor(constants::LightRing::LED_WNW, Color(Color::BLACK));

            global.robot.setLightColor(constants::LightRing::LED_NNW, Color(Color::BLACK));

            global.robot.setLightColor(constants::LightRing::LED_NNE, Color(Color::BLACK));

            global.robot.setLightColor(constants::LightRing::LED_ENE, Color(Color::BLACK));

        }

        if (running) {

            // Read the proximity values

            for (int i = 0; i < 4; i++) {

                vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight();

                vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset();

            }

			// 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],

            //         vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT],

            //         vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT]);

	//if (configLineFollowing==2)

	//   lineFollownew

	//else

            // lineFollowing(vcnl4020Proximity, rpmFuzzyCtrl, &global);

			lf.followLine(rpmFuzzyCtrl);

            setRpmSpeed(rpmFuzzyCtrl);

        }

		// this->sleep(US2ST(5));

		this->sleep(CAN::UPDATE_PERIOD);

	}