nxtOSEK/samples/nxtway_gs/nxtway_gs_main.c

   1 /**\r
   2  ******************************************************************************\r
   3  **     FILE NAME : nxtway_gs_main.c\r
   4  **\r
   5  **     ABSTRUCT  : NXTway-GS (with a HiTechnic Gyro Sensor)\r
   6  **                     Two wheeled self-balancing R/C robot controlled via Bluetooth.\r
   7  *******************************************************************************\r
   8  **/\r
   9 #include "kernel.h"\r
  10 #include "kernel_id.h"\r
  11 #include "ecrobot_interface.h"\r
  12 \r
  13 #include "balancer.h" /* NXTway-GS C API header file */\r
  14 #include "nxt_config.h"\r
  15 \r
  16 /*============================================================================\r
  17  * MACRO DEFINITIONS\r
  18  *===========================================================================*/\r
  19 typedef enum{\r
  20         INIT_MODE,       /* system initialize mode */\r
  21         CAL_MODE,        /* gyro sensor offset calibration mode */\r
  22         CONTROL_MODE /* balance and RC control mode */\r
  23 } MODE;\r
  24 \r
  25 #define BT_RCV_BUF_SIZE (32) /* 32bytes fixed for NXT GamePad */\r
  26 #define  MIN_DISTANCE    (25) /* minimum distance in cm for obstacle avoidance */\r
  27 \r
  28 /*============================================================================\r
  29  * DATA DEFINITIONS\r
  30  *===========================================================================*/\r
  31 MODE nxtway_gs_mode = INIT_MODE; /* NXTway-GS mode */\r
  32 volatile U8 obstacle_flag;      /* 1(obstacle detected)/0(no obstacle) */\r
  33 \r
  34 /*============================================================================\r
  35  * FUNCTIONS\r
  36  *===========================================================================*/\r
  37 /*============================================================================\r
  38  * Embedded Coder Robot hook functions\r
  39  *===========================================================================*/\r
  40 //*****************************************************************************\r
  41 // FUNCTION             : ecrobot_device_initialize\r
  42 // ARGUMENT             : none\r
  43 // RETURN               : none\r
  44 // DESCRIPTION  : ECRobot device init hook function\r
  45 //*****************************************************************************\r
  46 void ecrobot_device_initialize(void)\r
  47 {\r
  48         ecrobot_init_sonar_sensor(PORT_SONAR);\r
  49         ecrobot_init_bt_slave(BT_PASS_KEY);\r
  50 }\r
  51 \r
  52 //*****************************************************************************\r
  53 // FUNCTION             : ecrobot_device_terminate\r
  54 // ARGUMENT             : none\r
  55 // RETURN               : none\r
  56 // DESCRIPTION  : ECRobot device term hook function\r
  57 //*****************************************************************************\r
  58 void ecrobot_device_terminate(void)\r
  59 {\r
  60         nxt_motor_set_speed(PORT_MOTOR_L, 0, 1);\r
  61         nxt_motor_set_speed(PORT_MOTOR_R, 0, 1);\r
  62         ecrobot_term_sonar_sensor(PORT_SONAR);\r
  63         ecrobot_term_bt_connection();\r
  64 }\r
  65 \r
  66 /*============================================================================\r
  67  * TOPPERS OSEK specific Function/Tasks\r
  68  *===========================================================================*/\r
  69 DeclareCounter(SysTimerCnt);\r
  70 \r
  71 //*****************************************************************************\r
  72 // FUNCTION             : user_1ms_isr_type2\r
  73 // ARGUMENT             : none\r
  74 // RETURN               : none\r
  75 // DESCRIPTION  : 1msec periodical OSEK type 2 ISR\r
  76 //*****************************************************************************\r
  77 void user_1ms_isr_type2(void)\r
  78 {\r
  79         /* Increment System Timer Count to activate periodical Tasks */\r
  80         (void)SignalCounter(SysTimerCnt);\r
  81 }\r
  82 \r
  83 //*****************************************************************************\r
  84 // TASK                 : OSEK_Task_ts1\r
  85 // ARGUMENT             : none\r
  86 // RETURN               : none\r
  87 // DESCRIPTION  : 4msec periodical Task and controls NXTway-GS\r
  88 //                INIT_MODE\r
  89 //                \81«\r
  90 //                CAL_MODE\r
  91 //                \81«\r
  92 //                CONTROL_MODE \r
  93 //*****************************************************************************\r
  94 TASK(OSEK_Task_ts1)\r
  95 {\r
  96         static U32      gyro_offset;    /* gyro sensor offset value */\r
  97         static U32 avg_cnt;                 /* average count to calc gyro offset */\r
  98         static U32      cal_start_time; /* calibration start time */\r
  99         static U8       bt_receive_buf[BT_RCV_BUF_SIZE]; /* Bluetooth receive buffer(32bytes) */\r
 100         SINT i;\r
 101         S8  cmd_forward, cmd_turn;\r
 102         S8      pwm_l, pwm_r;\r
 103         \r
 104         switch(nxtway_gs_mode){\r
 105                 case(INIT_MODE):\r
 106                         gyro_offset = 0;\r
 107                         avg_cnt = 0;\r
 108                         for (i = 0; i < BT_RCV_BUF_SIZE; i++){\r
 109                                 bt_receive_buf[i] = 0;\r
 110                         }\r
 111                         balance_init(); /* NXTway-GS C API initialize function */\r
 112                         nxt_motor_set_count(PORT_MOTOR_L, 0); /* reset left motor count */\r
 113                         nxt_motor_set_count(PORT_MOTOR_R, 0); /* reset right motor count */\r
 114                         cal_start_time = ecrobot_get_systick_ms();\r
 115                         nxtway_gs_mode = CAL_MODE;\r
 116                         break;\r
 117                                                 \r
 118                 case(CAL_MODE):\r
 119                         gyro_offset += (U32)ecrobot_get_gyro_sensor(PORT_GYRO);\r
 120                         avg_cnt++;\r
 121                         if ((ecrobot_get_systick_ms() - cal_start_time) >= 1000U) {\r
 122                                 /* 1000msec later, start balancing */\r
 123                                 gyro_offset /= avg_cnt;\r
 124                                 ecrobot_sound_tone(440U, 500U, 30U); /* beep a tone */\r
 125                                 nxtway_gs_mode = CONTROL_MODE;\r
 126                         }\r
 127                         break;\r
 128                                                 \r
 129                 case(CONTROL_MODE):\r
 130                         (void)ecrobot_read_bt_packet(bt_receive_buf, BT_RCV_BUF_SIZE);\r
 131                         /* \r
 132                          * R/C command from NXT GamePad\r
 133                          * buf[0]: -100(forward max.) to 100(backward max.)\r
 134                          * buf[1]: -100(turn left max.) to 100(turn right max.)\r
 135                          */\r
 136                         cmd_forward = -(S8)bt_receive_buf[0]; /* reverse the direction */\r
 137                         cmd_turn = (S8)bt_receive_buf[1];\r
 138                         if (obstacle_flag == 1){\r
 139                                 /* make NXTway-GS move backward to avoid obstacle */\r
 140                                 cmd_forward = -100;\r
 141                                 cmd_turn = 0;\r
 142                         }\r
 143 \r
 144                         /* NXTway-GS C API balance control function (has to be invoked in 4msec period) */\r
 145                         balance_control(\r
 146                                 (F32)cmd_forward,\r
 147                                 (F32)cmd_turn,\r
 148                                 (F32)ecrobot_get_gyro_sensor(PORT_GYRO),\r
 149                                 (F32)gyro_offset,\r
 150                                 (F32)nxt_motor_get_count(PORT_MOTOR_L),\r
 151                                 (F32)nxt_motor_get_count(PORT_MOTOR_R),\r
 152                                 (F32)ecrobot_get_battery_voltage(),\r
 153                                 &pwm_l,\r
 154                                 &pwm_r);\r
 155                         nxt_motor_set_speed(PORT_MOTOR_L, pwm_l, 1);\r
 156                         nxt_motor_set_speed(PORT_MOTOR_R, pwm_r, 1);\r
 157 \r
 158                         ecrobot_bt_data_logger(cmd_forward, cmd_turn); /* Bluetooth data logger */\r
 159                         break;\r
 160                         \r
 161                 default:\r
 162                         /* unexpected mode */\r
 163                         nxt_motor_set_speed(PORT_MOTOR_L, 0, 1);\r
 164                         nxt_motor_set_speed(PORT_MOTOR_R, 0, 1);\r
 165                         break;\r
 166         }\r
 167 \r
 168         TerminateTask(); /* terminates this task and executed by System Timer Count */\r
 169 }\r
 170 \r
 171 //*****************************************************************************\r
 172 // TASK                 : OSEK_Task_ts2\r
 173 // ARGUMENT             : none\r
 174 // RETURN               : none\r
 175 // DESCRIPTION  : 40msec periodical Task and detects obstacles in front of\r
 176 //                NXTway-GS by using a sonar sensor\r
 177 //*****************************************************************************\r
 178 TASK(OSEK_Task_ts2)\r
 179 {\r
 180         obstacle_flag = 0; /* no obstacles */\r
 181         if ((nxtway_gs_mode == CONTROL_MODE) && \r
 182       (ecrobot_get_sonar_sensor(PORT_SONAR) <= MIN_DISTANCE)){\r
 183                 obstacle_flag = 1; /* obstacle detected */\r
 184         }\r
 185 \r
 186         TerminateTask(); /* terminates this task and executed by System Timer Count */\r
 187 }\r
 188 \r
 189 //*****************************************************************************\r
 190 // TASK                 : OSEK_Task_Background\r
 191 // ARGUMENT             : none\r
 192 // RETURN               : none\r
 193 // DESCRIPTION  : Background(never terminated) Task\r
 194 //*****************************************************************************\r
 195 TASK(OSEK_Task_Background)\r
 196 {\r
 197         while(1){\r
 198                 ecrobot_status_monitor("NXTway-GS"); /* LCD display */\r
 199                 systick_wait_ms(500); /* 500msec wait */\r
 200         }\r
 201 }\r
 202 \r
 203 /******************************** END OF FILE ********************************/\r