Software

Interactive C

The development tool for the used for the The Handy Board is called Interactive C (IC). IC is basically consist of two parts, a pseudo-code (pcode) machine language module and the interface/compiler portion (IC program).

IC compiles into pseudo-code (p-code) which is then executed by a custom stack machine. The p-code is interpreted by a run-time machines language program store in the 6811 MPU. This unique approach was taken because it allows IC to have run-time error check, small object code, source code portability and finally the most useful and powerful feature, multi tasking. The only drawback is at the expense of slower execution speed. However, this should not be a problem with most applications.

IC supports a subset of C, it includes:

Here is a block diagram of how IC works.

IC Library functions

DC Motors

void fd(int m)
Turns motor m on in the forward direction. Example: fd(3);
void bk(int m)
Turns motor m on in the backward direction. Example: bk(1);
void off(int m)
Turns off motor m. Example: off(1);
void alloff(), void ao()
Turns off all motors. ao is a short form for alloff.
void motor(int m, int p)
Turns on motor m at power level p. Power levels range from 100 for full on forward to -100 for full on backward.

Servo Motor

void servo_on()
Enables PA7 servo output waveform.
void servo_off()
Disables PA7 servo output waveform.
int servo(int period)
Sets length of servo control pulse. Value is the time in half-microseconds of the positive portion of a rectangular wave that is generated on the PA7 pin for use in controlling a servo motor. Minimum allowable value is 1400 (i.e., 700 sec); maximum is 4860.
int servo_rad(float angle)
Sets servo angle in radians.
int servo_deg(float angle)
Sets servo angle in degrees.

Sensor Input

int digital(int p)
Returns the value of the sensor in sensor port p, as a true/false value (1 for true and 0 for false).
int analog(int p)
Returns value of sensor port numbered p. Result is integer between 0 and 255.

User Buttons and Knob

int stop_button()
Returns value of button labeled Stop: 1 if pressed and 0 if released.
int start_button()
Returns value of button labeled Start.
void stop_press()
Waits for Stop button to be pressed, then released. Then issues a short beep and returns.
void start_press()
Like stop_press, but for the Start button.
int knob()
Returns the position of a knob as a value from 0 to 255.

Infrared Subsystem

int sony_init(1)
Enables the infrared driver.
int sony_init(0)
Disables the infrared driver.
int ir_data(int dummy)
Returns the data byte last received by the driver, or zero if no data has been received since the last call.

Time Commands

void reset_system_time()
Resets the count of system time to zero milliseconds.
long mseconds()
Returns the count of system time in milliseconds.
float seconds()
Returns the count of system time in seconds, as a floating point number. Resolution is one millisecond.
void sleep(float sec)
Waits for an amount of time equal to or slightly greater than sec seconds.
void msleep(long msec)
Waits for an amount of time equal to or greater than msec milliseconds. msec is a long integer.

Tone Functions

void beep()
Produces a tone of 500 Hertz for a period of 0.3 seconds.
void tone(float frequency, float length)
Produces a tone at pitch frequency Hertz for length seconds. Both frequency and length are floats.
void set_beeper_pitch(float frequency)
Sets the beeper tone to be frequency Hz. The subsequent function is then used to turn the beeper on.
void beeper_on()
Turns on the beeper at last frequency selected by the former function.
void beeper_off()
Turns off the beeper.

Multi-Tasking

int start_process( function-call( : :):, [TICKS] , [STACK-SIZE] )
start_process returns an integer, which is the process ID assigned to the new process.
int kill_process(int pid) kill_all
kills all currently running processes.
ps
prints out a list of the process status. The following information is presented: process ID, status code, program counter, stack pointer, stack pointer origin, number of ticks, and name of function that is currently executing.
void hog_processor()
Allocates an additional 256 milliseconds of execution to the currently running process.
void defer()
Makes a process swap out immediately after the function is called.

Floating Point Functions

float sin(float angle)
Returns sine of angle. Angle is specified in radians; result is in radians.
float cos(float angle)
Returns cosine of angle. Angle is specified in radians; result is in radians.
float tan(float angle)
Returns tangent of angle. Angle is specified in radians; result is in radians.
float atan(float angle)
Returns arc tangent of angle. Angle is specified in radians; result is in radians.
float sqrt(float num)
Returns square root of num.
float log10(float num)
Returns logarithm of num to the base 10.
float log(float num)
Returns natural logarithm of num.
float exp10(float num)
Returns 10 to the num power.
float exp(float num)
Returns e to the num power.
(float) a ^ (float) b
Returns a to the b power.

Memory Access Functions

int peek(int loc)
Returns the byte located at address loc.
int peekword(int loc)
Returns the 16-bit value located at address loc and loc+1. loc has the most significant byte, as per the 6811 16-bit addressing standard.
void poke(int loc, int byte)
Stores the 8-bit value byte at memory address loc.
void pokeword(int loc, int word)
Stores the 16-bit value word at memory addresses loc and loc+1.
void bit_set(int loc, int mask)
Sets bits that are set in mask at memory address loc.
void bit_clear(int loc, int mask)
Clears bits that are set in mask at memory address loc.

Here is an hierarchical sturcture of the IC library functions in relations to user programs.

Software model

The main function of the robot is to gather information from all of its sensors and decide what to do from the information collected. It must constantly poll, 4 infrared(edge), 4 ultrasound, and 4 contact sensors. In addition to checking the sensors, the motors are controlled via an open loop system with no feedbacks. Therefore it must use some timing function to control its maneuvers such as turning. The multi-tasking function and time commands of IC seems ideally suited for this application.

The ellipses are individual tasks. The sensors checking tasks writes the result of its sesnsor test to global variables. The "Main Program" polls these global variables and decides on what to do. It also have to make sure that only one "Move Robot" task can be active at a time. This information is communicated to the "Main Program" via task status global variables.



/*****************************************************
 *****   Dave's Multi-tasking Sumo Robot Program *****
 *****         Version .91   May 18, 97           *****
 *****************************************************/



/*****************************************************
 **********     Global Variablies       **************
 *****************************************************
 **********   Common fixed values       **************
 *****************************************************/
int moving=1,
    not_moving=0,
    robot_status,
    running=1,
    not_running=0;

/*****************************************************
 **********          Analog ports        *************
 *****************************************************
 ***********   Infared edge sensors    ***************
 *****************************************************
 ******  Values for edge   ******
 ******    1 = fl_edge     ******
 ******    2 = fr_edge     ******
 ******    4 = rl_edge     ******
 ******    8 = rr_edge     ******/

int rr_edge=0,
    rl_edge=1,
    fr_edge=2,
    fl_edge=3,
    first_edge=0,
    last_edge=3,
    suface_value=0,
    off_edge_value=0,
    white_value=0,
    temp_edge=0,
    edge=0;

/*****************************************************
 **************     Digital Ports      ***************
 *****************************************************/
int left_ultra=12,
    front_left_ultra=13,
    front_right_ultra14,
    right_ultra=15,
    first_ultra=12,
    last_ultra=15,
    ultra=0,
    temp_ultra=0;

int left_contact=7,
    right_contact=8,
    /*front_contact=xx,
    rear_contact=xx,*/
    first_contact=7,
    last_contact=8,
    contact=0,
    temp_contact=0;

/*****************************************************
 ********  Direction, Speed, and Duration   **********
 *****************************************************/
int fast=100,
    medium=50,
    slow=11,
    forward=1,
    right=2,
    left=3,
    fast_pivot=100,
    slow_pivot=50,
    reverse=-1;

long forever=255l;


/*****************************************************
 *********         Process ID's         **************
 *****************************************************/
int edge_pid,move_pid,contact_pid,ultra_pid,
    edge_evade_pid,contact_evade_pid,tracking_pid;


/*****************************************************
 *********       Process Status         **************
 *****************************************************/
int edge_status,move_status,ultra_status,contact_evade_status,
    edge_evade_status,tracking_status;
int edge_evade_stage1=not_running;



/*****************************************************
 ********      Sensor Checking Taskes      ***********
 *****************************************************
 ******** Edge detection Analog ports 0-3  ***********
 *****************************************************/
void check_edge()
{               
  int x,e;
  while(1)
  {  
     e=0;
     for (x=first_edge; x<=last_edge; x++)
     { 
       e <<= 1;    /* shift e 1 bit to the left */
       if (analog(x) <= white_value) /*||analog(x) >= off_edge_value)*/
	 e |= 1;   /* OR e with 0001 */
     }
     /*printf("edge=%d  e=%d\n",edge,e);*/
     if (e != edge)
       edge=e;
  }
}   


/*****************************************************
 ******  Contact sensors, Digital ports 7-10   *******
 *****************************************************/

void check_contact()
{               
  int x,c;
  while(1)
  {  
     c=0;
     for (x=first_contact; x<=last_contact; x++)
     { 
       c <<= 1;    /* shift c 1 bit to the left */
       if (digital(x))
	 c |= 1;   /* OR c with 0001 */
     }
     /*printf("contact=%d  c=%d\n",contact,c);*/
     if (c != contact)
       contact=c;
  }
}

/*****************************************************
 ****** Obstacle detection Digital ports 12-15 *******
 *****************************************************/

void check_ultra()
{
  int u,x,y,z;
  while (1)
  {
    u=0;
    for(x=first_ultra; x<=last_ultra; x++) /* left to right */
    {
      u <<= 1;    /* shift u 1 bit to the left */
      if(digital(x))
	u |= 1;   /* OR u with 0001 */
    }
    printf("ultra=%d  u=%d\n",ultra,u); 
    if (u != ultra)
      ultra=u;
  }
}

/*****************************************************
 *********         Move robot           **************
 *****************************************************/
void move(int direction, int speed, long duration)
{
  int sp1,sp2;
  long dur;
  
  move_status=running;
  if(direction==forward||direction==reverse)
  {
    sp1=direction*speed;
    sp2=sp1;
  }
  else if(direction==right)
  {
      sp1=reverse*speed;
      sp2=speed;
  }
  else if(direction==left)
  {
      sp1=speed;
      sp2=reverse*speed;
  }
  robot_status=moving;
  motor(0,sp1);
  motor(1,sp2);
  motor(2,sp2);
  motor(3,sp1);
  if (duration!=forever)
  {
    dur=mseconds();
    while(duration >=  mseconds()-dur);
    ao();
    robot_status=not_moving;
  }
  move_status=not_running;
  kill_process(move_pid);
}



/*****************************************************
 ************  Edge Evasive Maneuver   ***************
 *****************************************************/
void edge_evade()
{
  int dir,temp;
  
  edge_evade_status=running;
  temp=temp_edge;

  /**********     move away from edge     ***********/
  /* front edge detectors  dir=reverse */
  /* rear edge detectors   dir=forward */                      
  if (temp==1||temp==2||temp==3) 
    dir=reverse;
  else if (temp==4||temp==8||temp==12) 
    dir=forward;
  edge_evade_stage1=running;
  robot_status=moving;
  move_pid=start_process(move(dir,fast,forever));
  while (robot_status==moving);

  edge_evade_stage1=not_running;

  /****** turns towards the centre of platform  ******/
  if (temp==2||temp==8)
    dir=left;
  else if(temp==1||temp==4)
    dir=right;
  msleep(300l);
  move_status=running;
  move_pid=start_process(move(dir,medium,1500l));
  while (move_status==running);

  /******* move towards centre of platform  **********/
  move_status=running;
  move_pid=start_process(move(forward,slow,750l));
  while(move_status==running);
  
  edge_evade_status=not_running;
  kill_process(edge_evade_pid);
}

/*****************************************************
 **********  Contact Evasive Maneuver   **************
 *****************************************************/
void contact_evade()
{
  int c_dir;
  
  contact_evade_status=running;

  /* left contact  c_dir=right speed=fast_pivot duration=500l */
  /* right contact c_dir=left speed=fast_pivot duration=500l */
  /* front contact  c_dir=forward speed=fast duration=forever */
  /* rear contact   c_dir=reverse speed=fast duration=forever */                      
  if (temp_contact==1)  /* right contact */
    c_dir=left;
  else if (temp_contact==2)  /* left contact */ 
    c_dir=right;
  robot_status=moving;
  move_status=running;
  if (temp_contact <= 2 && temp_contact !=0)
    move_pid=start_process(move(c_dir,fast,forever));

  while(contact==temp_contact);
  
  contact_evade_status=not_running;
  kill_process(contact_evade_pid);
}

/*****************************************************
 ************    Obsticle Tracking     ***************
 *****************************************************/

void tracking()
{
 tracking_status=running;

 if(temp_ultra==1)
   move_pid=start_process(move(right,slow,forever));
 if(temp_ultra==2||temp_ultra==3||temp_ultra==4||temp_ultra==6||temp_ultra==12)
   move_pid=start_process(move(forward,slow,forever));
 if(temp_ultra==8)
   move_pid=start_process(move(left,slow_pivot,forever));

 tracking_status=not_running;
 kill_process(tracking_pid);
}


/*****************************************************
 *****************************************************
 *****************************************************
 *****************************************************
 *****************************************************
 ************        Main Program      ***************
 ***************************************************** 
 *****************************************************
 *****************************************************
 *****************************************************
 *****************************************************
 *****************************************************/
void main() 
{ 
  int e,m,k,x,y,z;
  ao(); /* make sure that all motors and relays are off */
  k=1;
  /*  Initalize edge sensor values  */
  for(x=first_edge; x<=last_edge; x++)
  {
    z=0;
    for(y=0; y<=10; y++)
      z=z+analog(x);
    z=z/10;
    suface_value=suface_value+z;
    k++;
  }
  suface_value=suface_value/k;
  white_value=suface_value-20;
  if (suface_value+5 > 253)
    off_edge_value=253;
  else
    off_edge_value=suface_value+5;
  edge_pid=start_process(check_edge());
  contact_pid=start_process(check_contact());
  ultra_pid=start_process(check_ultra());
  /*  Main loop  */
  while (1) 
  { 
    if(edge!=temp_edge)
    {
      if(edge!=0)
      {
	if(edge_evade_status==running)
	{  
	  kill_process(edge_evade_pid); 
	  edge_evade_status=not_running;
	}
	if(contact_evade_status==running)
	{
	  kill_process(contact_evade_pid);
	  contact_evade_status==not_running;
	}
	if(tracking_status==running)
	{
	  kill_process(tracking_pid);
	  tracking_status=not_running;
	}
	if(move_status==running)
	{
	  kill_process(move_pid);
	  move_status=not_running;
	}
	ao();
	robot_status=not_moving;
	temp_edge=edge;
	edge_evade_pid=start_process(edge_evade());
      }
      else 
      {
	if(edge_evade_stage1==running&&robot_status==moving)
	{
	  ao();
	  robot_status=not_moving;
	  if (move_status==running)
	    kill_process(move_pid);
	}
	temp_edge=edge;
      } 
    }
    else if(contact!=temp_contact&&edge_evade_status==not_running)
    {
      if(contact!=0)
      {
	if(tracking_status==running)
	{
	  kill_process(tracking_pid);
	  tracking_status=not_running;
	}
	if(move_status==running)
	{
	  kill_process(move_pid);
	  move_status=not_running;
	}
	ao();
	robot_status=not_moving;
	temp_contact=contact;
	contact_evade_pid=start_process(contact_evade());
      }
      else if(contact_evade_status==running || robot_status==moving)
      {
	if (contact_evade_status==running)
	{
	  kill_process(contact_evade_pid);
	  contact_evade_status=not_running;
	}
	ao();
	robot_status=not_moving;
	temp_contact=contact;
      }
    }
    else if(ultra!=temp_ultra && edge_evade_status==not_running && 
	    contact_evade_status==not_running)
    {
      if(ultra!=0)
      {
	if(tracking_status==running)
	{
	  kill_process(tracking_pid);
	  tracking_status=not_running;
	}
	if(move_status==running)
	{  
	  kill_process(move_pid);
	  move_status=not_running;
	}
	if(robot_status==moving)
	{
	  ao();
	  robot_status=not_moving;
	}
	temp_ultra=ultra;
	tracking_pid=start_process(tracking());
      }
      else
      {
	if(tracking_status==running)
	  kill_process(tracking_pid);
	if(move_status==running)
	  kill_process(move_pid);
	ao();
	robot_status=not_moving;
	temp_ultra=ultra;
      }
    }
  }
}

Next section


Back to Dave's SUMO robot project page 


Back to Dave's home page 


Dave Chu
Concordia University
Electrical and Computer Engineering
1455 De Maisonneuve O. H941
Montreal, Quebec, Canada
H3G 1M8

Telephone: (514) 848-3115
Fax: (514) 848-2802
Email: dave@ece.de-spam.concordia.ca