Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- /*
- * ROBOTAR.C -- A simple RoboTar program with uCOS-II
- * Written by: Rosbi Mamat 6/5/2014
- */
- #include "..\ucosobj\kernel.h" /* Always include these to use uCOS-II */
- #include "..\hal\hal_robo.h" /* and RoboTar HAL */
- #define TASK_STK_SZ 128 /* Size of each task's stacks (# of bytes) */
- #define TASK_START_PRIO 1 /* Highest priority */
- #define TASK_CHKCOLLIDE_PRIO 3
- #define TASK_CTRLMOTOR_PRIO 4
- #define TASK_CRUISE_PRIO 5 /* Lowest priority */
- #define TASK_LIGHT_PHOBIA 2
- OS_STK CtrlmotorStk[TASK_STK_SZ]; /* Task CtrlMotors stack */
- OS_STK CruiseStk[TASK_STK_SZ]; /* Task NavigRobot stack */
- OS_STK TaskStartStk[TASK_STK_SZ]; /* TaskStartTask stack */
- OS_STK ChkCollideStk[TASK_STK_SZ]; /* Task StopOnCollide stack */
- OS_STK LightPhobiaStk[TASK_STK_SZ];
- /* ------ Global shared variable -------*/
- /* Ideally, this should be protected by a semaphore etc */
- struct robostate
- {
- int rspeed; /* right motor speed (-100 -- +100) */
- int lspeed; /* leftt motor speed (-100 -- +100) */
- char collided; /* collision? 1 = yes, 0 = no */
- char light_side;
- } myrobot;
- /*------High pririority task----------*/
- void CheckCollision (void *data)
- {
- for(;;)
- {
- if ( (robo_bumpSensorR() == HIT) || /* collide ? */
- (robo_bumpSensorL() == HIT) )
- {
- myrobot.collided = 1; /* signal that collisioin */
- }
- else
- {
- myrobot.collided = 0; /* signal no collisioin */
- }
- OSTimeDlyHMSM(0, 0, 0, 100); /* Task period ~ 100 ms */
- }
- }
- /* Lowest Priority Task - Light Chase Task*/
- void LightPhobia (void *data)
- {
- int r_light;
- int l_light;
- int light_diff;
- for(;;)
- {
- r_light = robo_lightSensorR();
- l_light = robo_lightSensorL();
- light_diff = r_light - l_light;
- //TURN RIGHT
- if (r_light > 400 && l_light > 400 && light_diff > 30)
- {
- myrobot.light_side = 2;
- }
- //TURN LEFT
- else if (r_light > 400 && l_light > 400 && light_diff < -30)
- {
- myrobot.light_side = 1;
- }
- else //if (light_diff >= -30 && light_diff <= 30)
- {
- myrobot.light_side = 0;
- }
- OSTimeDlyHMSM(0, 0, 0, 200); /* Task period ~ 200 ms */
- }
- }
- /* Control robot Motors TASK */
- void CntrlMotors (void *data)
- {
- int speed_r, speed_l;
- for(;;)
- {
- speed_r = myrobot.rspeed;
- speed_l = myrobot.lspeed;
- robo_motorSpeed(speed_l, speed_r);
- OSTimeDlyHMSM(0, 0, 0, 200); /* Task period ~ 200 ms */
- }
- }
- /* -----------------------------------------------
- * Task for navigating robot
- * Write you own navigation task here
- */
- void Cruise (void *data)
- {
- int dist;
- for (;;)
- {
- dist = robo_distSensor(); /* Read distance sensor */
- if (myrobot.collided == 1) /* If collided then stop */
- {
- myrobot.rspeed = STOP_SPEED; /* STOP */
- myrobot.lspeed = STOP_SPEED;
- OSTimeDlyHMSM(0, 0, 0, 100); /* Wait 1 sec for stop to complete */
- myrobot.rspeed = -MEDIUM_SPEED; /* REVERSE */
- myrobot.lspeed = MEDIUM_SPEED;
- OSTimeDlyHMSM(0, 0, 2, 0); /* Wait 2 sec for turning right to complete */
- }
- else if(dist < 35)
- {
- robo_Honk();
- if(myrobot.light_side == 0 ){
- myrobot.rspeed = LOW_SPEED; /* turn right */
- myrobot.lspeed = -LOW_SPEED;
- OSTimeDlyHMSM(0, 0, 0, 200); /* Wait 200 ms for right turn to complete */
- }
- else if(myrobot.light_side == 1 ){
- myrobot.rspeed = -LOW_SPEED; /* turn right */
- myrobot.lspeed = LOW_SPEED;
- OSTimeDlyHMSM(0, 0, 0, 200); /* Wait 200 ms for right turn to complete */
- }
- else if(myrobot.light_side == 2 ){
- myrobot.rspeed = LOW_SPEED; /* turn left */
- myrobot.lspeed = -LOW_SPEED;
- OSTimeDlyHMSM(0, 0, 0, 200); /* Wait 200 ms for right turn to complete */
- }
- }
- else
- {
- myrobot.rspeed = HIGH_SPEED; /* move forward at straight */
- myrobot.lspeed = HIGH_SPEED;
- }
- OSTimeDlyHMSM(0, 0, 0, 100); /* Task period ~ 100 ms */
- }
- }
- /*------Highest pririority task----------*/
- /* Create all other tasks here */
- void TaskStart( void *data )
- {
- OS_ticks_init(); /*-enable RTOS timer tick */
- OSTaskCreate(LightPhobia, /*-Task function */
- (void *)0, /*-nothing passed to task */
- (void *)&LightPhobiaStk[TASK_STK_SZ - 1], /*-stack allocated to task */
- TASK_LIGHT_PHOBIA); /*-priority of task */
- OSTaskCreate(CheckCollision, /*-Task function */
- (void *)0, /*-nothing passed to task */
- (void *)&ChkCollideStk[TASK_STK_SZ - 1], /*-stack allocated to task */
- TASK_CHKCOLLIDE_PRIO); /*-priority of task */
- OSTaskCreate(CntrlMotors, /*-Task function */
- (void *)0, /*-nothing passed to task */
- (void *)&CtrlmotorStk[TASK_STK_SZ - 1], /*-stack allocated to task */
- TASK_CTRLMOTOR_PRIO); /*-priority of task */
- OSTaskCreate(Cruise, /*-Task function */
- (void *)0, /*-nothing passed to task */
- (void *)&CruiseStk[TASK_STK_SZ - 1], /*-stack allocated to task */
- TASK_CRUISE_PRIO); /*-priority of task */
- while(1)
- {
- OSTimeDlyHMSM(0, 0, 5, 0); /* Task period ~ 5 secs */
- robo_LED_toggle(); /* Show that we are alive */
- }
- }
- int main( void )
- {
- robo_Setup(); /* initialize HAL for robot */
- OSInit(); /* initialize UCOS-II kernel */
- robo_wait4goPress();
- robo_motorSpeed(STOP_SPEED, STOP_SPEED); /* Stop the robot */
- myrobot.rspeed = STOP_SPEED; /* Initialize myrobot states */
- myrobot.lspeed = STOP_SPEED;
- myrobot.collided = 0; /* No collisioin */
- myrobot.light_side = 0;
- OSTaskCreate(TaskStart, /* create TaskStart Task */
- (void *)0,
- (void *)&TaskStartStk[TASK_STK_SZ - 1],
- TASK_START_PRIO);
- OSStart(); /* Start multitasking */
- while (1); /* die here */
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement