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
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
13 * Special thanks to Yu Yang <sog@lattebox.com>
\r
16 #include "kernel_id.h"
\r
17 #include "ecrobot_interface.h"
\r
20 /* OSEK declarations */
\r
23 /* NXT Sensor port configration */
\r
24 #define NXT_PORT NXT_PORT_S3
\r
26 /* LATTEBOX NXTe channel configuration */
\r
27 #define CH1 1 /* channel 0 to 3 */
\r
29 /* LATTEBOX LSC servo channel configuration */
\r
30 #define SERVO_CH 0 /* servo channels: 0 to 21 */
\r
32 /* LEJOS OSEK hooks */
\r
33 void ecrobot_device_initialize()
\r
35 ecrobot_init_i2c(NXT_PORT, LOWSPEED_9V);
\r
38 void ecrobot_device_terminate()
\r
40 ecrobot_term_i2c(NXT_PORT);
\r
43 void user_1ms_isr_type2(void){}
\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
53 display_goto_xy(0, 0);
\r
54 display_string("NXTe22 Test");
\r
58 if (ecrobot_is_RUN_button_pressed())
\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
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
70 if (ecrobot_is_ENTER_button_pressed())
\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
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