nxtOSEK/samples_c++/turtlebot/avoid.cc

   1 //\r
   2 // avoid.cc\r
   3 //\r
   4 // Test of C++ and LEJOS-OSEK with TurtleBot II\r
   5 //\r
   6 // Written 20-jan-2008 by rwk\r
   7 //\r
   8 // Copyright 2007, 2008 by Takashi Chikamasa and Robert W. Kramer\r
   9 //\r
  10 \r
  11 #include <Motor.h>\r
  12 #include <SonarSensor.h>\r
  13 \r
  14 //\r
  15 // To properly interface with LEJOS-OSEK, we need to use C linkage between our\r
  16 // code and LEJOS-OSEK\r
  17 //\r
  18 \r
  19 extern "C" {\r
  20 \r
  21   #include "ecrobot_interface.h"\r
  22 \r
  23   // Port configration for Sensor and Motor\r
  24 \r
  25   Motor leftMotor(NXT_PORT_A);\r
  26   Motor rightMotor(NXT_PORT_B);\r
  27   SonarSensor sonar(NXT_PORT_S1);\r
  28 \r
  29   //\r
  30   // Main task (thread) declarations\r
  31   //\r
  32   // You need one DeclareTask( ) per task/thread\r
  33   //\r
  34 \r
  35   DeclareTask(LeftMotorTask);\r
  36   DeclareTask(RightMotorTask);\r
  37   DeclareTask(SonarTask);\r
  38 \r
  39   //\r
  40   // LEJOS OSEK callback hooks\r
  41   //\r
  42 \r
  43   void ecrobot_device_initialize(void)\r
  44   {\r
  45     sonar.activate();\r
  46   }\r
  47 \r
  48   void ecrobot_device_terminate(void)\r
  49   {\r
  50     sonar.passivate();\r
  51   }\r
  52 \r
  53   //===========================================================================\r
  54   // User tasks go here.\r
  55   //\r
  56 \r
  57   enum { FORWARD,BACKWARD,LEFT_TURN,RIGHT_TURN,STOP };\r
  58 \r
  59   int robotAction = FORWARD;\r
  60 \r
  61   void showMessage(char* msg)\r
  62   {\r
  63         display_clear(0);\r
  64         display_goto_xy(0, 0);\r
  65         display_string(msg);\r
  66         display_update();\r
  67   }\r
  68 \r
  69   TASK(LeftMotorTask)\r
  70   {\r
  71 \r
  72     leftMotor.setPower(75);\r
  73 \r
  74     if (robotAction == FORWARD || robotAction == RIGHT_TURN)\r
  75     {\r
  76       leftMotor.forward();\r
  77     }\r
  78     else if (robotAction != STOP)\r
  79     {\r
  80       leftMotor.backward();\r
  81     }\r
  82     else\r
  83     {\r
  84       leftMotor.flt();\r
  85     }\r
  86 \r
  87     //\r
  88     // your task should end like this unless it's a one-shot task\r
  89     //\r
  90 \r
  91     TaskType t;\r
  92     GetTaskID(&t);\r
  93     ChainTask(t);\r
  94   }\r
  95 \r
  96   TASK(RightMotorTask)\r
  97   {\r
  98 \r
  99     rightMotor.setPower(75);\r
 100 \r
 101     if (robotAction == FORWARD || robotAction == LEFT_TURN)\r
 102     {\r
 103       rightMotor.forward();\r
 104     }\r
 105     else if (robotAction != STOP)\r
 106     {\r
 107       rightMotor.backward();\r
 108     }\r
 109     else\r
 110     {\r
 111       rightMotor.flt();\r
 112     }\r
 113 \r
 114     //\r
 115     // your task should end like this unless it's a one-shot task\r
 116     //\r
 117 \r
 118     TaskType t;\r
 119     GetTaskID(&t);\r
 120     ChainTask(t);\r
 121   }\r
 122 \r
 123   TASK(SonarTask)\r
 124   {\r
 125     int d = sonar.getDistance();\r
 126 \r
 127     if (d < 18)\r
 128     {\r
 129       robotAction = STOP;\r
 130       showMessage("STOP");\r
 131 \r
 132       sleep(250);\r
 133 \r
 134       robotAction = BACKWARD;\r
 135       showMessage("BACKWARD");\r
 136       \r
 137       sleep(500);\r
 138 \r
 139       robotAction = STOP;\r
 140       showMessage("STOP");\r
 141 \r
 142       sleep(250);\r
 143 \r
 144       robotAction = LEFT_TURN;\r
 145       showMessage("LEFT_TURN");\r
 146 \r
 147       sleep(750);\r
 148 \r
 149       robotAction = STOP;\r
 150       showMessage("STOP");\r
 151 \r
 152       sleep(250);\r
 153 \r
 154       d = sonar.getDistance();\r
 155 \r
 156       robotAction = RIGHT_TURN;\r
 157       showMessage("RIGHT_TURN");\r
 158 \r
 159       sleep(1500);\r
 160 \r
 161       robotAction = STOP;\r
 162       showMessage("STOP");\r
 163 \r
 164       sleep(250);\r
 165 \r
 166       if (d > sonar.getDistance())\r
 167       {\r
 168                 robotAction = LEFT_TURN;\r
 169         showMessage("LEFT_TURN");\r
 170 \r
 171                 sleep(1500);\r
 172 \r
 173                 robotAction = STOP;\r
 174             showMessage("STOP");\r
 175 \r
 176                 sleep(250);\r
 177       }\r
 178     }\r
 179 \r
 180     robotAction = FORWARD;\r
 181     showMessage("FORWARD");\r
 182 \r
 183 \r
 184     //\r
 185     // your task should end like this unless it's a one-shot task\r
 186     //\r
 187 \r
 188     TaskType t;\r
 189     GetTaskID(&t);\r
 190     ChainTask(t);\r
 191   }\r
 192         \r
 193 }\r