AMiRo-OS

#include "userthread.hpp"

#include "global.hpp"

#include "linefollow.hpp" 

using namespace amiro;

extern Global global;

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

int16_t accel_z;

bool running = false;

/**

 * Set speed.

 * 

 * @param rpmSpeed speed for left and right wheel in rounds/min

 */

void UserThread::setRpmSpeedFuzzy(const int (&rpmSpeed)[2]) {

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

}

void UserThread::setRpmSpeed(const int (&rpmSpeed)[2]) {

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

}

void UserThread::lightOneLed(Color color, int idx){

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

}

void UserThread::lightAllLeds(Color color){

  int led = 0;

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

        lightOneLed(color, led);

      }

}

void UserThread::showChargingState(){

  uint8_t numLeds = global.robot.getPowerStatus().state_of_charge / 12;

  Color color = Color::GREEN;

  if (numLeds <= 2){

    color = Color::RED;

  }else if(numLeds <= 6){

    color = Color::YELLOW;

  }

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

    lightOneLed(color, i);

    this->sleep(300);

  }

  this->sleep(1000);

  lightAllLeds(Color::BLACK);

}

/**

 * Blocks as long as the position changes.

 */

void UserThread::checkForMotion(){

  int motion = 1;

  int led = 0;

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

  while(motion){

    this->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++;

  }

}

bool UserThread::checkFrontalObject(){

  uint32_t thresh = 1000;

  uint32_t prox;

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

    prox = global.robot.getProximityRingValue(i);

    if((i == 3) || (i == 4)){

      if(prox < thresh){

        return false;

      }

    }else{

      if(prox > thresh){

        return false;

      }

    }

  }

  return true;

}

bool UserThread::checkPinVoltage(){

  return global.ltc4412.isPluggedIn(); 

}

bool UserThread::checkPinEnabled(){

  return global.ltc4412.isEnabled();

}

int UserThread::checkDockingSuccess(){

  // setRpmSpeed(stop);

  checkForMotion();

  int success = 0;

  global.odometry.resetPosition();

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

  global.motorcontrol.setMotorEnable(false);

  this->sleep(1000);

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

  // Amiro moved, docking was not successful

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

    lightAllLeds(Color::RED);

    // Enable Motor again if docking was not successful

    global.motorcontrol.setMotorEnable(true);

    success = 0;

  }else{

    lightAllLeds(Color::GREEN);

    success = 1;

  }

  // this->sleep(500);

  lightAllLeds(Color::BLACK);

  return success;

}

int UserThread::getProxyRingSum(){

  int prox_sum = 0;

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

    prox_sum += global.robot.getProximityRingValue(i);;

  }

  return prox_sum;

}

UserThread::UserThread() :

  chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>()

{

}

UserThread::~UserThread()

{

}

msg_t

UserThread::main()

{

  /*

   * SETUP

   */

  // User thread state:

  //  int whiteBuf = 0;

  //  int proxyBuf = 0;

  //  int reverseBuf = 0;

   int correctionStep = 0;

  //  int dist_count = 0;

  //  bool needDistance = false;

   uint16_t rProx[8]; // buffer for ring proxy values

  int rpmSpeed[2] = {0};

  int stop[2] = {0};

  int turn[2] = {5,-5};

  LineFollowStrategy lStrategy = LineFollowStrategy::EDGE_RIGHT;

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

    if (accel_z < -900 /*-0.9g*/) { 

      // Start line following when AMiRo is rotated

      if(utState == states::IDLE){

        newState = states::FOLLOW_LINE;

      }else{

        newState = states::IDLE;

      }

      lightAllLeds(Color::GREEN);

      this->sleep(1000);

      lightAllLeds(Color::BLACK);

    // If message was received handle it here:

    } else if(global.msgReceived){

      global.msgReceived = false;

      // running = true;

      switch(global.lfStrategy){

        case msg_content::START:

          newState = states::FOLLOW_LINE;

          break;

        case msg_content::STOP:

          newState = states::IDLE;

          break;

        case msg_content::EDGE_RIGHT:

          // newState = states::FOLLOW_LINE;

          lStrategy = LineFollowStrategy::EDGE_RIGHT;

          break;

        case  msg_content::EDGE_LEFT:

          // newState = states::FOLLOW_LINE;

          lStrategy = LineFollowStrategy::EDGE_LEFT;

          break;

        case msg_content::FUZZY:

          // newState = states::FOLLOW_LINE;

          lStrategy = LineFollowStrategy::FUZZY;

          break;

        case msg_content::DOCK:

          newState = states::DETECT_STATION;

          break;

        case msg_content::UNDOCK:

          newState = states::RELEASE;

          break;

        case msg_content::CHARGE:

          newState = states::CHARGING;

          break;

        default:

          newState = states::IDLE;

          break;

      }

    }

    // newState = utState;

    // Get sensor data 

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

    // uint16_t WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_RIGHT].getProximityScaledWoOffset();

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

      rProx[i] = global.robot.getProximityRingValue(i);

    }

    // int FL = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();

    // int FR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();

    switch(utState){

      case states::IDLE:

        global.motorcontrol.setMotorEnable(true);

        setRpmSpeed(stop);

        if(/* checkPinVoltage() && */ checkPinEnabled()){

          global.robot.requestCharging(0);

        }

        utCount.whiteCount = 0;

        utCount.proxyCount = 0;

        utCount.errorCount = 0;

        utCount.idleCount++;

        if (utCount.idleCount > 10){

          utCount.idleCount = 0;

        // chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState);

        global.robot.transmitState(utState);

      }

        break;

      // ---------------------------------------

      case states::FOLLOW_LINE:

      // Set correct forward speed to every strategy

        if (global.forwardSpeed != global.rpmForward[0]){

          global.forwardSpeed = global.rpmForward[0];

        }

        if(lf.getStrategy() != lStrategy){

          lf.setStrategy(lStrategy);

        }

        if(lf.followLine(rpmSpeed)){

          utCount.whiteCount++;

          if(utCount.whiteCount >= WHITE_COUNT_THRESH){

            rpmSpeed[0] = stop[0];

            rpmSpeed[1] = stop[1];

            newState = states::IDLE;

          }

        }else{

          utCount.whiteCount = 0;

          // setRpmSpeed(rpmSpeed);

        }

        if(getProxyRingSum() > PROXY_RING_THRESH){

          utCount.proxyCount++;

          if(utCount.proxyCount > WHITE_COUNT_THRESH){

            rpmSpeed[0] = stop[0];

            rpmSpeed[1] = stop[1];

            // newState = states::IDLE;

            if (continue_on_obstacle){

              newState = states::TURN;

              utCount.proxyCount = 0;

            }else{

              newState = states::IDLE;

            }

          }

        }else{

            utCount.proxyCount = 0;

        }

        if ((rProx[3] + rProx[4]) > RING_PROX_FRONT_THRESH){

          rpmSpeed[0] = stop[0];

          rpmSpeed[1] = stop[1];

        }

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

          setRpmSpeedFuzzy(rpmSpeed);

        }else{

          setRpmSpeed(rpmSpeed);

        }

        break;

      // ---------------------------------------

      case states::TURN:

        checkForMotion();

        // Check if only front sensors are active 

        if(checkFrontalObject()){

          global.distcontrol.setTargetPosition(0, ROTATION_180, ROTATION_DURATION);

          // BaseThread::sleep(8000);

          checkForMotion();

          newState = states::FOLLOW_LINE;

          lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT);

        }else{

          newState = states::IDLE;

        }

        break;

      // ---------------------------------------

      case states::DETECT_STATION:

        if (global.forwardSpeed != DETECTION_SPEED){

          global.forwardSpeed = DETECTION_SPEED;

        }

        if(lf.getStrategy() != LineFollowStrategy::EDGE_RIGHT){

          lf.setStrategy(LineFollowStrategy::EDGE_RIGHT);

        }

        lf.followLine(rpmSpeed);

        setRpmSpeed(rpmSpeed);

        // // Detect marker before docking station

        // if ((WL+WR) < PROXY_WHEEL_THRESH){

        // Use proxy ring 

        if ((rProx[3]+rProx[4]) > RING_PROX_FRONT_THRESH){

          setRpmSpeed(stop);

          checkForMotion();

          // 180° Rotation 

          global.distcontrol.setTargetPosition(0, ROTATION_180, ROTATION_DURATION);

          // BaseThread::sleep(8000);

          checkForMotion();

          newState = states::CORRECT_POSITIONING;

        }

        break;

      // ---------------------------------------

      case states::CORRECT_POSITIONING:

        if (global.forwardSpeed != CHARGING_SPEED){

          global.forwardSpeed = CHARGING_SPEED;

        }

        if(lf.getStrategy() != LineFollowStrategy::EDGE_LEFT){

          lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT);

        }

        lf.followLine(rpmSpeed);

        setRpmSpeed(rpmSpeed);

        utCount.stepCount++;

        if (utCount.stepCount >= MAX_CORRECTION_STEPS){

          utCount.stepCount = 0;

          newState = states::REVERSE;

          setRpmSpeed(stop);

          checkForMotion();

        }

        break;

      // ---------------------------------------

      case states::REVERSE:

        if(lf.getStrategy() != LineFollowStrategy::REVERSE){

          lf.setStrategy(LineFollowStrategy::REVERSE);

        }

        lf.followLine(rpmSpeed);

        setRpmSpeed(rpmSpeed);

        // if ((WL+WR) < PROXY_WHEEL_THRESH){

        // Is of those sensors at it max val means that the AMiRo cant drive back

        // so check if correctly positioned

        //definitely wrong positioned -> correct position directly without rotation

        // Both sensors are at a wall -> potential loading station dock and on magnet -> need to rotate

        // if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){

        //   setRpmSpeed(stop);

        //   checkForMotion();

        //   newState = states::CHECK_POSITIONING;

        // } else 

        if((rProx[0] >= PROX_MAX_VAL) || (rProx[7] >= PROX_MAX_VAL)){

          // setRpmSpeed(stop);

          // checkForMotion();

          utCount.stepCount = 0;

          newState = states::PUSH_BACK;

        }

        break;

      // ---------------------------------------

      case states::PUSH_BACK:

        if(lf.getStrategy() != LineFollowStrategy::REVERSE){

          lf.setStrategy(LineFollowStrategy::REVERSE);

        }

        lf.followLine(rpmSpeed);

        setRpmSpeed(rpmSpeed);

        utCount.stepCount++;

        if (utCount.stepCount > PUSH_BACK_COUNT){

          newState = states::CHECK_POSITIONING;

        }

        break;

      // ---------------------------------------

      case states::CHECK_POSITIONING:

        setRpmSpeed(stop);

        checkForMotion();

        // if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){

        if(checkDockingSuccess()){

          newState = states::CHECK_VOLTAGE;

        }else{

          utCount.errorCount++;

          newState = states::CORRECT_POSITIONING;

          if (utCount.errorCount > DOCKING_ERROR_THRESH){

              newState = states::ERROR;

            }

        }

        // }

        // else{

        //   newState = CORRECT_POSITIONING;

        // }

        break;

      // ---------------------------------------

      case states::CHECK_VOLTAGE:

        if(!checkPinEnabled()){

          global.robot.requestCharging(1);

        } else {

          if(checkPinVoltage()){

            // Pins are under voltage -> correctly docked 

            utCount.errorCount = 0;

            newState = states::CHARGING;

          }else{

            utCount.errorCount++;

            // No voltage on pins -> falsely docked

            // deactivate pins

            global.motorcontrol.setMotorEnable(true);

            global.robot.requestCharging(0);

            // TODO: Soft release when docking falsely

            if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){

              newState = states::RELEASE_TO_CORRECT;

            } else {

              newState = states::RELEASE_TO_CORRECT; //states::CORRECT_POSITIONING; 

            }

            if (utCount.errorCount > DOCKING_ERROR_THRESH){

              newState = states::ERROR;

            }

          }

        }

        break;

      // ---------------------------------------

      case states::RELEASE_TO_CORRECT:

        global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);

        checkForMotion();

        // move 1cm forward

        global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);

        checkForMotion();

        // rotate back

        global.distcontrol.setTargetPosition(0, -2*ROTATION_20, ROTATION_DURATION);

        checkForMotion();

        global.distcontrol.setTargetPosition(7000, 0, ROTATION_DURATION);

        checkForMotion();

        newState = states::CORRECT_POSITIONING;

        break;

      // ---------------------------------------

      case states::ERROR:

        utCount.errorCount = 0;

        // lStrategy = LineFollowStrategy::EDGE_RIGHT;

        newState = states::RELEASE;

        break;

      // ---------------------------------------

      case states::CHARGING:

        global.motorcontrol.setMotorEnable(false);

        // Formulate Request to enable charging

        if(/* checkPinVoltage() && */ !checkPinEnabled()){

          global.robot.requestCharging(1);

        }

        if(checkPinEnabled()){

          showChargingState();

        }

        break;

      // ---------------------------------------

      case states::RELEASE:

      if (global.forwardSpeed != DETECTION_SPEED){

          global.rpmForward[0] = DETECTION_SPEED;

        }

        if(/* checkPinVoltage() && */ checkPinEnabled()){

          global.robot.requestCharging(0);

        }else{

          global.motorcontrol.setMotorEnable(true);

          //Rotate -20° to free from magnet

          global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);

          checkForMotion();

          // move 1cm forward

          global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);

          checkForMotion();

          // rotate back

          global.distcontrol.setTargetPosition(0, -ROTATION_20, ROTATION_DURATION);

          checkForMotion();

          // global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);

          // checkForMotion();

          lStrategy = LineFollowStrategy::EDGE_RIGHT;

          newState = states::FOLLOW_LINE;

            // whiteBuf = -100;

          // lf.followLine(rpmSpeed);

          // setRpmSpeed(rpmSpeed);

        }

        // lightAllLeds(Color::BLACK);

        break;

      default:

        break;

      }

      if (utState != newState){

        chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState);

        global.robot.transmitState(newState);

      }

      utState = newState;

    this->sleep(CAN::UPDATE_PERIOD);

  }

  return RDY_OK;

}