AMiRo-OS

// #include "userthread.hpp"

#include "global.hpp"

#include <cmath>

#include "linefollow.hpp"

#include "userthread.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] / 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);