--- /dev/null
+/**\r
+ ******************************************************************************\r
+ ** FILE NAME : nxtway_gs_main.c\r
+ **\r
+ ** ABSTRUCT : NXTway-GS (with a HiTechnic Gyro Sensor) for TOPPERS/JSP\r
+ ** Two wheeled self-balancing R/C robot controlled via Bluetooth.\r
+ *******************************************************************************\r
+ **/\r
+#include "nxtway_gs.h"\r
+#include "kernel_id.h"\r
+#include "ecrobot_interface.h"\r
+\r
+#include "balancer.h" /* NXTway-GS C API header file */\r
+#include "nxt_config.h"\r
+\r
+/*============================================================================\r
+ * MACRO DEFINITIONS\r
+ *===========================================================================*/\r
+typedef enum{\r
+ INIT_MODE, /* system initialize mode */\r
+ CAL_MODE, /* gyro sensor offset calibration mode */\r
+ CONTROL_MODE /* balance and RC control mode */\r
+} MODE_ENUM;\r
+\r
+#define BT_RCV_BUF_SIZE (32) /* 32bytes fixed for NXT GamePad */\r
+#define MIN_DISTANCE (25) /* minimum distance in cm for obstacle avoidance */\r
+\r
+/*============================================================================\r
+ * DATA DEFINITIONS\r
+ *===========================================================================*/\r
+MODE_ENUM nxtway_gs_mode = INIT_MODE; /* NXTway-GS mode */\r
+volatile U8 obstacle_flag; /* 1(obstacle detected)/0(no obstacle) */\r
+\r
+/*============================================================================\r
+ * FUNCTIONS\r
+ *===========================================================================*/\r
+/*============================================================================\r
+ * Embedded Coder Robot hook functions\r
+ *===========================================================================*/\r
+//*****************************************************************************\r
+// FUNCTION : ecrobot_device_initialize\r
+// ARGUMENT : none\r
+// RETURN : none\r
+// DESCRIPTION : ECRobot device init hook function\r
+//*****************************************************************************\r
+void ecrobot_device_initialize(void)\r
+{\r
+ ecrobot_init_sonar_sensor(PORT_SONAR);\r
+ ecrobot_init_bt_slave(BT_PASS_KEY);\r
+}\r
+\r
+//*****************************************************************************\r
+// FUNCTION : ecrobot_device_terminate\r
+// ARGUMENT : none\r
+// RETURN : none\r
+// DESCRIPTION : ECRobot device term hook function\r
+//*****************************************************************************\r
+void ecrobot_device_terminate(void)\r
+{\r
+ nxt_motor_set_speed(PORT_MOTOR_L, 0, 1);\r
+ nxt_motor_set_speed(PORT_MOTOR_R, 0, 1);\r
+ ecrobot_term_sonar_sensor(PORT_SONAR);\r
+ ecrobot_term_bt_connection();\r
+}\r
+\r
+/*============================================================================\r
+ * TOPPERS JSP specific Function/Tasks\r
+ *===========================================================================*/\r
+\r
+//*****************************************************************************\r
+// FUNCTION : jsp_systick_low_priority\r
+// ARGUMENT : none\r
+// RETURN : none\r
+// DESCRIPTION : 1msec periodical ISR\r
+//*****************************************************************************\r
+void jsp_systick_low_priority(void)\r
+{\r
+ if (get_OS_flag()) /* check whether JSP already started or not */\r
+ {\r
+ isig_tim(); /* cyclic task dispatcher */\r
+ check_NXT_buttons(); /* this must be called here */\r
+ }\r
+}\r
+\r
+//*****************************************************************************\r
+// TASK : cyc0\r
+// DESCRIPTION : 4msec periodical Task handler\r
+//*****************************************************************************\r
+void cyc0(VP_INT exinf)\r
+{\r
+ iact_tsk(TSK0);\r
+}\r
+\r
+//*****************************************************************************\r
+// TASK : cyc1\r
+// DESCRIPTION : 40msec periodical Task handler\r
+//*****************************************************************************\r
+void cyc1(VP_INT exinf)\r
+{\r
+ iact_tsk(TSK1);\r
+}\r
+\r
+//*****************************************************************************\r
+// TASK : tsk0\r
+// DESCRIPTION : 4msec periodical Task and controls NXTway-GS\r
+// INIT_MODE\r
+// \81«\r
+// CAL_MODE\r
+// \81«\r
+// CONTROL_MODE \r
+//*****************************************************************************\r
+void tsk0(VP_INT exinf)\r
+{\r
+ static U32 gyro_offset; /* gyro sensor offset value */\r
+ static U32 avg_cnt; /* average count to calc gyro offset */\r
+ static U32 cal_start_time; /* calibration start time */\r
+ static U8 bt_receive_buf[BT_RCV_BUF_SIZE]; /* Bluetooth receive buffer(32bytes) */\r
+ SINT i;\r
+ S8 cmd_forward, cmd_turn;\r
+ S8 pwm_l, pwm_r;\r
+ \r
+ switch(nxtway_gs_mode){\r
+ case(INIT_MODE):\r
+ gyro_offset = 0;\r
+ avg_cnt = 0;\r
+ for (i = 0; i < BT_RCV_BUF_SIZE; i++){\r
+ bt_receive_buf[i] = 0;\r
+ }\r
+ balance_init(); /* NXTway-GS C API initialize function */\r
+ nxt_motor_set_count(PORT_MOTOR_L, 0); /* reset left motor count */\r
+ nxt_motor_set_count(PORT_MOTOR_R, 0); /* reset right motor count */\r
+ cal_start_time = ecrobot_get_systick_ms();\r
+ nxtway_gs_mode = CAL_MODE;\r
+ break;\r
+ \r
+ case(CAL_MODE):\r
+ gyro_offset += (U32)ecrobot_get_gyro_sensor(PORT_GYRO);\r
+ avg_cnt++;\r
+ if ((ecrobot_get_systick_ms() - cal_start_time) >= 1000U) {\r
+ /* 1000msec later, start balancing */\r
+ gyro_offset /= avg_cnt;\r
+ ecrobot_sound_tone(440U, 500U, 30U); /* beep a tone */\r
+ nxtway_gs_mode = CONTROL_MODE;\r
+ }\r
+ break;\r
+ \r
+ case(CONTROL_MODE):\r
+ (void)ecrobot_read_bt_packet(bt_receive_buf, BT_RCV_BUF_SIZE);\r
+ /* \r
+ * R/C command from NXT GamePad\r
+ * buf[0]: -100(forward max.) to 100(backward max.)\r
+ * buf[1]: -100(turn left max.) to 100(turn right max.)\r
+ */\r
+ cmd_forward = -(S8)bt_receive_buf[0]; /* reverse the direction */\r
+ cmd_turn = (S8)bt_receive_buf[1];\r
+ if (obstacle_flag == 1){\r
+ /* make NXTway-GS move backward to avoid obstacle */\r
+ cmd_forward = -100;\r
+ cmd_turn = 0;\r
+ }\r
+\r
+ /* NXTway-GS C API balance control function (has to be invoked in 4msec period) */\r
+ balance_control(\r
+ (F32)cmd_forward,\r
+ (F32)cmd_turn,\r
+ (F32)ecrobot_get_gyro_sensor(PORT_GYRO),\r
+ (F32)gyro_offset,\r
+ (F32)nxt_motor_get_count(PORT_MOTOR_L),\r
+ (F32)nxt_motor_get_count(PORT_MOTOR_R),\r
+ (F32)ecrobot_get_battery_voltage(),\r
+ &pwm_l,\r
+ &pwm_r);\r
+ nxt_motor_set_speed(PORT_MOTOR_L, pwm_l, 1);\r
+ nxt_motor_set_speed(PORT_MOTOR_R, pwm_r, 1);\r
+\r
+ ecrobot_bt_data_logger(cmd_forward, cmd_turn); /* Bluetooth data logger */\r
+ break;\r
+ \r
+ default:\r
+ /* unexpected mode */\r
+ nxt_motor_set_speed(PORT_MOTOR_L, 0, 1);\r
+ nxt_motor_set_speed(PORT_MOTOR_R, 0, 1);\r
+ break;\r
+ }\r
+\r
+ ext_tsk(); /* terminates this task */\r
+}\r
+\r
+//*****************************************************************************\r
+// TASK : tsk1\r
+// DESCRIPTION : 40msec periodical Task and detects obstacles in front of\r
+// NXTway-GS by using a sonar sensor\r
+//*****************************************************************************\r
+void tsk1(VP_INT exinf)\r
+{\r
+ obstacle_flag = 0; /* no obstacles */\r
+ if ((nxtway_gs_mode == CONTROL_MODE) && \r
+ (ecrobot_get_sonar_sensor(PORT_SONAR) <= MIN_DISTANCE)){\r
+ obstacle_flag = 1; /* obstacle detected */\r
+ }\r
+\r
+ ext_tsk(); /* terminates this task */\r
+}\r
+\r
+//*****************************************************************************\r
+// TASK : tsk2\r
+// DESCRIPTION : Background(never terminated) Task\r
+//*****************************************************************************\r
+void tsk2(VP_INT exinf)\r
+{\r
+ while(1){\r
+ ecrobot_status_monitor("NXTway-GS JSP"); /* LCD display */\r
+ systick_wait_ms(500U); /* 500msec wait */\r
+ }\r
+}\r
+\r
+/******************************** END OF FILE ********************************/\r
OSDN Git Service
