Senaste versionen av labben

#include "scheduler.h"
#include "semphr.h"
#include <MeAurigaLab.h>

MeLineFollower lineSensor(PORT_9);
MeUltrasonicSensor opticalSensor(PORT_10);

TaskHandle_t xHandle1 = NULL;
TaskHandle_t xHandle2 = NULL;
TaskHandle_t xHandle3 = NULL;
TaskHandle_t xHandle4 = NULL;
TaskHandle_t xHandle5 = NULL;
TaskHandle_t xHandle6 = NULL;

MeEncoderOnBoard leftMotor(SLOT1);
MeEncoderOnBoard rightMotor(SLOT2);

/*////  Global variables  ////*/
int lineSensorData;
bool obstacleFound = false;
int leftMotorSpeed;
int rightMotorSpeed;
bool lineFound = false;
bool moveOn = false;
/*////////////////////////////*/

/*////  Semaphore ////*/
SemaphoreHandle_t ReadLineSensor;
SemaphoreHandle_t ApplyMotorSpeed;
SemaphoreHandle_t ReadOpticalSensor;
/*////////////////////*/


/*////  INIT MOTOR  ////*/
void isr_process_leftMotor(void)
{
  if(digitalRead(leftMotor.getPortB()) == 0)
  {
    leftMotor.pulsePosMinus();
  }
  else
  {
    leftMotor.pulsePosPlus();
  }
}

void isr_process_rightMotor(void)
{
  if(digitalRead(rightMotor.getPortB()) == 0){
    rightMotor.pulsePosMinus();
  }
  else
  {
    rightMotor.pulsePosPlus();
  }
}
/*//////////////////////*/

/*////  TaskReadLineSensor  ////*/
void TaskReadLineSensor(void *a)
{
  if(xSemaphoreTake(ReadLineSensor, 5) == pdTRUE)
  {
    lineSensorData = lineSensor.readSensors();
    xSemaphoreGive(ReadLineSensor);
  }
}
/*//////////////////////////////*/

/*////  TaskReadOpticalSensor  ////*/
void TaskReadOpticalSensor(void *a)
{
  if(xSemaphoreTake(ReadOpticalSensor, 5) == pdTRUE)
  {
    if(opticalSensor.distanceCm() < 15)
    {
      obstacleFound = true;
    }
    else
    {
      obstacleFound = false;
    }

    xSemaphoreGive(ReadOpticalSensor);
  }
}
/*//////////////////////////////*/

/*////  TaskSetMotorSpeed ////*/
void TaskSetMotorSpeed(void *a)
{
  if(xSemaphoreTake(ApplyMotorSpeed, 5) == pdTRUE)
  {
    leftMotor.setMotorPwm(-leftMotorSpeed);
    rightMotor.setMotorPwm(rightMotorSpeed);
    xSemaphoreGive(ApplyMotorSpeed);
  }
}
/*////////////////////////////*/

/*////  TaskFollowLine  ////*/
void TaskFollowLine(void *a)
{
  static int lastTurn;

  if(!obstacleFound && lineFound && xSemaphoreTake(ReadLineSensor, 5) == pdTRUE && xSemaphoreTake(ApplyMotorSpeed, 5) == pdTRUE && xSemaphoreTake(ReadOpticalSensor, 5) == pdTRUE) //Go in here only if no obstacle is found and the initial line has been found atleast once! check TaskSearchForLine
  {
    switch(lineSensorData)
    {
      case 0: //Forward
      leftMotorSpeed =  100;
      rightMotorSpeed = 100;
      lastTurn = 0;
      break;

      case 1: //Left
      leftMotorSpeed =  150;
      rightMotorSpeed = -150;
      lastTurn = 1;
      break;

      case 2: //Right
      leftMotorSpeed =  -150;
      rightMotorSpeed = 150;
      lastTurn = 2;
      break;

      case 3: //NoLine
      switch(lastTurn)  //Make correction if it was about to turn but then went outside the line
      {
        case 0: //It went forward
        leftMotorSpeed =  -100;
        rightMotorSpeed = -100;
        break;

        case 1: //It went left
        leftMotorSpeed =  150;
        rightMotorSpeed = -150;
        break;

        case 2: //It went right
        leftMotorSpeed =  -150;
        rightMotorSpeed = 150;
        break;
      }
      break;
    }
    xSemaphoreGive(ReadLineSensor);
    xSemaphoreGive(ReadOpticalSensor);
    xSemaphoreGive(ApplyMotorSpeed);
  }
}
/*//////////////////////////*/

/*////  TaskSearchForLine ////*/
void TaskSearchForLine(void *a)
{
  if(xSemaphoreTake(ReadLineSensor, 5) == pdTRUE)
  {
    if(lineSensorData == 3)
    {
      //Searching
      Serial.println("Searching");
    }
    else
    {
      //Found it
      lineFound = true;
    }

    xSemaphoreGive(ReadLineSensor);
  }
}
/*////////////////////////////*/

void TaskObstacleAvoidance(void *a)
{
  Serial.println(objective);
  switch(objective)
  {
    case 0: //Reverse
    if(obstacleFound)
    {
       if(xSemaphoreTake(ApplyMotorSpeed, 5) == pdTRUE)
       {
          leftMotorSpeed = 0;
          rightMotorSpeed = 0;
          xSemaphoreGive(ApplyMotorSpeed);
       }
       moveOn = true;
    }
    else if(moveOn && !obstacleFound)
    {
        objective = 1;
    }
    break;
  }
}



void setup() {

  attachInterrupt(leftMotor.getIntNum(), isr_process_leftMotor, RISING);
  attachInterrupt(rightMotor.getIntNum(), isr_process_rightMotor, RISING);

  Serial.begin(9600);

  TCCR1A = _BV(WGM10);
  TCCR1B = _BV(CS11) | _BV(WGM12);

  TCCR2A = _BV(WGM21) | _BV(WGM20);
  TCCR2B = _BV(CS21);

  //Semaphores
  ReadLineSensor = xSemaphoreCreateMutex();
  ApplyMotorSpeed = xSemaphoreCreateMutex();
  ReadOpticalSensor = xSemaphoreCreateMutex();

  vSchedulerInit();

  vSchedulerPeriodicTaskCreate(TaskReadOpticalSensor, "opticalSensorData", configMINIMAL_STACK_SIZE, NULL , 1, &xHandle1, pdMS_TO_TICKS(0),
  pdMS_TO_TICKS(20), pdMS_TO_TICKS(20), pdMS_TO_TICKS(20));

  vSchedulerPeriodicTaskCreate(TaskObstacleAvoidance, "obstacleAvoidance", configMINIMAL_STACK_SIZE, NULL , 2, &xHandle2, pdMS_TO_TICKS(0),
  pdMS_TO_TICKS(20), pdMS_TO_TICKS(20), pdMS_TO_TICKS(20));

  vSchedulerPeriodicTaskCreate(TaskSetMotorSpeed, "setMotorSpeed", configMINIMAL_STACK_SIZE, NULL , 3, &xHandle3, pdMS_TO_TICKS(0),
  pdMS_TO_TICKS(20), pdMS_TO_TICKS(20), pdMS_TO_TICKS(20));

  vSchedulerPeriodicTaskCreate(TaskReadLineSensor, "lineSensorData", configMINIMAL_STACK_SIZE, NULL , 4, &xHandle4, pdMS_TO_TICKS(0),
  pdMS_TO_TICKS(20), pdMS_TO_TICKS(20), pdMS_TO_TICKS(20));

  vSchedulerPeriodicTaskCreate(TaskFollowLine, "followLine", configMINIMAL_STACK_SIZE, NULL , 5, &xHandle5, pdMS_TO_TICKS(0),
  pdMS_TO_TICKS(20), pdMS_TO_TICKS(20), pdMS_TO_TICKS(20));

  vSchedulerPeriodicTaskCreate(TaskSearchForLine, "searchForLine", configMINIMAL_STACK_SIZE, NULL , 6, &xHandle6, pdMS_TO_TICKS(0),
  pdMS_TO_TICKS(20), pdMS_TO_TICKS(20), pdMS_TO_TICKS(20));



  vSchedulerStart();
}

void loop() {
  // put your main code here, to run repeatedly:

}