AMiRo-OS

#include "userthread.hpp"

#include "global.hpp"

#include "linefollow2.hpp" 

using namespace amiro;

extern Global global;

enum class states{

  FOLLOW_LINE,

  OCCUPY,

  RELEASE,

  CHARGING

};

enum msg_content{

  STOP, 

  START,

  EDGE_LEFT,

  EDGE_RIGHT,

  FUZZY,

  DOCKING

};

// User thread state:

states utState = states::FOLLOW_LINE;

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

int16_t accel_z;

bool running = false;

int vcnl4020AmbientLight[4] = {0};

int vcnl4020Proximity[4] = {0};

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

}

void lightOneLed(Color color, int idx){

  global.robot.setLightColor(idx, Color(color));

}

void lightAllLeds(Color color){

  int led = 0;

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

        lightOneLed(color, led);

      }

}

/**

 * Get the wanted strategy from the global object.

 */

LineFollowStrategy determineStrategy(){

  switch(global.lfStrategy){

    case 0:

      return LineFollowStrategy::EDGE_RIGHT;

      break;

    case 1:

      return LineFollowStrategy::EDGE_LEFT;

      break;

    case 2:

      return LineFollowStrategy::FUZZY;

      break;

    case 3:

      return LineFollowStrategy::DOCKING;

    default:

      break;

  }

  return LineFollowStrategy::NONE;

}

/**

 * Blocks as long as the position changes.

 */

void checkForMotion(UserThread *ut){

  int motion = 1;

  int led = 0;

  types::position oldPos = global.odometry.getPosition();

  while(motion){

    ut->sleep(500);

    types::position tmp = global.odometry.getPosition();

    motion = abs(oldPos.x - tmp.x)+ abs(oldPos.y - tmp.y)+abs(oldPos.z - tmp.z);

    oldPos = tmp;

      global.robot.setLightColor((led + 1) % 8, Color(Color::YELLOW));

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

      led++;

  }

}

/**

 * Turns MotorControl off and checks if position remains stable.

 */

int checkDockingSuccess(UserThread *ut){

  // global.motorcontrol.setTargetRPM(0,0);

  checkForMotion(ut);

  int led = 0;

  int success = 0;

  global.odometry.resetPosition();

  types::position start = global.startPos = global.odometry.getPosition();

  global.motorcontrol.toggleMotorEnable();

  ut->sleep(1000);

  types::position stop = global.endPos = global.odometry.getPosition();

  global.motorcontrol.toggleMotorEnable();

  // Amiro moved, docking was not successful

  if ((start.x + stop.x) || (start.y + stop.y)){

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

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

    }

    success = 0;

  }else{

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

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

    }

    success = 1;

  }

  ut->sleep(500);

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

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

  }

  return success;

}

void correctPosition(UserThread *ut, LineFollow &lf, int steps){

  int checkWhite = 0;

  int rpmSpeed[2] ={0};

  lf.setStrategy(LineFollowStrategy::EDGE_LEFT);

  for (int correction=0; correction<steps; correction++){

    checkWhite = lf.followLine(rpmSpeed);

    setRpmSpeed(rpmSpeed);

    ut->sleep(CAN::UPDATE_PERIOD);

  }

}

UserThread::UserThread() :

  chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>()

{

}

UserThread::~UserThread()

{

}

msg_t

UserThread::main()

{

  /*

   * SETUP

   */

  int rpmSpeed[2] = {0};

  int stop[2] = {0};

  int proxyWheelThresh = 18000;

  LineFollowStrategy lStrategy;

  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

        // set the front LEDs to blue for one second

        lightAllLeds(Color::GREEN);

        this->sleep(1000);

        lightAllLeds(Color::BLACK);

        running = !running;

    // If message was received handle it here:

    } else if(global.msgReceived){

      global.msgReceived = false;

      running = true;

      switch(global.lfStrategy){

        case msg_content::START:

          break;

        case msg_content::STOP:

          running = false;

          break;

        case msg_content::EDGE_RIGHT:

          lStrategy = LineFollowStrategy::EDGE_RIGHT;

          break;

        case  msg_content::EDGE_LEFT:

          lStrategy = LineFollowStrategy::EDGE_LEFT;

          break;

        case msg_content::FUZZY:

          lStrategy = LineFollowStrategy::FUZZY;

          break;

        case msg_content::DOCKING:

          utState = states::OCCUPY;

          lStrategy = LineFollowStrategy::EDGE_RIGHT;

          break;

        default:

          break;

      }

    }

    if(running){

      switch(utState){

        case states::FOLLOW_LINE:{

          // LineFollowStrategy lStrategy = determineStrategy();

          if(lStrategy == LineFollowStrategy::DOCKING){

            utState = states::OCCUPY;

            break;

          }

          lf.setStrategy(lStrategy);

          lf.followLine(rpmSpeed);

          setRpmSpeed(rpmSpeed);

          break;

        }

        case states::OCCUPY:{

          // Get Wheel proxy sensors

          int WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();

          int WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();

          // Check for black

          if ((WL+WR) < proxyWheelThresh){ /*TODO: Only check for one sensor */

            global.motorcontrol.setTargetRPM(0,0);

            // Stop when in docking state and take further actions

            if(lf.getStrategy() == LineFollowStrategy::DOCKING){

              if(checkDockingSuccess(this)){

                utState = states::CHARGING;

                break;

              }else{

                correctPosition(this, lf, 250);

                lf.setStrategy(LineFollowStrategy::DOCKING);

                // break;

              }

            }else{

              checkForMotion(this);

              // 180° Rotation 

              global.distcontrol.setTargetPosition(0, 3141592, 10000);

              // BaseThread::sleep(8000);

              checkForMotion(this);

              correctPosition(this, lf, 250);

              // break;

              lf.setStrategy(LineFollowStrategy::DOCKING);

            }

          }

          // Set correct following

          // lf.setStrategy(LineFollowStrategy::EDGE_RIGHT);

          lf.followLine(rpmSpeed);

          setRpmSpeed(rpmSpeed);

          break;

        }

        case states::RELEASE:{

          break;

        }

        case states::CHARGING:{

          setRpmSpeed(stop);

          break;

        }

        default:{

          break;

        }

      }

    }else{

      // Stop

        setRpmSpeed(stop);

      }

    this->sleep(CAN::UPDATE_PERIOD);

  }

  return RDY_OK;

}