4 * This is a sample program for LATTEBOX NXTe/LSC based biped robot.
\r
6 * Special thanks to Yu Yang <sog@lattebox.com>
\r
9 #include "kernel_id.h"
\r
11 #include "ecrobot_interface.h"
\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
23 /* NXT Sensor port configration */
\r
24 #define NXTe_PORT NXT_PORT_S3
\r
26 /* robot motion command */
\r
27 static motionCmd_t motionCmd;
\r
29 /*=============================================================================
\r
30 * nxtOSEK system hook functions
\r
32 void ecrobot_device_initialize()
\r
34 ecrobot_init_i2c(NXTe_PORT, LOWSPEED_9V);
\r
35 ecrobot_init_bt_slave("nxtOSEK");
\r
38 void ecrobot_device_terminate()
\r
40 ecrobot_term_i2c(NXTe_PORT);
\r
41 ecrobot_term_bt_connection();
\r
44 void user_1ms_isr_type2(void)
\r
48 ercd = SignalCounter(SysTimerCnt); /* Increment OSEK Alarm Counter */
\r
54 check_sleepers(); /* check on sleeping Tasks */
\r
57 /*=============================================================================
\r
58 * User Tasks go here
\r
61 motionCmd_t getMotionCommand(S8 in1, S8 in2)
\r
63 motionCmd_t cmd = STAND_UP; /* default command */
\r
88 * Initialize application
\r
92 initServo(NXTe_PORT);
\r
93 motionCmd = STAND_UP; /* default motion */
\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
107 TASK(Task_Commander)
\r
109 U8 bt_receive_buf[32]; /* buffer size is fixed as 32 */
\r
111 /* read Bluetooth Rx data */
\r
112 if (ecrobot_read_bt_packet(bt_receive_buf, 32))
\r
114 GetResource(ResourceCommand);
\r
115 motionCmd = getMotionCommand(-(*(S8 *)(&bt_receive_buf[0])), (*(S8 *)(&bt_receive_buf[1])));
\r
116 ReleaseResource(ResourceCommand);
\r
123 * Display NXT standard sensors/motors/internal status information.
\r
127 ecrobot_status_monitor("BIPED ROBOT");
\r
133 * Send angle data to NXTe according to motion command.
\r
134 * This Task should be invoked as a background Task.
\r
136 TASK(Task_MotionControl)
\r
140 GetResource(ResourceCommand);
\r
141 cmd = motionCmd; /* latch motion command */
\r
142 ReleaseResource(ResourceCommand);
\r
144 /* set a robot motion */
\r
145 setMotion(NXTe_PORT, cmd);
\r
147 /* back ground task should end like this unless it's a one-shot task */
\r