AMiRo-OS

#include "userthread.hpp"

#include "global.hpp"

#include "linefollow2.hpp" 

using namespace amiro;

extern Global global;

enum class states{

  FOLLOW_LINE,

  OCCUPY,

  RELEASE,

  CHARGING

};

// User thread state:

states utState = states::FOLLOW_LINE;

// a buffer for the z-value of the accelerometer

int16_t accel_z;

bool running = false;

int vcnl4020AmbientLight[4] = {0};

int vcnl4020Proximity[4] = {0};

// Set the speed by the array

void setRpmSpeed(const int (&rpmSpeed)[2]) {

  global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);

}

void lightOneLed(Color color, int idx){

  global.robot.setLightColor(idx, Color(color));

}

void lightAllLeds(Color color){

  int led = 0;

  for(led=0; led<8; led++){

        lightOneLed(color, led);

      }

}

UserThread::UserThread() :

  chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>()

{

}

UserThread::~UserThread()

{

}

msg_t

UserThread::main()

{

  /*

   * SETUP

   */

  int rpmFuzzyCtrl[2] = {0};

  for (uint8_t led = 0; led < 8; ++led) {

    global.robot.setLightColor(led, Color(Color::BLACK));

  }

  running = false;

  LineFollow lf(&global);

  /*

   * LOOP

   */

  while (!this->shouldTerminate())

  {

        /*

         * read accelerometer z-value

         */

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

        /*

         * evaluate the accelerometer

         */

  //Features over can: (Line Following)

  //SetKP

  //SetKI

  //SetKd ?

  //SetSleepTime

  //SeForwardSpeed? or SetMaxSpeed

  //DriveOnLeftLine

  //DriveOnRightLine

  //if accel_z<-900

    //send can event (one time)

    // Line following is started if amiro roteted

    if (accel_z < -900 /*-0.9g*/) { //new: (can.allowLineFollowOnTurn && accel_z < 900) || can.startLineFollow optional feature

        // set the front LEDs to blue for one second

        lightAllLeds(Color::GREEN);

        this->sleep(1000);

        lightAllLeds(Color::BLACK);

        running = !running;

    }

    if(running){

    }

      lf.followLine(rpmFuzzyCtrl);

            setRpmSpeed(rpmFuzzyCtrl);

    // this->sleep(US2ST(5));

    this->sleep(CAN::UPDATE_PERIOD);

  }

  return RDY_OK;

}