Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <MeAurigaLab.h> // Contains sensor and port handler
- #include "scheduler.h" // Contains FreeRTOS and ESFree
- #include <Wire.h>
- #include "queue.h"
- #include "semphr.h"
- MeEncoderOnBoard Encoder_1(SLOT1); //Knyter motor encodern
- MeEncoderOnBoard Encoder_2(SLOT2); // till rätt motor kontakt
- //Pekare till tasks
- TaskHandle_t xHandle1 = NULL; // varibles containing pointers
- TaskHandle_t xHandle2 = NULL; // to tasks
- TaskHandle_t xHandle3 = NULL; // to tasks
- TaskHandle_t xHandle4 = NULL; // to tasks
- TaskHandle_t xHandle5 = NULL; // to tasks
- TaskHandle_t xHandle6 = NULL; // to tasks
- TaskHandle_t xHandle7 = NULL; // to tasks
- TaskHandle_t xHandle8 = NULL; // to tasks
- //Global linesensor
- int sensorState;
- //Global stop bool says if ulta sensor is within range or not
- bool stay = false;
- //The range for to close
- const int AvoidDist = 25;
- /*const int ult = 200;
- const int high = 90;
- const int low = 60;
- const int off = 0;*/
- int speedLeft;
- int speedRight;
- //Globala lägeskontroller
- int count = 0;
- //Flaga som säger om rob ska backa eller köra används när linjen ska hittas
- int flag = 0;
- //Tells task to perform linefollow task
- bool go = false;
- //Tells task to avoid current object
- bool avoid=false;
- //Strike to tell if programm needs to find line once again
- int strike=0;
- //flag sem
- SemaphoreHandle_t FlagSem;
- //Turnflag
- SemaphoreHandle_t TurnSem;
- //Sem that hold linefinder task
- SemaphoreHandle_t FindSem;
- //Sem that holds linefollower task
- SemaphoreHandle_t GoSem;
- //For motor speed task
- SemaphoreHandle_t speedSem;
- //For line sensor task
- SemaphoreHandle_t LineSem;
- //For ultra sonic task
- SemaphoreHandle_t UltSonSem;
- //For count
- SemaphoreHandle_t CountSem;
- //For avoid
- SemaphoreHandle_t AvoidSem;
- //For strike count
- SemaphoreHandle_t StrikeSem;
- char c1 = 'a';
- char c2 = 'b';
- char c3 = 'c';
- char c4 = 'd';
- char c5 = 'e';
- char c6 = 'f';
- char c7 = 'g';
- char c8 = 'h';
- /*Interupt processer som behövs för att hantera positioner i motorerna
- använd alltid dessa när motorerna används */
- void isr_process_encoder1(void)
- {
- if (digitalRead(Encoder_1.getPortB()) == 0)
- Encoder_1.pulsePosMinus();
- else
- Encoder_1.pulsePosPlus();
- }
- void isr_process_encoder2(void)
- {
- if (digitalRead(Encoder_2.getPortB()) == 0)
- Encoder_2.pulsePosMinus();
- else
- Encoder_2.pulsePosPlus();
- }
- //Sensor ports
- MeLineFollower lineFinder(PORT_6);//Linefollower
- MeUltrasonicSensor ultraSensor(PORT_7);//Obstecle sensor
- //***********************************************************************************************************************************
- //***********************************************************************************************************************************
- //Strike count task
- void StrikeCount(void *pvParameters)
- {
- if (xSemaphoreTake(StrikeSem, 0) == pdTRUE)
- {
- switch(sensorState)
- {
- //Strike gets +1 if outside of line for to long
- case S1_OUT_S2_OUT:
- strike++;
- break;
- //Strike resets if black line is found again
- default:
- strike=0;
- break;
- }
- xSemaphoreGive(StrikeSem);
- }
- }
- //***********************************************************************************************************************************
- //***********************************************************************************************************************************
- //Store Linesensor task
- void StoreLineSensor(void *pvParameters)
- {
- if (xSemaphoreTake(LineSem, 0) == pdTRUE)
- {
- sensorState = lineFinder.readSensors();
- xSemaphoreGive(LineSem);
- }
- }
- //***********************************************************************************************************************************
- //***********************************************************************************************************************************
- //Store ultsensor task
- void StoreUltSensor(void *pvParameters)
- {
- if (xSemaphoreTake(UltSonSem, 0) == pdTRUE)
- {
- if (ultraSensor.distanceCm() <= AvoidDist)
- stay=true;
- else
- stay=false;
- xSemaphoreGive(UltSonSem);
- }
- }
- //***********************************************************************************************************************************
- //***********************************************************************************************************************************
- //Speed set task
- void TaskSetSpeed(void *pvParameters)
- {
- if (xSemaphoreTake(speedSem, 3) == pdTRUE)
- {
- Encoder_1.setMotorPwm(-speedRight);
- Encoder_2.setMotorPwm(speedLeft);
- xSemaphoreGive(speedSem);
- }
- }
- //***********************************************************************************************************************************
- //***********************************************************************************************************************************
- //Speed set task
- void TaskCount(void *pvParameters)
- {
- if(xSemaphoreTake(CountSem,3)== pdTRUE)
- {
- if(xSemaphoreTake(FlagSem,3)== pdTRUE)
- {
- if(flag==0)
- count++;
- else
- count--;
- xSemaphoreGive(FlagSem);
- }
- xSemaphoreGive(CountSem);
- }
- }
- //***********************************************************************************************************************************
- //***********************************************************************************************************************************
- //Line follower task
- void TaskFollowLine(void *pvParameters)
- {
- if(xSemaphoreTake(GoSem,0)== pdTRUE)
- {
- //Flaga som säger hur roboten ska svänga när båda sensorna kommer ur bana
- static int turnflag;
- if (go)
- {
- count=0;
- if(!avoid)
- {
- if(xSemaphoreTake(StrikeSem, 5) == pdTRUE)
- {
- //Undvik om objekt förekommer
- if (stay)
- {
- go=false;
- avoid=true;
- }
- //If car lose line it will start linesearch algorithm again
- else if(strike>2000)
- {
- avoid=false;
- go=false;
- }
- else
- {
- switch (sensorState)
- {
- //Drive straight when both sensors are inside of black line
- case S1_IN_S2_IN:
- if (xSemaphoreTake(speedSem, 3) == pdTRUE)
- {
- speedLeft= 120;
- speedRight= 120;
- turnflag=0;
- xSemaphoreGive(speedSem);
- }
- break;
- //Turn slightly to the right if sensor 2 is outside black line
- case S1_IN_S2_OUT:
- if (xSemaphoreTake(speedSem, 3) == pdTRUE)
- {
- speedLeft= -120;
- speedRight= 120;
- turnflag=1;
- xSemaphoreGive(speedSem);
- }
- break;
- //Turn slightly to the left if sensor 1 is outside of black line
- case S1_OUT_S2_IN:
- if (xSemaphoreTake(speedSem, 3) == pdTRUE)
- {
- speedLeft= 120;
- speedRight= - 120;
- turnflag=2;
- xSemaphoreGive(speedSem);
- }
- break;
- //Booth in white extra turn or back
- case S1_OUT_S2_OUT:
- switch (turnflag)
- {
- case 0:
- if (xSemaphoreTake(speedSem, 3) == pdTRUE)
- {
- speedLeft= - 90;
- speedRight= - 90;
- xSemaphoreGive(speedSem);
- }
- break;
- case 1:
- if (xSemaphoreTake(speedSem, 3) == pdTRUE)
- {
- speedLeft= -160;
- speedRight= 160;
- xSemaphoreGive(speedSem);
- }
- break;
- case 2:
- if (xSemaphoreTake(speedSem, 3) == pdTRUE)
- {
- speedLeft= 160;
- speedRight= - 160;
- xSemaphoreGive(speedSem);
- }
- break;
- default:break;
- }
- break;
- default: break;
- }
- }
- xSemaphoreGive(StrikeSem);
- }
- }
- }
- xSemaphoreGive(GoSem);
- }
- }
- //***********************************************************************************************************************************
- //***********************************************************************************************************************************
- //Object avoidence task
- void TaskAvoidObject(void *pvParameters)
- {
- if(xSemaphoreTake(AvoidSem,0)== pdTRUE)
- {
- if(avoid)
- {
- if (xSemaphoreTake(StrikeSem, 5) == pdTRUE)
- {
- if (stay)
- {
- if (xSemaphoreTake(speedSem, 5) == pdTRUE)
- {
- speedLeft = -150;
- speedRight = 150;
- xSemaphoreGive(speedSem);
- }
- delay(800);
- }
- else if(strike>5000)
- {
- avoid=false;
- go=false;
- }
- else
- {
- switch (sensorState)
- {
- case S1_IN_S2_IN:
- if (xSemaphoreTake(speedSem, 5) == pdTRUE)
- {
- speedLeft = -180;
- speedRight = 180;
- xSemaphoreGive(speedSem);
- }
- delay(200);
- go=true;
- avoid=false;
- break;
- default:
- if (xSemaphoreTake(speedSem, 5) == pdTRUE)
- {
- speedLeft = 150;
- speedRight = 90;
- xSemaphoreGive(speedSem);
- }
- break;
- }
- }
- xSemaphoreGive(StrikeSem);
- }
- }
- xSemaphoreGive(AvoidSem);
- }
- }
- //***********************************************************************************************************************************
- //***********************************************************************************************************************************
- //Find the black line task!
- void TaskFindLine(void *pvParameters)
- {
- if(xSemaphoreTake(FindSem,0)== pdTRUE)
- {
- if (!go)
- {
- if (stay)
- {
- //Stop and back up
- if(xSemaphoreTake(FlagSem,5)== pdTRUE)
- {
- flag = 1;
- xSemaphoreGive(FlagSem);
- }
- //Start back up
- if (xSemaphoreTake(speedSem, 5) == pdTRUE)
- {
- speedLeft = -80;
- speedRight = -80;
- xSemaphoreGive(speedSem);
- }
- //start counting down
- }
- else
- {
- switch (sensorState)
- {
- //Stop when both sensors are inside of black line
- case S1_IN_S2_IN:
- //Turn slightly when line is found
- if (xSemaphoreTake(speedSem, 5) == pdTRUE)
- {
- speedLeft=-150;
- speedRight=150;
- xSemaphoreGive(speedSem);
- }
- //Set go bool to true
- go = true;
- delay(100);
- break;
- //Turn slightly to the left if sensor 2 is outside black line
- case S1_IN_S2_OUT:
- if (xSemaphoreTake(speedSem, 5) == pdTRUE)
- {
- //Turn to the left
- speedLeft=-150;
- speedRight=150;
- xSemaphoreGive(speedSem);
- }
- go = true;
- delay(100);
- break;
- //Turn slightly to the left if sensor 1 is outside of black line
- case S1_OUT_S2_IN:
- if (xSemaphoreTake(speedSem, 5) == pdTRUE)
- {
- //Turn to the left
- speedLeft=150;
- speedRight=-150;
- xSemaphoreGive(speedSem);
- }
- //Set go bool to true
- go = true;
- delay(100);
- break;
- default:
- if(xSemaphoreTake(FlagSem,5)== pdTRUE)
- {
- if (flag == 0)
- {
- if(xSemaphoreTake(CountSem,5)== pdTRUE)
- {
- if (count <= 3500)
- {
- if (xSemaphoreTake(speedSem, 5) == pdTRUE)
- {
- //Kör frammåt leta efter bana
- speedLeft=90;
- speedRight=90;
- xSemaphoreGive(speedSem);
- }
- }
- else
- {
- flag = 1;
- }
- xSemaphoreGive(CountSem);
- }
- }
- else
- {
- if(xSemaphoreTake(CountSem,5)== pdTRUE)
- {
- if (count >= 0)
- {
- if (xSemaphoreTake(speedSem, 5) == pdTRUE)
- {
- speedLeft=-90;
- speedRight=-90;
- xSemaphoreGive(speedSem);
- }
- }
- else
- {
- flag = 0;
- if (xSemaphoreTake(speedSem, 5) == pdTRUE)
- {
- speedLeft=-150;
- speedRight=150;
- xSemaphoreGive(speedSem);
- }
- delay(500); //Svänger för att testa om bana finns drygt 45 grader åt vänster
- }
- xSemaphoreGive(CountSem);
- }
- }
- //Release flag sem
- xSemaphoreGive(FlagSem);
- }
- break;
- }
- }
- }
- xSemaphoreGive(FindSem);
- }
- }
- //***********************************************************************************************************************************
- //***********************************************************************************************************************************
- void setup()
- {
- attachInterrupt(Encoder_1.getIntNum(), isr_process_encoder1, RISING);
- attachInterrupt(Encoder_2.getIntNum(), isr_process_encoder2, RISING);
- Serial.begin(9600); // Initiating Serial port bps needs to be the same as parameter in IDE Serial monitor.
- vSchedulerInit(); // Initiate the scheduler.
- //Set PWM 8KHz (sätter modulationsfrekvensen inkludera alltid med motorn)
- TCCR1A = _BV(WGM10);
- TCCR1B = _BV(CS11) | _BV(WGM12);
- TCCR2A = _BV(WGM21) | _BV(WGM20);
- TCCR2B = _BV(CS21);
- //Create the semaphores
- CountSem=xSemaphoreCreateMutex();
- FlagSem=xSemaphoreCreateMutex();
- TurnSem=xSemaphoreCreateMutex();
- FindSem = xSemaphoreCreateMutex();
- GoSem = xSemaphoreCreateMutex();
- speedSem = xSemaphoreCreateMutex();
- LineSem = xSemaphoreCreateMutex();
- UltSonSem = xSemaphoreCreateMutex();
- CountSem = xSemaphoreCreateMutex();
- AvoidSem = xSemaphoreCreateMutex();
- StrikeSem = xSemaphoreCreateMutex();
- /* Creates a periodic task.(TaskBlip: The task function.
- "t2": Name of the task.
- configMINIMAL_STACK_SIZE: Stack size of the task in words, not bytes.
- &c1: Parameters to the task function.
- 1: Priority of the task. (Only used when scheduling policy is set to
- manual)
- &xHandle1: Pointer to the task handle.
- pdMS_TO_TICKS(0): Phase given in software ticks. Counted from when
- vSchedulerStart is called. pdMS_TO_TICKS() converts microseconds to ticks.
- pdMS_TO_TICKS(100): Period given in software ticks.
- pdMS_TO_TICKS(100): Worst-case execution time given in software ticks.
- pdMS_TO_TICKS(100): Relative deadline given in software ticks.):
- * */
- //Ultra sonic task
- vSchedulerPeriodicTaskCreate(StoreUltSensor,
- "t1",
- configMINIMAL_STACK_SIZE,
- &c1,
- 1,
- &xHandle1,
- pdMS_TO_TICKS(0),
- pdMS_TO_TICKS(10),
- pdMS_TO_TICKS(10),
- pdMS_TO_TICKS(10));
- //Get line task
- vSchedulerPeriodicTaskCreate(StoreLineSensor,
- "t2",
- configMINIMAL_STACK_SIZE,
- &c2,
- 2,
- &xHandle2,
- pdMS_TO_TICKS(0),
- pdMS_TO_TICKS(10),
- pdMS_TO_TICKS(10),
- pdMS_TO_TICKS(10));
- // Motor task
- vSchedulerPeriodicTaskCreate(TaskSetSpeed,
- "t3",
- configMINIMAL_STACK_SIZE,
- &c3,
- 3,
- &xHandle3,
- pdMS_TO_TICKS(0),
- pdMS_TO_TICKS(10),
- pdMS_TO_TICKS(10),
- pdMS_TO_TICKS(10));
- // Object avoidence task
- vSchedulerPeriodicTaskCreate(TaskAvoidObject,
- "t4",
- configMINIMAL_STACK_SIZE,
- &c4,
- 4,
- &xHandle4,
- pdMS_TO_TICKS(0),
- pdMS_TO_TICKS(10),
- pdMS_TO_TICKS(10),
- pdMS_TO_TICKS(10));
- //Line follow task
- vSchedulerPeriodicTaskCreate(TaskFollowLine,
- "t5",
- configMINIMAL_STACK_SIZE,
- &c5,
- 5,
- &xHandle5,
- pdMS_TO_TICKS(0),
- pdMS_TO_TICKS(10),
- pdMS_TO_TICKS(10),
- pdMS_TO_TICKS(10));
- //Line finder task
- vSchedulerPeriodicTaskCreate(TaskFindLine,
- "t6",
- configMINIMAL_STACK_SIZE,
- &c6,
- 6,
- &xHandle6,
- pdMS_TO_TICKS(0),
- pdMS_TO_TICKS(10),
- pdMS_TO_TICKS(10),
- pdMS_TO_TICKS(10));
- //Count task
- vSchedulerPeriodicTaskCreate(TaskCount,
- "t7",
- configMINIMAL_STACK_SIZE,
- &c7,
- 7,
- &xHandle7,
- pdMS_TO_TICKS(0),
- pdMS_TO_TICKS(10),
- pdMS_TO_TICKS(10),
- pdMS_TO_TICKS(10));
- //Count task
- vSchedulerPeriodicTaskCreate(StrikeCount,
- "t8",
- configMINIMAL_STACK_SIZE,
- &c8,
- 8,
- &xHandle8,
- pdMS_TO_TICKS(0),
- pdMS_TO_TICKS(10),
- pdMS_TO_TICKS(10),
- pdMS_TO_TICKS(10));
- vSchedulerStart(); // Start the program.
- }
- //***********************************************************************************************************************************
- //***********************************************************************************************************************************
- //DONT CARE
- void loop()
- {
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement