AMiRo-OS

// #include "userthread.hpp"

#include "global.hpp"

#include <cmath>

#include "linefollow.hpp" 

// #include <cmath>

// #include "global.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] / 4;

      rpmSpeed[1] = rpmSpeed[1] / 4;

  }

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

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

  }

  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) > 0 /* || (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;

}

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

  /*

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

        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;

          newState = states::TURN;

        }

        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, 2792526, ROTATION_DURATION);

          // BaseThread::sleep(8000);

          checkForMotion();

          newState = states::FOLLOW_LINE;

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

        }else{

          newState = states::PROXY_DETECTION_ERROR;

        }

      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;

          }

        }

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

          if(devCor.RCase){

            rpmSpeed[0] = DEVIATION_CORRECTION_SPEED;

            rpmSpeed[1] = 0;

          }else {

            rpmSpeed[0] = 0;

            rpmSpeed[1] = DEVIATION_CORRECTION_SPEED;

          }

          setRpmSpeed(rpmSpeed);

        }else if (utCount.stateTime >= DEVIATION_CORRECTION_DURATION + 10) {  

          // Wait to clear the mean window buffer

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

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

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

      default:

        newState = states::UNKNOWN_STATE_ERROR;

      break;

      }

      if (currentState != newState){

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

        global.robot.transmitState(newState);

      }

      prevState = currentState;

      currentState = newState;

      if (utCount.stateCount > CAN_TRANSMIT_STATE_THRESH){

          utCount.stateCount = 0;

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

        global.robot.transmitState(currentState);

        // global.robot.setOdometry(global.odometry.getPosition());

      }else{

        utCount.stateCount++;

      }

    this->sleep(CAN::UPDATE_PERIOD);

  }

  return RDY_OK;

}