Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include "scheduler.h"
- #include "semphr.h"
- #include <MeAurigaLab.h>
- MeLineFollower lineSensor(PORT_9);
- MeUltrasonicSensor opticalSensor(PORT_10);
- MeEncoderOnBoard rightMotor(SLOT1);
- MeEncoderOnBoard leftMotor(SLOT2);
- /*//// 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();
- }
- }
- /*//// TASKS ////*/
- TaskHandle_t xHandle1 = NULL;
- TaskHandle_t xHandle2 = NULL;
- TaskHandle_t xHandle3 = NULL;
- TaskHandle_t xHandle4 = NULL;
- TaskHandle_t xHandle5 = NULL;
- TaskHandle_t xHandle6 = NULL;
- /*//// SEMAPHORES ////*/
- SemaphoreHandle_t ReadOpticalSensorData;
- SemaphoreHandle_t ReadLineSensorData;
- SemaphoreHandle_t SetMotorSpeedData;
- /*//// GLOBAL VARIABLES ////*/
- bool obstacleFound = false;
- bool obstacleWasFound = false;
- bool makeArch = false;
- bool lineFound = false;
- int lineSensorData;
- int leftMotorSpeed;
- int rightMotorSpeed;
- /*//// FIND-LINE ////*/
- bool forward = true;
- bool left = false;
- int count = 30;
- /*//// READ-TASKS ////*/
- void TaskReadOpticalSensor(void *a)
- {
- if(xSemaphoreTake(ReadOpticalSensorData, 5) == pdTRUE)
- {
- if(opticalSensor.distanceCm() <= 20)
- {
- obstacleFound = true;
- }
- else
- {
- obstacleFound = false;
- }
- xSemaphoreGive(ReadOpticalSensorData);
- }
- //vTaskDelay(1); // one tick delay (15ms) in between reads for stability
- }
- void TaskReadLineSensor(void *a)
- {
- if(xSemaphoreTake(ReadLineSensorData, 5) == pdTRUE)
- {
- lineSensorData = lineSensor.readSensors();
- xSemaphoreGive(ReadLineSensorData);
- }
- //vTaskDelay(1); // one tick delay (15ms) in between reads for stability
- }
- /*//// MOTOR-TASK ////*/
- void TaskApplyMotorSpeed(void *a)
- {
- if(xSemaphoreTake(SetMotorSpeedData, 5) == pdTRUE)
- {
- leftMotor.setMotorPwm(leftMotorSpeed);
- rightMotor.setMotorPwm(-rightMotorSpeed);
- xSemaphoreGive(SetMotorSpeedData);
- }
- }
- /*//// OBSTACLE-AVOIDANCE ////*/
- void TaskAvoidObstacle(void *a)
- {
- if(xSemaphoreTake(ReadOpticalSensorData, 5) == pdTRUE)
- {
- //Go here until their is no obstacle in sight
- if(obstacleFound)
- {
- //Reverse
- obstacleWasFound = true;
- if(xSemaphoreTake(SetMotorSpeedData, 5) == pdTRUE)
- {
- leftMotorSpeed = -100;
- rightMotorSpeed = -100;
- xSemaphoreGive(SetMotorSpeedData);
- }
- }
- //After the robot has reversed we need to perfom the "left-arch manuver"
- else if(obstacleWasFound)
- {
- //Make left-turn
- if(xSemaphoreTake(SetMotorSpeedData, 5) == pdTRUE)
- {
- leftMotorSpeed = -180;
- rightMotorSpeed = 180;
- xSemaphoreGive(SetMotorSpeedData);
- makeArch = true;
- obstacleWasFound = false;
- }
- delay(400);
- /*//Make left-arch
- if(xSemaphoreTake(SetMotorSpeedData, 5) == pdTRUE)
- {
- leftMotorSpeed = 150;
- rightMotorSpeed = 90;
- xSemaphoreGive(SetMotorSpeedData);
- }
- delay(1500);
- obstacleWasFound = false;*/
- }
- else if(makeArch)
- {
- if(lineSensorData == 3)
- {
- if(xSemaphoreTake(SetMotorSpeedData, 5) == pdTRUE)
- {
- leftMotorSpeed = 150;
- rightMotorSpeed = 90;
- xSemaphoreGive(SetMotorSpeedData);
- }
- }
- else
- {
- makeArch = false;
- }
- }
- xSemaphoreGive(ReadOpticalSensorData);
- }
- }
- /*//// LINE-TASKS ////*/
- void TaskFollowTheLine(void *a)
- {
- static int lastTurn;
- if(xSemaphoreTake(ReadOpticalSensorData, 5) == pdTRUE)
- {
- if(lineFound && !obstacleFound && !makeArch)
- {
- if(xSemaphoreTake(ReadLineSensorData, 5) == pdTRUE)
- {
- if(xSemaphoreTake(SetMotorSpeedData, 5) == pdTRUE)
- {
- switch(lineSensorData)
- {
- case 0: //Forward
- leftMotorSpeed = 120;
- rightMotorSpeed = 120;
- lastTurn = 0;
- obstacleWasFound = false;
- break;
- case 1: //Left
- leftMotorSpeed = -120;
- rightMotorSpeed = 120;
- lastTurn = 1;
- obstacleWasFound = false;
- break;
- case 2: //Right
- leftMotorSpeed = 120;
- rightMotorSpeed = -120;
- lastTurn = 2;
- obstacleWasFound = false;
- break;
- case 3: //No line
- switch(lastTurn)
- {
- case 0: //Reverse
- leftMotorSpeed = -90;
- rightMotorSpeed = -90;
- break;
- case 1: //Left
- leftMotorSpeed = -160;
- rightMotorSpeed = 160;
- break;
- case 2: //Right
- leftMotorSpeed = 160;
- rightMotorSpeed = -160;
- break;
- }
- break;
- }
- xSemaphoreGive(SetMotorSpeedData);
- }
- xSemaphoreGive(ReadLineSensorData);
- }
- }
- xSemaphoreGive(ReadOpticalSensorData);
- }
- }
- void TaskFindTheLine(void *a)
- {
- if(xSemaphoreTake(ReadLineSensorData, 5) == pdTRUE)
- {
- if(lineSensorData != 3) //3 == no line
- {
- lineFound = true; //This is always true when the initial line is found.
- }
- else if(!lineFound)
- {
- Serial.print(count);
- Serial.print(":");
- //Search for the initial line. This happens at the start of the program
- if(forward && !left && count > 18)
- {
- Serial.println("Forward");
- if(xSemaphoreTake(SetMotorSpeedData, 5) == pdTRUE)
- {
- //Forward
- leftMotorSpeed = 120;
- rightMotorSpeed = 120;
- xSemaphoreGive(SetMotorSpeedData);
- }
- delay(100);
- count--;
- if(count <= 18)
- forward = false;
- }
- else if(!forward && count > 6 && count <= 18)
- {
- Serial.println("Reverse");
- if(xSemaphoreTake(SetMotorSpeedData, 5) == pdTRUE)
- {
- //Reverse
- leftMotorSpeed = -120;
- rightMotorSpeed = -120;
- xSemaphoreGive(SetMotorSpeedData);
- }
- delay(100);
- count--;
- if(count <= 6)
- {
- left = true;
- forward = true;
- }
- }
- else if(left && count > 0 && count <= 6)
- {
- Serial.println("Left");
- if(xSemaphoreTake(SetMotorSpeedData, 5) == pdTRUE)
- {
- //left-turn
- leftMotorSpeed = -180;
- rightMotorSpeed = 180;
- xSemaphoreGive(SetMotorSpeedData);
- }
- delay(100);
- count--;
- if(count <= 0)
- {
- left = false;
- count = 30;
- }
- }
- }
- xSemaphoreGive(ReadLineSensorData);
- }
- }
- 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 ////*/
- ReadOpticalSensorData = xSemaphoreCreateMutex();
- ReadLineSensorData = xSemaphoreCreateMutex();
- SetMotorSpeedData = xSemaphoreCreateMutex();
- vSchedulerInit();
- /*//// TASKS ////*/
- vSchedulerPeriodicTaskCreate(TaskReadOpticalSensor, "t1", configMINIMAL_STACK_SIZE, NULL , 1, &xHandle1, pdMS_TO_TICKS(0),
- pdMS_TO_TICKS(20), pdMS_TO_TICKS(20), pdMS_TO_TICKS(20));
- vSchedulerPeriodicTaskCreate(TaskReadLineSensor, "t2", configMINIMAL_STACK_SIZE, NULL , 1, &xHandle2, pdMS_TO_TICKS(0),
- pdMS_TO_TICKS(20), pdMS_TO_TICKS(20), pdMS_TO_TICKS(20));
- vSchedulerPeriodicTaskCreate(TaskApplyMotorSpeed, "t3", configMINIMAL_STACK_SIZE, NULL , 1, &xHandle3, pdMS_TO_TICKS(0),
- pdMS_TO_TICKS(20), pdMS_TO_TICKS(20), pdMS_TO_TICKS(20));
- vSchedulerPeriodicTaskCreate(TaskAvoidObstacle, "t4", configMINIMAL_STACK_SIZE, NULL , 1, &xHandle4, pdMS_TO_TICKS(0),
- pdMS_TO_TICKS(20), pdMS_TO_TICKS(20), pdMS_TO_TICKS(20));
- vSchedulerPeriodicTaskCreate(TaskFollowTheLine, "t5", configMINIMAL_STACK_SIZE, NULL , 1, &xHandle5, pdMS_TO_TICKS(0),
- pdMS_TO_TICKS(20), pdMS_TO_TICKS(20), pdMS_TO_TICKS(20));
- vSchedulerPeriodicTaskCreate(TaskFindTheLine, "t6", configMINIMAL_STACK_SIZE, NULL , 1, &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:
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement