nxtOSEK/samples_c/hitechtest/hitechtest.c

   1 /* helloworld.c */ \r
   2 #include "kernel.h"\r
   3 #include "kernel_id.h"\r
   4 #include "ecrobot_interface.h"\r
   5 \r
   6 /* OSEK declarations */\r
   7 DeclareCounter(SysTimerCnt);\r
   8 DeclareTask(Task1);\r
   9 \r
  10 /* LEJOS OSEK hooks */\r
  11 void ecrobot_device_initialize()\r
  12 {\r
  13         ecrobot_init_accel_sensor(NXT_PORT_S3);\r
  14 }\r
  15 void ecrobot_device_terminate()\r
  16 {\r
  17         ecrobot_term_accel_sensor(NXT_PORT_S3);\r
  18 }\r
  19 \r
  20 /* LEJOS OSEK hook to be invoked from an ISR in category 2 */\r
  21 void user_1ms_isr_type2(void)\r
  22 {\r
  23   StatusType ercd;\r
  24 \r
  25   ercd = SignalCounter(SysTimerCnt); /* Increment OSEK Alarm Counter */\r
  26   if(ercd != E_OK)\r
  27   {\r
  28     ShutdownOS(ercd);\r
  29   }\r
  30 }\r
  31 \r
  32 /* Task1 executed every 50msec */\r
  33 TASK(Task1)\r
  34 {\r
  35         U16 yaw_rate;\r
  36         S16 accel_data[3];\r
  37         \r
  38         yaw_rate   = ecrobot_get_gyro_sensor(NXT_PORT_S1);\r
  39         ecrobot_get_accel_sensor(NXT_PORT_S3, accel_data);\r
  40 \r
  41         display_clear(0);\r
  42 \r
  43         display_goto_xy(0, 0);\r
  44         display_string("HiTech Test");\r
  45 \r
  46         display_goto_xy(0, 1);\r
  47         display_string("YAW:    ");\r
  48         display_int(yaw_rate, 0);\r
  49 \r
  50         display_goto_xy(0, 2);\r
  51         display_string("X ACCEL:");\r
  52         display_int(accel_data[0], 0);\r
  53 \r
  54         display_goto_xy(0, 3);\r
  55         display_string("Y ACCEL:");\r
  56         display_int(accel_data[1], 0);\r
  57 \r
  58         display_goto_xy(0, 4);\r
  59         display_string("Z ACCEL:");\r
  60         display_int(accel_data[2], 0);\r
  61 \r
  62         display_update();\r
  63 \r
  64         TerminateTask();\r
  65 }\r