AMiRo-OS

/**

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

  }

}

  int rpmSpeed[2] = {0};

  int stop[2] = {0};

  int proxyWheelThresh = 18000;

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

          break;

        }

        default:{

          break;

        }

      }

    }else{

      // Stop

        setRpmSpeed(stop);

      }