nxtOSEK/samples/biped_robot/biped_robot.c

   1 /**\r
   2  *  biped_robot.c \r
   3  * \r
   4  * This is a sample program for LATTEBOX NXTe/LSC based biped robot.\r
   5  * \r
   6  * Special thanks to Yu Yang <sog@lattebox.com>\r
   7  */ \r
   8 #include "kernel.h"\r
   9 #include "kernel_id.h"\r
  10 \r
  11 #include "ecrobot_interface.h"\r
  12 #include "sleep.h"\r
  13 #include "motion.h"\r
  14 \r
  15 /* OSEK declarations */\r
  16 DeclareTask(Task_Init);\r
  17 DeclareTask(Task_Commander);\r
  18 DeclareTask(Task_Display);\r
  19 DeclareTask(Task_MotionControl);\r
  20 DeclareResource(ResourceCommand);\r
  21 DeclareCounter(SysTimerCnt);\r
  22 \r
  23 /* NXT Sensor port configration */\r
  24 #define NXTe_PORT  NXT_PORT_S3\r
  25 \r
  26 /* robot motion command */\r
  27 static motionCmd_t motionCmd;\r
  28 \r
  29 /*=============================================================================\r
  30  * nxtOSEK system hook functions\r
  31  */\r
  32 void ecrobot_device_initialize()\r
  33 {\r
  34         ecrobot_init_i2c(NXTe_PORT, LOWSPEED_9V);\r
  35         ecrobot_init_bt_slave("nxtOSEK");\r
  36 }\r
  37 \r
  38 void ecrobot_device_terminate()\r
  39 {\r
  40         ecrobot_term_i2c(NXTe_PORT);\r
  41         ecrobot_term_bt_connection();\r
  42 }\r
  43 \r
  44 void user_1ms_isr_type2(void)\r
  45 {\r
  46         StatusType ercd;\r
  47 \r
  48         ercd = SignalCounter(SysTimerCnt); /* Increment OSEK Alarm Counter */\r
  49         if(ercd != E_OK)\r
  50         {\r
  51                 ShutdownOS(ercd);\r
  52         }\r
  53   \r
  54     check_sleepers(); /* check on sleeping Tasks */\r
  55 }\r
  56 \r
  57 /*=============================================================================\r
  58  * User Tasks go here\r
  59  */\r
  60 \r
  61 motionCmd_t getMotionCommand(S8 in1, S8 in2)\r
  62 {\r
  63         motionCmd_t cmd = STAND_UP; /* default command */\r
  64         \r
  65         if (in1 == 0)\r
  66         {\r
  67                 if (in2 > 0)\r
  68                 {\r
  69                         cmd = RIGHT_TURN;\r
  70                 }\r
  71                 else if (in2 < 0)\r
  72                 {\r
  73                         cmd = LEFT_TURN;\r
  74                 }\r
  75         }\r
  76         else if (in1 > 0)\r
  77         {\r
  78                 if (in2 == 0)\r
  79                 {\r
  80                         cmd = FORWARD;\r
  81                 }\r
  82         }\r
  83 \r
  84         return cmd;\r
  85 } \r
  86 \r
  87 /**\r
  88  * Initialize application\r
  89  */\r
  90 TASK(Task_Init)\r
  91 {\r
  92         initServo(NXTe_PORT);\r
  93         motionCmd = STAND_UP; /* default motion */\r
  94         \r
  95         TerminateTask();\r
  96 }\r
  97 \r
  98 /**\r
  99  * Receive Bluetooth message and publish a motion command.\r
 100  * This command decoder is assumed to be use for NXT GamePad.\r
 101  * NXT GamePad sends two user inputs \r
 102  * - packet0: analog left stick input in vertical direction\r
 103  *   (upper max to lower max = -100 to 100)\r
 104  * - packet1: analog right stick input in horizontal direction\r
 105  *   (left max to right max = -100 to 100) \r
 106  */\r
 107 TASK(Task_Commander)\r
 108 {\r
 109         U8 bt_receive_buf[32]; /* buffer size is fixed as 32 */ \r
 110 \r
 111         /* read Bluetooth Rx data */\r
 112         if (ecrobot_read_bt_packet(bt_receive_buf, 32))\r
 113         {\r
 114                 GetResource(ResourceCommand);\r
 115                 motionCmd = getMotionCommand(-(*(S8 *)(&bt_receive_buf[0])), (*(S8 *)(&bt_receive_buf[1])));\r
 116                 ReleaseResource(ResourceCommand);\r
 117         }\r
 118 \r
 119         TerminateTask();\r
 120 }\r
 121 \r
 122 /**\r
 123  * Display NXT standard sensors/motors/internal status information.\r
 124  */\r
 125 TASK(Task_Display)\r
 126 {\r
 127         ecrobot_status_monitor("BIPED ROBOT");\r
 128 \r
 129         TerminateTask();\r
 130 }\r
 131 \r
 132 /**\r
 133  * Send angle data to NXTe according to motion command.\r
 134  * This Task should be invoked as a background Task.\r
 135  */\r
 136 TASK(Task_MotionControl)\r
 137 {\r
 138         motionCmd_t cmd;\r
 139 \r
 140         GetResource(ResourceCommand);\r
 141         cmd = motionCmd; /* latch motion command */\r
 142         ReleaseResource(ResourceCommand);\r
 143 \r
 144         /* set a robot motion */\r
 145         setMotion(NXTe_PORT, cmd);\r
 146 \r
 147         /* back ground task should end like this unless it's a one-shot task */\r
 148     TaskType t;\r
 149     GetTaskID(&t);\r
 150     ChainTask(t);\r
 151 }\r