AMiRo-OS

#include "userthread.hpp"

#include "global.hpp"

#include "linefollow2.hpp" 

using namespace amiro;

extern Global global;

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

// };

// a buffer for the z-value of the accelerometer

int16_t accel_z;

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

int vcnl4020AmbientLight[4] = {0};

int vcnl4020Proximity[4] = {0};

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

}

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

}

// Get the next policy rule

states getNextPolicy() {

        // If the policy is over, start again

        if (policyCounter >= sizeOfPolicy)

                policyCounter = 3;

        return policy[policyCounter++];

}

UserThread::UserThread() :

  chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>()

{

}

UserThread::~UserThread()

{

}

msg_t

UserThread::main()

{

        /*

         * 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())

        {

        /*

         * read accelerometer z-value

         */

        accel_z = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Z);

        /*

         * evaluate the accelerometer

         */

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

        }

  return RDY_OK;

}