nxtOSEK: NXTway-GS C API

NXTway-GS(a two wheeled self-balancing robot) C API

NXTway (a two wheeled self-balancing robot) is a challengeable applications using the NXT. The most difficult thing is, of course, "How to balance the robot" and it requires complex mathematical equations and the knowledge of physics/control theory. However, in this sample, you can make a NXTway with a HiTechnic Gyro Sensor without deep knowledge about these difficult stuff. What you need to do is: building a robot with the instructions and using NXTway-GS C API. Furthermore, NXTway-GS sample can be used as the driving base of further NXTway based robot applications.

Building Instructions : NXTway-GS_Building_Instructions.pdf

The new version of NXTway-GS is easier to build, required less parts, more rigid compared to the previous version, and supports several variations. NXTway-GS supports the following variations.
- RCX motorcycle tires or NXT standard tires
- Narrow or wide tread
NXTway-GS with the motorcycle tires is more robust, faster, quicker response and looks nicer! however the motorcycle tires are not included in a NXT retail set.

NXTway-GS C API:

NXTway-GS C API has simple API structure (a control function and an initialize function). The control function (balance_control) has to be invoked every 4msec and the initialize function (balance_init) initializes internal state variables to reset the balance control functionality.
These C functions are not hand coded, but designed as a Simulink model and a C/C++ code generator which is called Real-Time Workshop Embedded Coder automatically generated those C functions. If you were a Simulink user, you could download the original model from The MathWorks File Exchange site. The model also includes a PDF document and it is valuable to understand the control theory (servo control: state + integral feedback) used in this API. NXTway-GS C API is provided as a C library and source code is stored in nxtOSEK\ecrobot\nxtway_gs_balancer directory. The Simulink model which is the design source of these C API is also stored as html files (Web browser must be SVG-compatible). To use NXTway-GS C API, you need to include balancer.h header file in the source code.

NXTway-GS C API

Description

void
balance_control(
    F32 args_cmd_forward,
    F32 args_cmd_turn,
    F32 args_gyro,
    F32 args_gyro_offset,
    F32 args_theta_m_l,
    F32 args_theta_m_r,
    F32 args_battery,
    S8 *ret_pwm_l,
    S8 *ret_pwm_r)

controls the robot being balanced. This API has to be invoked every 4msec and designed for the robot which is built with the building instructions. Gyro Sensor offset value may be varied depending on a product and it has drift while it's turned on, so these things need to be considered. Left & right motor revolutions may be different even if a same PWM output is given, in this case, user needs to add some motor revolution compensator.

Parameters:
    args_cmd_forward: robot control command to move forward/backward.
       100.0F(max. forward speed) to -100.0F (max. backward speed)
    args_cmd_turn: robot control command to turn right/left.
       100.0F(max. right turn speed) to -100.0F (max. left turn speed)
    args_gyro: HiTechnic Gyro Sensor raw data to measure the robot body pitch rate
    args_gyro_offset: HiTechnic Gyro Sensor raw data when the robot body pitch rate is 0
    args_theta_m_l: Left wheel motor count raw data
    args_theta_m_r: Right wheel motor count raw data
    args_battery: battery voltage in mV
Returns:
    ret_pwm_l: Left wheel motor PWM output
    ret_pwm_r: Right wheel motor PWM output

void
balance_init(void)

initializes internal state variables. This API resets the balance control functionality. To use this function, left & right wheel motors count also need to be 0 reset.

Parameters:
none
Returns:
none

Sample program:

NXTway-GS sample program is hand coded and you can control the robot via Bluetooth R/C using a PC HID GamePad with analog inputs. This sample program is assumed to be used with NXT GamePad utility software in PC. An Ultrasonic(Sonar) Sensor is used to detect obstacles and while the robot is approaching to an obstacle within 25cm, the robot moves backward to avoid the obstacle regardless of the R/C control inputs.
By default, the sample program is configured for NXTway-GS with RCX Motorcycle tires and it supports both of narrow and wide tread. When NXT standard tires are used, it needs to add a compile macro (NXT_STD_TIRE) to USER_DEF macro definition in user Makefile (In the below sample Makefile, USER_DEF macro definition is commented out). Performance of NXTway-GS balance control is also determined by control parameters which are defined in balancer_param.c file. These parameters are optimised for NXTway-GS robots which are built with the building instructions, however, you may be able to tune up the robot by changing the parameters.

samples\nxtway_gs\Makefile

# Target specific macros
TARGET = NXTway_GS

NXTOSEK_ROOT = ../..

# nxtway_gs_balancer library desiged for NXTway-GS two wheeled self-balancing robot
USER_INC_PATH = $(NXTOSEK_ROOT)/ecrobot/nxtway_gs_balancer
USER_LIB = nxtway_gs_balancer

# using NXT standard tires (not Motorcycle tires)
#USER_DEF = NXT_STD_TIRE <-This macro has to be enabled for NXT standard tires

# User application source
TARGET_SOURCES := \
balancer_param.c \
nxtway_gs_main.c

# OSEK OIL file
TOPPERS_OSEK_OIL_SOURCE := ./nxtway_gs.oil

# below part should not be modified
O_PATH ?= build
include $(NXTOSEK_ROOT)/ecrobot/lejos_osek.tmf

samples\nxtway_gs\nxt_config.h

/**
*******************************************************************************
** FILE NAME : nxt_config.h
**
** ABSTRUCT : NXT device configration
*******************************************************************************
**/

#ifndef _NXT_CONFIG_H_
#define _NXT_CONFIG_H_

#include "ecrobot_interface.h"

/* NXT motor port configuration */
#define PORT_MOTOR_R NXT_PORT_B
#define PORT_MOTOR_L NXT_PORT_C
/* NXT sensor port configuration */
#define PORT_SONAR NXT_PORT_S2
#define PORT_GYRO NXT_PORT_S4
/* NXT Bluetooth configuration */
#define BT_PASS_KEY "1234" <-Bluetooth pass key which needs to be set in PC
#endif

samples\nxtway_gs\nxtway_gs_main.c

/**
******************************************************************************
** FILE NAME : nxtway_gs_main.c
**
** ABSTRUCT : NXTway-GS (with a HiTechnic Gyro Sensor)
**             Two wheeled self-balancing R/C robot controlled via Bluetooth.
******************************************************************************
**/
#include "kernel.h"
#include "kernel_id.h"
#include "ecrobot_interface.h"

#include "balancer.h" /* NXTway-GS C API header file */ <-This header file has to be included
#include "nxt_config.h"

/*============================================================================
* MACRO DEFINITIONS
*===========================================================================*/
typedef enum {
  INIT_MODE,   /* system initialize mode */
  CAL_MODE,    /* gyro sensor offset calibration mode */
  CONTROL_MODE /* balance and RC control mode */
} MODE;

#define MIN_DISTANCE (25)    /* minimum distance in cm for obstacle avoidance */
#define BT_RCV_BUF_SIZE (32) /* it must be 32bytes with NXT GamePad */

/*============================================================================
* DATA DEFINITIONS
*===========================================================================*/
MODE nxtway_gs_mode = INIT_MODE; /* NXTway-GS mode */
volatile U8 obstacle_flag;       /* 1(obstacle detected)/0(no obstacle) */

/*============================================================================
* FUNCTIONS
*===========================================================================*/
/*============================================================================
* Embedded Coder Robot hook functions
*===========================================================================*/
//*****************************************************************************
// FUNCTION    : ecrobot_device_initialize
// ARGUMENT    : none
// RETURN      : none
// DESCRIPTION : ECRobot device init hook function
//*****************************************************************************
void ecrobot_device_initialize(void)
{
  ecrobot_init_sonar_sensor(PORT_SONAR);
  ecrobot_init_bt_slave(BT_PASS_KEY);
}

//*****************************************************************************
// FUNCTION    : ecrobot_device_terminate
// ARGUMENT    : none
// RETURN      : none
// DESCRIPTION : ECRobot device term hook function
//*****************************************************************************
void ecrobot_device_terminate(void)
{
  nxt_motor_set_speed(PORT_MOTOR_L, 0, 1);
  nxt_motor_set_speed(PORT_MOTOR_R, 0, 1);
  ecrobot_term_sonar_sensor(PORT_SONAR);
  ecrobot_term_bt_connection();
}

/*============================================================================
* TOPPERS ATK specific Function/Tasks
*===========================================================================*/
DeclareCounter(SysTimerCnt);

//*****************************************************************************
// FUNCTION    : user_1ms_isr_type2
// ARGUMENT    : none
// RETURN      : none
// DESCRIPTION : 1msec periodical OSEK type 2 ISR
//*****************************************************************************
void user_1ms_isr_type2(void)
{
  /* Increment System Timer Count to activate periodical Tasks */
  (void)SignalCounter(SysTimerCnt);
}

//*****************************************************************************
// TASK        : OSEK_Task_ts1
// ARGUMENT    : none
// RETURN      : none
// DESCRIPTION : 4msec periodical Task and controls NXTway-GS
//               INIT_MODE
//               ↓
//               CAL_MODE
//               ↓
//               CONTROL_MODE
//*****************************************************************************
TASK(OSEK_Task_ts1)
{
  static U32 gyro_offset;    /* gyro sensor offset value */
  static U32 avg_cnt;        /* average counter to calc gyro offset */
  static U32 cal_start_time; /* calibration start time */
  static U8 bt_receive_buf[BT_RCV_BUF_SIZE]; /* Bluetooth receive buffer(32bytes) */
  SINT i;
  S8 cmd_forward,cmd_turn;
  S8 pwm_l,pwm_r;

  switch(nxtway_gs_mode){
    case(INIT_MODE):
      gyro_offset = 0;
      avg_cnt = 0;
      for (i = 0; i < BT_RCV_BUF_SIZE; i++){
        bt_receive_buf[i] = 0;
      }
      balance_init(); /* NXTway-GS C API initialize function */ <-Call balance init function
      nxt_motor_set_count(PORT_MOTOR_L, 0); /* reset left motor count */
      nxt_motor_set_count(PORT_MOTOR_R, 0); /* reset right motor count */
      cal_start_time = ecrobot_get_systick_ms();
      nxtway_gs_mode = CAL_MODE;
      break;

    case(CAL_MODE):
      gyro_offset += (U32)ecrobot_get_gyro_sensor(PORT_GYRO);
      avg_cnt++;
      /* 1000msec later, start balancing */
      if ((ecrobot_get_systick_ms() - cal_start_time) >= 1000U){
        gyro_offset /= avg_cnt;
        ecrobot_sound_tone(440U, 500U, 30); /* beep a tone */
        nxtway_gs_mode = CONTROL_MODE;
      }
      break;

    case(CONTROL_MODE):
      /*
       * R/C command from NXT GamePad
       * buf[0]: -100(forward max.) to 100(backward max.)
       * buf[1]: -100(turn left max.) to 100(turn right max.)
       */
      (void)ecrobot_read_bt_packet(bt_receive_buf, BT_RCV_BUF_SIZE);
      cmd_forward = -(S8)bt_receive_buf[0]; /* reverse the direction */
      cmd_turn = (S8)bt_receive_buf[1];
      if (obstacle_flag == 1){
        /* make NXTway-GS move backward to avoid obstacle */
        cmd_forward = -100;
        cmd_turn = 0;
      }

      /* NXTway-GS C API balance control function (has to be invoked in 4msec period) */
      balance_control( <-Call balance control function
        (F32)cmd_forward,
        (F32)cmd_turn,
        (F32)ecrobot_get_gyro_sensor(PORT_GYRO),
        (F32)gyro_offset,
        (F32)nxt_motor_get_count(PORT_MOTOR_L),
        (F32)nxt_motor_get_count(PORT_MOTOR_R),
        (F32)ecrobot_get_battery_voltage(),
        &pwm_l,
        &pwm_r);
      nxt_motor_set_speed(PORT_MOTOR_L, pwm_l, 1);
      nxt_motor_set_speed(PORT_MOTOR_R, pwm_r, 1);

      ecrobot_bt_data_logger(cmd_forward, cmd_turn); /* Bluetooth data logger */
      break;

    default:
      /* unexpected mode */
      nxt_motor_set_speed(PORT_MOTOR_L, 0, 1);
      nxt_motor_set_speed(PORT_MOTOR_R, 0, 1);
      break;
  }

  TerminateTask(); /* terminates this task and executed by System Timer Count */
}

//*****************************************************************************
// TASK        : OSEK_Task_ts2
// ARGUMENT    : none
// RETURN      : none
// DESCRIPTION : 40msec periodical Task and detects obstacle in front of
//               NXTway-GS by using a sonar sensor
//*****************************************************************************
TASK(OSEK_Task_ts2)
{
  obstacle_flag = 0; /* no obstacles */
  if ((nxtway_gs_mode == CONTROL_MODE) &&
    (ecrobot_get_sonar_sensor(PORT_SONAR) <= MIN_DISTANCE)){
     obstacle_flag = 1; /* obstacle detected */
  }

  TerminateTask(); /* terminates this task and executed by System Timer Count */
}

//*****************************************************************************
// TASK        : OSEK_Task_Background
// ARGUMENT    : none
// RETURN      : none
// DESCRIPTION : Background(never terminated) Task
//*****************************************************************************
TASK(OSEK_Task_Background)
{
  while(1){
    ecrobot_status_monitor("NXTway-GS"); /* LCD display */
    systick_wait_ms(500); /* 500msec wait */
  }
}

/******************************** END OF FILE ********************************/

samples\nxtway_gs\balancer_param.c

/**
*******************************************************************************
** FILE NAME : balancer_param.c
**
** ABSTRUCT : NXTway-GS balance control parameters
**
** NOTE: These parameters definitely affect the balance control of NXTway-GS.
*******************************************************************************
**/
#include "ecrobot_interface.h"

/*============================================================================
* DATA DEFINITIONS
*===========================================================================*/
F32 A_D = 0.8F;   /* low pass filter gain for motors average count */
F32 A_R = 0.996F; /* low pass filter gain for motors target count */

/*
* NOTE: When NXT standard tires are used for NXTway-GS, a compiler macro (NXT_STD_TIRE)
* has to be added to USER_DEF macro definition in user Makefile
* E.g. USER_DEF = NXT_STD_TIRE
*/
#ifdef NXT_STD_TIRE
  /* state feedback gain for NXT standard tire */
  F32 K_F[4] = {-0.834434F, -38.1749F, -1.0985F, -3.55477F};
#else
  /* state feedback gain for RCX Motorcycle tire */
  F32 K_F[4] = {-0.870303F, -31.9978F, -1.1566F, -2.78873F};
#endif

F32 K_I = -0.44721F;   /* servo control integral gain */
F32 K_PHIDOT = 25.0F; /* turn target speed gain */
F32 K_THETADOT = 7.5F; /* forward target speed gain */

const F32 BATTERY_GAIN = 0.001089F; /* battery voltage gain for motor PWM outputs */
const F32 BATTERY_OFFSET = 0.625F; /* battery voltage offset for motor PWM outputs */

/******************************** END OF FILE ********************************/

samples\nxtway_gs\nxtway_gs.oil

/**
******************************************************************************
** FILE NAME : nxtway_gs.oil
**
** ABSTRUCT : OSEK OIL(OSEK Implementation Language) file for NXTway-GS
******************************************************************************
**/
#include "implementation.oil"

CPU ATMEL_AT91SAM7S256
{
  OS LEJOS_OSEK
  {
    STATUS = EXTENDED;
    STARTUPHOOK = FALSE;
    ERRORHOOK = FALSE;
    SHUTDOWNHOOK = FALSE;
    PRETASKHOOK = FALSE;
    POSTTASKHOOK = FALSE;
    USEGETSERVICEID = FALSE;
    USEPARAMETERACCESS = FALSE;
    USERESSCHEDULER = FALSE;
  };

  /* Definition of application mode */
  APPMODE appmode1{};

  /* Definitions of a periodical task: OSEK_Task_ts1 */
  TASK OSEK_Task_ts1
  {
    AUTOSTART = FALSE;
    PRIORITY = 3;
    ACTIVATION = 1;
    SCHEDULE = FULL;
    STACKSIZE = 512; /* bytes */
  };
  ALARM OSEK_Alarm_task_ts1
  {
    COUNTER = SysTimerCnt;
    ACTION = ACTIVATETASK
    {
      TASK = OSEK_Task_ts1;
    };
    AUTOSTART = TRUE
    {
      APPMODE = appmode1;
      ALARMTIME = 1;
      CYCLETIME = 4; /* executed every 4msec */ <-Configure Task execution period to 4msec
    };
  };

  /* Definitions of a periodical task: OSEK_Task_ts2 */
  TASK OSEK_Task_ts2
  {
    AUTOSTART = FALSE;
    PRIORITY = 2;
    ACTIVATION = 1;
    SCHEDULE = FULL;
    STACKSIZE = 512; /* bytes */
  };
  ALARM OSEK_Alarm_task_ts2
  {
    COUNTER = SysTimerCnt;
    ACTION = ACTIVATETASK
    {
      TASK = OSEK_Task_ts2;
    };
    AUTOSTART = TRUE
    {
      APPMODE = appmode1;
      ALARMTIME = 1;
      CYCLETIME = 40; /* executed every 40msec */
    };
  };

  /* Definition of background task: OSEK_Task_Background */
  TASK OSEK_Task_Background
  {
    AUTOSTART = TRUE
    {
      APPMODE = appmode1;
    };
    PRIORITY = 1; /* lowest priority */
    ACTIVATION = 1;
    SCHEDULE = FULL;
    STACKSIZE = 512; /* bytes */
  };

  /* Definition of OSEK Alarm Counter: SysTimerCnt */
  COUNTER SysTimerCnt
  {
    MINCYCLE = 1;
    MAXALLOWEDVALUE = 10000;
    TICKSPERBASE = 1;
  };

Back to Samples