nxtOSEK/samples/nxte22/nxte22_test.c

   1 /**\r
   2  *  nxte_test.c \r
   3  * \r
   4  * This is a sample program for LATTEBOX NXTe(NXT Extension Kit Product)\r
   5  * and LSC22(22-Axis Servo Kit Product)\r
   6  * <http://www.lattebox.com/product_list_e.php?pts_type=1>\r
   7  * \r
   8  * It enables a NXT to control RC servo motors via I2C.\r
   9  * Maximum number of RC servos to be controlled is:\r
  10  * Number of NXT Sensor ports(4) * Number of NXTe channels (4) * Number of LSC22 channels (22)\r
  11  * = 352 RC Servos might be controlled by using a NXT!\r
  12  * \r
  13  * Special thanks to Yu Yang <sog@lattebox.com>\r
  14  */ \r
  15 #include "kernel.h"\r
  16 #include "kernel_id.h"\r
  17 #include "ecrobot_interface.h"\r
  18 #include "nxte22.h"\r
  19 \r
  20 /* OSEK declarations */\r
  21 DeclareTask(Task1);\r
  22 \r
  23 /* NXT Sensor port configration */\r
  24 #define NXT_PORT  NXT_PORT_S3\r
  25 \r
  26 /* LATTEBOX NXTe channel configuration */\r
  27 #define CH1 1 /* channel 0 to 3 */\r
  28 \r
  29 /* LATTEBOX LSC servo channel configuration */  \r
  30 #define SERVO_CH 0 /* servo channels: 0 to 21 */\r
  31 \r
  32 /* LEJOS OSEK hooks */\r
  33 void ecrobot_device_initialize()\r
  34 {\r
  35         ecrobot_init_i2c(NXT_PORT, LOWSPEED_9V);\r
  36 }\r
  37 \r
  38 void ecrobot_device_terminate()\r
  39 {\r
  40         ecrobot_term_i2c(NXT_PORT);\r
  41 }\r
  42 \r
  43 void user_1ms_isr_type2(void){}\r
  44 \r
  45 \r
  46 TASK(Task1)\r
  47 {\r
  48         init_NXTe22(NXT_PORT);\r
  49         sync_NXTe22Servo(NXT_PORT, NXTe_CH[CH1]);\r
  50         load_NXTe22Servo(NXT_PORT, NXTe_CH[CH1], ALL_22SERVO_ENABLE);\r
  51         set_NXTe22Delay(NXT_PORT, NXTe_CH[CH1], SERVO_CH, 0); \r
  52 \r
  53         display_goto_xy(0, 0);\r
  54         display_string("NXTe22 Test");\r
  55         display_update();\r
  56         while(1)\r
  57         {\r
  58                 if (ecrobot_is_RUN_button_pressed())\r
  59                 {\r
  60                         set_NXTe22Angle(NXT_PORT, NXTe_CH[CH1], SERVO_CH, MAX_ANGLE);\r
  61                         while(read_NXTe22Motion(NXT_PORT, NXTe_CH[CH1]) != 0);\r
  62                         display_clear(0);\r
  63                         display_goto_xy(0, 0);\r
  64                         display_string("NXTe22 Test");\r
  65                         display_goto_xy(0, 1);\r
  66                         display_int(get_NXTe22Angle(NXT_PORT, NXTe_CH[CH1], SERVO_CH), 0);\r
  67                         display_update();\r
  68                 }\r
  69                 \r
  70                 if (ecrobot_is_ENTER_button_pressed())\r
  71                 {\r
  72                         set_NXTe22Angle(NXT_PORT, NXTe_CH[CH1], SERVO_CH, MIN_ANGLE);\r
  73                         while(read_NXTe22Motion(NXT_PORT, NXTe_CH[CH1]) != 0);\r
  74                         display_clear(0);\r
  75                         display_goto_xy(0, 0);\r
  76                         display_string("NXTe22 Test");\r
  77                         display_goto_xy(0, 1);\r
  78                         display_int(get_NXTe22Angle(NXT_PORT, NXTe_CH[CH1], SERVO_CH), 0);\r
  79                         display_update();\r
  80                 }\r
  81         }       \r
  82         \r
  83         TerminateTask();\r
  84 }\r