AMiRo-OS

#include "userthread.hpp"

#include "amiro_map.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);

}

void UserThread::chargeAsLED(){

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

}

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

void UserThread::getProxySectorVals(uint16_t (&proxVals)[8], uint16_t (&sProx)[8]){

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

    sProx[i] = (proxVals[i] < proxVals[(i+1) % 8]) ? proxVals[i] : proxVals[(i+1) % 8];

    // chprintf((BaseSequentialStream*)&global.sercanmux1, "%d: %d, ", i, sProx[i]);

  }

    // chprintf((BaseSequentialStream*)&global.sercanmux1, "\n");

}

void UserThread::getMaxFrontSectorVal(uint16_t (&sProx)[8], int32_t &sPMax){

  for (int i=2; i<5; i++){

    sPMax = (sPMax < sProx[i]) ? sProx[i] : sPMax;

  }

}

void UserThread::proxSectorSpeedCorrection(int (&rpmSpeed)[2], uint16_t (&proxVals)[8]){

  int i;

  uint16_t sProx[8];

  int32_t sPMax = 0;

  getProxySectorVals(proxVals, sProx);

  getMaxFrontSectorVal(sProx, sPMax);

  int32_t speedL = rpmSpeed[0] - (sPMax * pCtrl.pFactor);

  int32_t speedR = rpmSpeed[1] - (sPMax * pCtrl.pFactor);

  if(sPMax > pCtrl.threshMid){

      rpmSpeed[0] = 0;

      rpmSpeed[1] = 0;

      pCtrl.staticCont++;

  }else if((speedL > 0) || (speedR > 0)){

    pCtrl.staticCont = 0;

    rpmSpeed[0] = speedL;

    rpmSpeed[1] = speedR;

  }else{

    rpmSpeed[0] = 4000000 + (rpmSpeed[0] - global.rpmForward[0] * 1000000);

    rpmSpeed[1] = 4000000 + (rpmSpeed[1] - global.rpmForward[0] * 1000000);

  }

  for(i=4; i<5; i++){

    if ((proxVals[i] > pCtrl.threshMid) && (proxVals[i+1] > pCtrl.threshLow)){

      rpmSpeed[0] = -5000000 ;

      rpmSpeed[1] = -5000000 ;

      // pCtrl.staticCont++;

      break;

    }

  }

  chargeAsLED();

  // chprintf((BaseSequentialStream*)&global.sercanmux1, "Max: %d factor: %d, Panel: %d SpeedL: %d SpeedR: %d ActualL: %d ActualR: %d\n",sPMax,  pCtrl.pFactor,  sPMax * pCtrl.pFactor, speedL, speedR, rpmSpeed[0], rpmSpeed[1]);

}

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

void UserThread::preventCollision( int (&rpmSpeed)[2], uint16_t (&proxVals)[8]) {

  if((proxVals[3] > pCtrl.threshLow) || (proxVals[4] > pCtrl.threshLow)){

      rpmSpeed[0] = rpmSpeed[0] / 2;

      rpmSpeed[1] = rpmSpeed[1] / 2;

  }

  if((proxVals[3] > pCtrl.threshMid) || (proxVals[4] > pCtrl.threshMid)){

      rpmSpeed[0] = rpmSpeed[0] / 3;

      rpmSpeed[1] = rpmSpeed[1] / 3;

  }

  if((proxVals[3] > pCtrl.threshHigh) || (proxVals[4] > pCtrl.threshHigh)){

      rpmSpeed[0] = 0;

      rpmSpeed[1] = 0;

      utCount.ringProxCount++;

  }else{

    utCount.ringProxCount = 0;

  }

}

/**

 * Blocks as long as the position changes.

 */

void UserThread::checkForMotion(){

  bool motion = true;

  int led = 0;

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

  while(motion){

    this->sleep(200);

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

    motion = oldPos.x != tmp.x; //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++;

  }

  lightAllLeds(Color::BLACK);

}

bool UserThread::checkFrontalObject(){

  uint32_t thresh = pCtrl.threshMid;

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

  if (abs(start.x - stop_.x) > 200 /* || (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;

}

int32_t UserThread::meanDeviation(uint16_t a, uint16_t b){

  int32_t diff = a - b;

  int32_t res = 0;

  devCor.proxbuf[devCor.pCount] = (diff*100)/((a+b)/2);

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

    res += devCor.proxbuf[i];

  }

  devCor.pCount++;

  devCor.pCount = devCor.pCount % PROX_DEVIATION_MEAN_WINDOW;

  devCor.currentDeviation =  res / PROX_DEVIATION_MEAN_WINDOW;

  return devCor.currentDeviation;

}

void setAttributes(uint8_t (&map)[MAX_NODES][NODE_ATTRIBUTES],

                          uint8_t id, uint8_t l, uint8_t r, uint8_t att) {

  map[id][0] = l;

  map[id][1] = r;

  map[id][2] = att;

}

UserThread::UserThread() :

  chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>()

{

}

UserThread::~UserThread()

{

}

msg_t

UserThread::main()

{

  /*

   * SETUP

   */

  // User thread state:

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

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

  }

  running = false;

  LineFollowStrategy lStrategy = LineFollowStrategy::EDGE_RIGHT;

  LineFollow lf(&global);

  AmiroMap map(&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(currentState == states::INACTIVE){

        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::MSG_START:

        newState = states::CALIBRATION_CHECK;

        break;

      case msg_content::MSG_STOP:

        newState = states::IDLE;

        break;

      case msg_content::MSG_EDGE_RIGHT:

        // newState = states::FOLLOW_LINE;

        lStrategy = LineFollowStrategy::EDGE_RIGHT;

        break;

      case  msg_content::MSG_EDGE_LEFT:

        // newState = states::FOLLOW_LINE;

        lStrategy = LineFollowStrategy::EDGE_LEFT;

        break;

      case msg_content::MSG_FUZZY:

        // newState = states::FOLLOW_LINE;

        lStrategy = LineFollowStrategy::FUZZY;

        break;

      case msg_content::MSG_DOCK:

        newState = states::DETECT_STATION;

        break;

      case msg_content::MSG_UNDOCK:

        newState = states::RELEASE;

        break;

      case msg_content::MSG_CHARGE:

        newState = states::CHARGING;

        break;

      case msg_content::MSG_RESET_ODOMETRY:

        global.odometry.resetPosition();

        break;

      case msg_content::MSG_CALIBRATE_BLACK:

        proxCalib.calibrateBlack = true;

        // global.odometry.resetPosition();

        newState = states::CALIBRATION;

        break;

      case msg_content::MSG_CALIBRATE_WHITE:

        proxCalib.calibrateBlack = false;

        newState = states::CALIBRATION;

        break;

      case msg_content::MSG_TEST_MAP_STATE:

        newState = states::TEST_MAP_STATE;

        break;

      default:

        newState = states::IDLE;

        break;

      }

    }

    // newState = currentState;

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

    }

    // Continously update devication values

    meanDeviation(rProx[0] & 0xFFF0, rProx[7] & 0xFFF0);

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

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

    switch(currentState){

      case states::INACTIVE:

        // Dummy state to deactivate every interaction

      break;

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

      case states::CALIBRATION_CHECK:

        // global.robot.calibrate();

        if(global.linePID.BThresh >= global.linePID.WThresh){

          newState = states::CALIBRATION_ERROR;

        }else{

          newState = states::FOLLOW_LINE;

        }

      break;

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

      case states::CALIBRATION:

        /* Calibrate the global thresholds for black or white.

            This values will be used by the line follow object

        */

        proxCalib.buf = 0;

        if(proxCalib.calibrateBlack){

          chprintf((BaseSequentialStream*)&global.sercanmux1, "Black Calibration, Place AMiRo on black Surface!\n");

          global.robot.calibrate();

        }

        for(int i=0; i <= proxCalib.meanWindow; i++){

          proxCalib.buf += global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset()

                          + global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();

          this->sleep(CAN::UPDATE_PERIOD);

        }

        proxCalib.buf = proxCalib.buf / (2*proxCalib.meanWindow);

        if(proxCalib.calibrateBlack){

          global.linePID.BThresh = proxCalib.buf;

        }else  {

          global.linePID.WThresh  = proxCalib.buf;

        }

          chprintf((BaseSequentialStream*)&global.sercanmux1, "Black: %d, White: %d!\n", global.linePID.BThresh, global.linePID.WThresh);

        newState = states::IDLE;

      break;

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

      case states::IDLE:

        global.motorcontrol.setMotorEnable(true);

        setRpmSpeed(stop);

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

          global.robot.requestCharging(0);

        }

        // pCtrl.pFactor = 0;

        pCtrl.staticCont = 0;

        utCount.whiteCount = 0;

        utCount.ringProxCount = 0;

        utCount.errorCount = 0;

        newState = states::INACTIVE;

      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_DETETION_TIMEOUT){

            setRpmSpeed(stop);

            utCount.whiteCount = 0;

            newState = states::WHITE_DETECTION_ERROR;

          }

        }else{

          utCount.whiteCount = 0;

        }

        preventCollision(rpmSpeed, rProx);

        // proxSectorSpeedCorrection(rpmSpeed, rProx);

        if(utCount.ringProxCount > RING_PROX_DETECTION_TIMEOUT){

          utCount.ringProxCount = 0;

          checkForMotion();

          // Check if only front sensors are active

          if (checkFrontalObject()) {

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

            // // BaseThread::sleep(8000);

            // checkForMotion();

            this->utCount.whiteCount = 0;

            newState = states::TURN;

            // lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT);

          } else {

            newState = states::PROXY_DETECTION_ERROR;

          }

        }

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

          setRpmSpeedFuzzy(rpmSpeed);

        }else{

          setRpmSpeed(rpmSpeed);

        }

      break;

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

    case states::TURN:{

        // Check the line strategy in order to continue driving on the right side

      int factor = SPEED_CONVERSION_FACTOR;

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

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

      int blackSensor = 0;

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

        factor = -factor;

        blackSensor = frontL;

      }else{

        blackSensor = frontR;

      }

      rpmSpeed[0] = factor * CHARGING_SPEED;

      rpmSpeed[1] = -factor * CHARGING_SPEED;

      setRpmSpeed(rpmSpeed);

      if ((blackSensor >= global.linePID.WThresh )){

        utCount.whiteCount = 1;

      }else {

        if((utCount.whiteCount == 1) && (blackSensor <= global.linePID.BThresh)){

          utCount.whiteCount = 0;

          newState = states::FOLLOW_LINE;

          setRpmSpeed(stop);

        }

      }

      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.stateTime++;

        if (utCount.stateTime >= DOCKING_CORRECTION_TIMEOUT){

          utCount.stateTime = 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);

        // utCount.stateTime++;

        // Docking is only successful if Deviation is in range and sensors are at their max values.

        if((rProx[0] >= PROX_MAX_VAL)

           && (rProx[7] >= PROX_MAX_VAL)

           && ((devCor.currentDeviation > -MAX_DEVIATION_FACTOR) && (devCor.currentDeviation < MAX_DEVIATION_FACTOR) )){

          // setRpmSpeed(stop);

          // checkForMotion();

          utCount.stateTime = 0;

          newState = states::PUSH_BACK;

        }else if ((devCor.currentDeviation <= -MAX_DEVIATION_FACTOR) && ((rProx[0] > DEVIATION_DIST_THRESH) || (rProx[7] > DEVIATION_DIST_THRESH))){

          // Case R

          utCount.stateTime = 0;

          setRpmSpeed(stop);

          devCor.RCase = true;

          lightAllLeds(Color::YELLOW);

          newState = states::DEVIATION_CORRECTION;

        }else if ((devCor.currentDeviation >= MAX_DEVIATION_FACTOR) && ((rProx[0] > DEVIATION_DIST_THRESH) || (rProx[7] > DEVIATION_DIST_THRESH))){

          // Case L

          utCount.stateTime = 0;

          setRpmSpeed(stop);

          devCor.RCase = false;

          lightAllLeds(Color::WHITE);

          newState = states::DEVIATION_CORRECTION;

        }else if (utCount.stateTime >= REVERSE_DOCKING_TIMEOUT){

          setRpmSpeed(stop);

          utCount.stateTime = 0;

          utCount.errorCount++;

          if (utCount.errorCount >= DOCKING_ERROR_THRESH){

            newState = states::DOCKING_ERROR;

          }else{

            newState = states::CORRECT_POSITIONING;

          }

        }

        // if((devCor.currentDeviation <= -10)){

        //   rpmSpeed[0] -= 2000000;

        // }else if(devCor.currentDeviation >= 10){

        //   rpmSpeed[1] -= 2000000;

        // }

        // setRpmSpeed(rpmSpeed);

      break;

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

    case states::DEVIATION_CORRECTION:

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

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

        // }

        // lf.followLine(rpmSpeed);

        // setRpmSpeed(rpmSpeed);

        if(utCount.stateTime < DEVIATION_CORRECTION_DURATION / 2 ){

          if(devCor.RCase){

            rpmSpeed[0] = 0;

            rpmSpeed[1] = DEVIATION_CORRECTION_SPEED;

          }else {

            rpmSpeed[0] = DEVIATION_CORRECTION_SPEED;

            rpmSpeed[1] = 0;

          }

          setRpmSpeed(rpmSpeed);

        }else if (((utCount.stateTime >= DEVIATION_CORRECTION_DURATION / 2) && (utCount.stateTime < DEVIATION_CORRECTION_DURATION +10)) ){

          if(devCor.RCase){

            rpmSpeed[0] = DEVIATION_CORRECTION_SPEED;

            rpmSpeed[1] = 0;

          }else {

            rpmSpeed[0] = 0;

            rpmSpeed[1] = DEVIATION_CORRECTION_SPEED;

          }

          setRpmSpeed(rpmSpeed);

          if(((devCor.currentDeviation >= -5) && (devCor.currentDeviation <= 5))){

            utCount.stateTime = 0;

            newState = states::REVERSE;

            setRpmSpeed(stop);

          }

        }else{

          utCount.stateTime = 0;

          newState = states::REVERSE;

          setRpmSpeed(stop);

        }

        utCount.stateTime++;

        // if (utCount.stateTime > PUSH_BACK_TIMEOUT){

        //   utCount.stateTime = 0;

        //   newState = states::CHECK_POSITIONING;

        // }

      break;

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

    case states::PUSH_BACK:

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

          lf.setStrategy(LineFollowStrategy::REVERSE);

        }

        lf.followLine(rpmSpeed);

        setRpmSpeed(rpmSpeed);

        utCount.stateTime++;

        if (utCount.stateTime > PUSH_BACK_TIMEOUT){

          utCount.stateTime = 0;

          newState = states::CHECK_POSITIONING;

        }

      break;

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

    case states::CHECK_POSITIONING:

        setRpmSpeed(stop);

        checkForMotion();

        if(checkDockingSuccess()){

          newState = states::CHECK_VOLTAGE;

        }else{

          utCount.errorCount++;

          newState = states::CORRECT_POSITIONING;

          if (utCount.errorCount >= DOCKING_ERROR_THRESH){

              newState = states::DOCKING_ERROR;

            }

        }

      break;

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

    case states::CHECK_VOLTAGE:

        if(!checkPinEnabled()){

          global.robot.requestCharging(1);

        } else {

          if(checkPinVoltage()){

            // Pins are under voltage -> correctly docked

            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::DOCKING_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(1500, 0, ROTATION_DURATION);

        checkForMotion();

        newState = states::CORRECT_POSITIONING;

      break;

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

    case states::CHARGING:

        global.motorcontrol.setMotorEnable(false);

        utCount.errorCount = 0;

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

          // TODO: Use controlled

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

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

    case states::DOCKING_ERROR:

      newState = states::RELEASE;

      break;

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

    case states::REVERSE_TIMEOUT_ERROR:

      newState = states::IDLE;

      break;

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

    case states::CALIBRATION_ERROR:

      newState = states::IDLE;

      break;

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

    case states::WHITE_DETECTION_ERROR:

      newState = states::IDLE;

      break;

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

    case states::PROXY_DETECTION_ERROR:

      newState = states::IDLE;

      break;

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

    case states::NO_CHARGING_POWER_ERROR:

      newState = states::IDLE;

      break;

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

    case states::UNKNOWN_STATE_ERROR:

        newState = states::IDLE;

      break;

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

    case states::TEST_MAP_STATE:{

      // Test suit for amiro map

      // AmiroMap map = AmiroMap(&global);

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

      global.tcase = 0;

      // Set basic valid map configuration

      setAttributes(global.testmap, 0, 1, 2, 1);

      setAttributes(global.testmap, 1, 2, 2, 0);

      setAttributes(global.testmap, 2, 1, 0, 0);

      setAttributes(global.testmap, 3, 0, 0, 0xff);

      chprintf((BaseSequentialStream *)&global.sercanmux1, "Init Case: %d, res: %d\n",global.tcase, map.initialize());

      global.testres[global.tcase] = map.get_state()->valid;

      global.tcase++; // 1

      // Test map fail if first node is flagged with end

      setAttributes(global.testmap, 0, 1, 2, 0xff);

      map.initialize();

      global.testres[global.tcase] = !map.get_state()->valid;

      global.tcase++; // 2

      // Test if node 2 is set as start node

      setAttributes(global.testmap, 0, 1, 2, 0);

      setAttributes(global.testmap, 2, 1, 0, 1);

      map.initialize();

      global.testres[global.tcase] = map.get_state()->current == 2;

      global.tcase++; // 3

      // Test if non reachable nodes will trigger invalid map

      setAttributes(global.testmap, 3, 0, 0, 0);

      setAttributes(global.testmap, 4, 0, 0, 0xff);

      map.initialize();

      global.testres[global.tcase] = !map.get_state()->valid;

      global.tcase++; // 4

      // Test Reinitialization

      setAttributes(global.testmap, 0, 1, 2, 1);

      setAttributes(global.testmap, 1, 2, 2, 0);

      setAttributes(global.testmap, 2, 1, 0, 0);

      setAttributes(global.testmap, 3, 0, 0, 0xff);

      map.initialize();

      global.testres[global.tcase] = map.get_state()->valid;

      global.odometry.resetPosition();

      uint8_t ret = 0;

      global.tcase++; // 5

      // Test update under normal linefollowing without fixpoint

      ret = map.update(20000, 20000, LineFollowStrategy::EDGE_RIGHT);

      chprintf((BaseSequentialStream *)&global.sercanmux1,

               "Update test %d: Ret %d, cur %d, nex %d\n", global.tcase, ret,

               map.get_state()->current, map.get_state()->next);

      // No case should be true because neither was a node visited nor

      // was a fixpoint detected.

      global.testres[global.tcase] = (ret == 0x4);

      global.odometry.setPosition(1.0, 0.0, 0.0);

      chprintf((BaseSequentialStream *)&global.sercanmux1, "Current Point: %d\n", global.odometry.getPosition().x);

      global.tcase++; // 6

      // Fixpoint on left side

      ret = map.update(0, 20000, LineFollowStrategy::EDGE_RIGHT);

      chprintf((BaseSequentialStream *)&global.sercanmux1,

               "Update test %d: Ret %d, cur %d, nex %d\n", global.tcase, ret,

               map.get_state()->current, map.get_state()->next);

      // No case should be true because neither was a node visited nor

      // was a fixpoint detected.

      // global.odometry

      global.testres[global.tcase] = (ret == 0x1)

        && (map.get_state()->strategy == 0x01)

        && (map.get_state()->dist == 0)

        && (map.get_state()->current == 2);

      global.odometry.setPosition(1.5, 0.0, 0.0);

      chprintf((BaseSequentialStream *)&global.sercanmux1,

               "Current Point: %d\n", global.odometry.getPosition().x);

      global.tcase++; // 7

      // Fixpoint on left side, no update should appear because fixpoint already

      // marked

      ret = map.update(0, 20000, LineFollowStrategy::EDGE_RIGHT);

      chprintf((BaseSequentialStream *)&global.sercanmux1,

               "Update test %d: Ret %d, cur %d, nex %d\n", global.tcase, ret,

               map.get_state()->current, map.get_state()->next);

      // No case should be true because neither was a node visited nor

      // was a fixpoint detected.

      global.testres[global.tcase] = (ret == 0x00)

        && (map.get_state()->strategy == 0x01);

        // && (map.get_state()->dist == 0);

      global.odometry.setPosition(1.2, 0.0, 0.0);

      global.tcase++; // 8

      // Fixpoint on left side, no update should appear because fixpoint already

      // marked

      ret = map.update(20000, 20000, LineFollowStrategy::EDGE_RIGHT);

      chprintf((BaseSequentialStream *)&global.sercanmux1,

               "Update test %d: Ret %d, cur %d, nex %d, dist %d, len %d\n", global.tcase, ret,

               map.get_state()->current, map.get_state()->next, map.get_state()->dist, map.get_state()->eLength);

      // No case should be true because neither was a node visited nor

      // was a fixpoint detected.

      global.testres[global.tcase] =

          (ret == 0x04) && (map.get_state()->strategy == 0x01)

        && (map.get_state()->dist == 0);

      global.odometry.setPosition(.5, 0.0, 0.0);

      chprintf((BaseSequentialStream *)&global.sercanmux1,

               "Current Point: %d\n", global.odometry.getPosition().x);

      global.tcase++; // 9

      // Fixpoint on left side, no update should appear because fixpoint already

      // marked

      ret = map.update(0, 20000, LineFollowStrategy::EDGE_RIGHT);

      chprintf((BaseSequentialStream *)&global.sercanmux1,

               "Update test %d: Ret %d, cur %d, nex %d, dist %d, len %d\n",

               global.tcase, ret, map.get_state()->current,

               map.get_state()->next, map.get_state()->dist,

               map.get_state()->eLength);

      // No case should be true because neither was a node visited nor

      // was a fixpoint detected.

      global.testres[global.tcase] =

        (ret == 9) &&

        (map.get_state()->strategy == 1) &&

        (map.get_state()->dist == 0) &&

        (map.get_state()->eLength == 50);

      global.odometry.setPosition(.75, 0.0, 0.0);

      chprintf((BaseSequentialStream *)&global.sercanmux1,

               "Current Point: %d\n", global.odometry.getPosition().x);

      global.tcase++; // 10

      // Fixpoint on left side, no update should appear because fixpoint already

      // marked

      ret = map.update(20000, 20000, LineFollowStrategy::EDGE_RIGHT);

      chprintf((BaseSequentialStream *)&global.sercanmux1,

               "Update test %d: Ret %d, cur %d, nex %d, dist %d, len %d\n",

               global.tcase, ret, map.get_state()->current,

               map.get_state()->next, map.get_state()->dist,

               map.get_state()->eLength);

      // No case should be true because neither was a node visited nor

      // was a fixpoint detected.

      global.testres[global.tcase] =

          (ret == 12) && (map.get_state()->strategy == 1) &&

          (map.get_state()->dist == 50) && (map.get_state()->eLength == 50);

      int failed = 0;

      int passed = 0;

      for (int i = 0; i <= global.tcase; i++) {

        if (global.testres[i]) {

          passed++;

          chprintf((BaseSequentialStream *)&global.sercanmux1,

                   "Test %d Passed!\n", i);

        } else {

          failed++;

          chprintf((BaseSequentialStream *)&global.sercanmux1,

                   "Test %d Failed\n", i);

        }

      }

      chprintf((BaseSequentialStream *)&global.sercanmux1,

               "Total: %d, Passed: %d, Failed: %d\n", global.tcase + 1, passed,

               failed);

      newState = states::IDLE;

      break;

    }

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

    default:

      newState = states::UNKNOWN_STATE_ERROR;

      break;

    }

    // In case a new state is set:

    // 1. Record the state transition

    if (currentState != newState){

      global.stateTransitionCounter++;

      // Clear all state transitions to prevent overflow

      if (global.stateTransitionCounter >= 255) {

        global.stateTransitionCounter = 0;