AMiRo-OS

// #include "userthread.hpp"

#include "global.hpp"

#include <cmath>

#include "userthread.hpp"

// #include <cmath>

// #include "global.hpp"

/**

 * Set speed.

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

 */

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

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

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

    }

  }

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

  }else{

    utCount.ringProxCount = 0;

  }

}

 * Blocks as long as the position changes.

 */

void UserThread::checkForMotion(){

  int led = 0;

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

  while(motion){

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

    oldPos = tmp;

  }

  lightAllLeds(Color::BLACK);

}

}

bool UserThread::checkPinVoltage(){

}

bool UserThread::checkPinEnabled(){

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

    success = 1;

  }

  // this->sleep(500);

  lightAllLeds(Color::BLACK);

  return success;

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

  int32_t diff = a - b;

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

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

    res += devCor.proxbuf[i];

    * read accelerometer z-value

    */

    accel_z = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Z);

      // Start line following when AMiRo is rotated

      if(currentState == states::INACTIVE){

        newState = states::FOLLOW_LINE;

    }

    // newState = currentState;

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

          global.robot.calibrate();

        }

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

          this->sleep(CAN::UPDATE_PERIOD);

        }

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

        if(proxCalib.calibrateBlack){

          global.linePID.BThresh = proxCalib.buf;

        }else  {

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

          global.forwardSpeed = global.rpmForward[0];

        }

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

          lf.setStrategy(lStrategy);

        }

        if(utCount.ringProxCount > RING_PROX_DETECTION_TIMEOUT){

          utCount.ringProxCount = 0;

          checkForMotion();

          // Check if only front sensors are active

          setRpmSpeed(rpmSpeed);

        }

      break;

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

    case states::TURN:{

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

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

        factor = -factor;

      }

      rpmSpeed[0] = factor * CHARGING_SPEED;

      rpmSpeed[1] = -factor * CHARGING_SPEED;

      setRpmSpeed(rpmSpeed);

        utCount.whiteCount = 1;

          utCount.whiteCount = 0;

          newState = states::FOLLOW_LINE;

          setRpmSpeed(stop);

    }

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

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

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

          setRpmSpeed(stop);

          checkForMotion();

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

          // BaseThread::sleep(8000);

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

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

          utCount.stateTime = 0;

          setRpmSpeed(stop);

          devCor.RCase = false;

          global.robot.requestCharging(1);

        } else {

          if(checkPinVoltage()){

            newState = states::CHARGING;

          }else{

            utCount.errorCount++;

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

              newState = states::RELEASE_TO_CORRECT;

            } else {

            }

            if (utCount.errorCount > DOCKING_ERROR_THRESH){

      break;

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

      case states::RELEASE_TO_CORRECT:

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

        checkForMotion();

        // move 1cm forward

          global.robot.requestCharging(0);

        }else{

          global.motorcontrol.setMotorEnable(true);

          //Rotate -20° to free from magnet

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

          checkForMotion();

          {global.stateTracker[states::CALIBRATION_CHECK] += 1;}

        else if (newState == states::DEVIATION_CORRECTION)

          {global.stateTracker[states::DEVIATION_CORRECTION] += 1;}

        else if (newState == states::DOCKING_ERROR){global.stateTracker[16+(-states::DOCKING_ERROR)] += 1;}

        else if (newState == states::REVERSE_TIMEOUT_ERROR){global.stateTracker[16+(-states::REVERSE_TIMEOUT_ERROR)] += 1;}

        else if (newState == states::CALIBRATION_ERROR){global.stateTracker[16+(-states::CALIBRATION_ERROR)] += 1;}

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

      //   global.robot.transmitState(currentState);

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

      // }else{

      //   utCount.stateCount++;

      // }

#include <amiroosconf.h>

#include <amiro/Color.h>

// #include "global.hpp"

#include <cmath>

#define DOCKING_CORRECTION_TIMEOUT 200

#define REVERSE_DOCKING_TIMEOUT 2*DOCKING_CORRECTION_TIMEOUT

#define REVERSE_ADJUSTMENT_TIMEOUT 200

// Thresh for wheel proxy sensors, when summed values fall below the state changes

// #define PROXY_WHEEL_THRESH 18000

// #define WHITE_COUNT_THRESH 150

#define WHITE_DETETION_TIMEOUT 150

// #define RING_PROX_COUNT_THRESH 1000

// Rotation around 180 degrees in microradian

// #define ROTATION_180 3141592

#define ROTATION_180 3141592

    uint32_t buf = 0;

    uint8_t meanWindow = 150;

  };

  struct deviation_correction {

    bool RCase = true;

    int8_t pCount = 0;

  // static const struct ut_counter emptyUtCount;

  ut_counter utCount;

  proxy_ctrl pCtrl;

  deviation_correction devCor;

  * Returns percentage of mean deviation between two given values.

  * It is intended to calculate the mean deviation between two proxy sensor

  * values. PROX_DEVIATION_MEAN_WINDOW determains the size of the mean window.

  * */

  int32_t meanDeviation(uint16_t a, uint16_t b);

   * Check sectors around and stop if a thresh in one sector is detected.

   */

  void preventCollision(int (&rpmSpeed)[2], uint16_t (&proxVals)[8]);

  /**

   * Same as prevent collision but also lowers the speed when object is detected.

   */

  void getProxySectorVals(uint16_t (&proxVals)[8], uint16_t (&sProx)[8]);

  void getMaxFrontSectorVal(uint16_t (&sProx)[8], int32_t &sPMax);

  void chargeAsLED();

  /**

   * Returns true when front sensors reaching high values

   * and all others are low. This indicates that the loading station is ahead.

  /**

   * Check if current position changes when the wheel are deactivated.

   * When AMiRo drives towards the loading station, it stops when a specific marker is reached.

   * In order to validate that the AMiRo is correctly positioned in the loading station

   * the wheels are turned off. When the position remains the same the docking procedure

   */

  int checkDockingSuccess();