AMiRo-OS

#include "global.hpp"

#include <cmath>

#include "linefollow.hpp"

#include "userthread.hpp"

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