/*
 * Firmware for ECEBot Zeta (Fall 2008 version)
 *
 * Rob Maher
 *
 * Copyright (c) 2005 Department of Electrical and Computer Engineering, Montana State University
 *
 * Audit trail
 *              Start  RCM 11 October 2005    Initial C program to handle motors, LEDs, display, and bumpers
 *
 *										 RCM 16 October 2005    Completed 7 modes, ready to test for firmware (version 2.0)
 *
 *                     RCM 15 September 2008  for CSMB12 board
 *
 *                     RCM 16 October   2008  serial experiments
 *
 * Description:
 *   Includes, defines, and globals listed first, then
 *   Interrupt service routines,
 *   Low level display and control functions,
 *   High level command functions,
 *   and finally,
 *   the main() function. 
 */

/**********************************************************************************************/
/*
 * Modes are set by selecting DIP switch pattern (in RUN mode) and pressing Reset button.
 * NOTE that the DIP switches are numbered left to right!
 * NOTE that only switches 1-4 are connected to the microcontroller; switches 5-8 are unused.
 *
 * Mode 0   0000    PCB test (flashes LEDs 4&5, does 4-digit display pattern)
 * Mode 1   1000		Analog-to-Digital  (Displays digital representation of potentiometer wiper position)
 * Mode 2   0100    Forward test (rolls forward for 5 seconds, then stops)
 * Mode 3   1100    Forward-Backward (rolls forward and then backward repeatedly)
 * Mode 4   0010    Dance pattern (rolls forward, then rotates, then forward again, repeatedly)
 * Mode 5   1010    Bumper Bot (rolls forward; if left bumper hits, backs up and turns right; if
 *                               right bumper hits, backs up and turns left)
 * Mode 6   0110    Programmable mode (user must download a command sequence to RAM)
 * Mode 7   1110    Figure-Eight (Robot executes a sequence of segments and turns to be a figure-8 pattern)
 *
 * Modes 8-15:  not defined
 *
 */
 
/**********************************************************************************************/
#include <hidef.h>      /* common defines and macros */
#include <MC9S12C128.h>     /* derivative information */


#include "main_asm.h" /* interface to the assembly module */


#include <stdlib.h>

#pragma LINK_INFO DERIVATIVE "mc9s12c128"

#define CODE_REV_MAJOR 3	 /* Revision code             */
#define CODE_REV_MINOR 1   //RCM, was 0

#define DELAY 2						 /* Tick count (millisec) used for display updates  */
#define MAX_STEP 256			 /* Number of 2-byte commands available for mode 6 downloads  */

#define MAX_TIME 0x9f60		 /* Pulse width corresponding to maximum "on" time          */
#define MIN_TIME 0x79E0		 /* Pulse width to motor corresponding to minimum "on" time */
                           /* 0x79E0 for 1.3ms; 0x9f60 for 1.7ms	 0x8CA0 for 1.5ms		*/
                           
#define RIGHT_FORWARD  MIN_TIME
#define LEFT_FORWARD   MAX_TIME
#define RIGHT_BACKWARD MAX_TIME
#define LEFT_BACKWARD  MIN_TIME

/* macro to adjust tick count to compensate 1ms vs. 1.024 ms (rounds to nearest) */
#define MS(a) ((int)(0.5+a/1.024))

/* enable LED4 (left, PTAD6) and LED5 (right, PTAD7) */
#define LEFT_LED_ON(a)          ((a)?(PTAD_PTAD6 = 0):(PTAD_PTAD6 = 1)) /* LED4 (active low) */
#define RIGHT_LED_ON(a)         ((a)?(PTAD_PTAD7 = 0):(PTAD_PTAD7 = 1)) /* LED5 (active low) */

/* enable interrupt on timer chan 0 (right motor PWM) and timer chan 1 (left motor PWM)  */
#define MOTOR_INTERRUPTS_ON(a)  ((a)?(TIE_C0I = 1,TIE_C1I = 1):(TIE_C0I = 0,TIE_C1I = 0)) 


/**********************************************************************************************/
/* 
 * Global variables
 */
volatile int Val_right,Val_left,Speed_right,Speed_left;
volatile int Vcnt;
unsigned int Bunch_o_mem[MAX_STEP] @(0x1000-MAX_STEP*sizeof(unsigned int));
// pre-test: unsigned int Bunch_o_mem[MAX_STEP] ;
int Step_num;
volatile unsigned char Bump=0x03;
void (*Rti_func)(void) = NULL;
void initsci(void);
void putchar(char);
char getchar(void);
char scichar_ready(void);

/**********************************************************************************************/
/*
 * Interrupt service routines
 */
/**********************************************************************************************/

void interrupt 7 RTI_handler() {
/*
 * This function is called when a real time interrupt (RTI) occurs.
 * 
 * The function asserts the RTI flag and increments global variable Vcnt.
 * If a function pointer has been assigned to global variable Rti_func, then
 * that function is also called. 
 */  

  CRGFLG_RTIF = 1;  // Set RTI flag to reset counter (MUST do this each time)
  Vcnt++;
  if( Rti_func ) (Rti_func)();
  
} /* end of RTI_handler() */

 
/**********************************************************************************************/
void interrupt 8 outcap_0(void) {
/*
 * Pulse width modulation (PWM) control interrupt.
 * This interrupt is for the right motor
 */

  TFLG1 = 0x01;								// Clear chan 0 flag
  TC0 = TC0 + Speed_right;		// Set time for next interrupt (current time + offset)
  Val_right = (Val_right+1)%9;  // Variable counts 0-8: when zero, pulse out is high, otherwise low (1 out of 9 duty cycle)
  if( Val_right ) TCTL2 = (TCTL2&0xFC)|0x02 ;  // Clear output on next interrupt
  else TCTL2=(TCTL2&0xFC)|0x03;	               // Set output on next interrupt
} /* end of outcap_0() */


/**********************************************************************************************/
void interrupt 9 outcap_1(void) {
/*
 * Pulse width modulation (PWM) control interrupt.
 * This interrupt is for the left motor
 */

  TFLG1 = 0x02;								 // Clear chan 1 flag
  TC1 = TC1 + Speed_left;			 // Set time for next interrupt (current time + offset)
  Val_left = (Val_left+1)%9;	 // Variable counts 0-8: when zero, pulse out is high, otherwise low (1 out of 9 duty cycle)
  if( Val_left ) TCTL2 = (TCTL2&0xF3) | 0x08 ; // Clear output on next interrupt
  else TCTL2=(TCTL2&0xF3) | 0x0C;	             // Set output on next interrupt
} /* end of outcap_1() */

/**********************************************************************************************/
void delay_ms(int delcount) {
/*
 * Delay for specified number of milliseconds.
 * The function also checks the bumper status:  if bump, exit and save it in global variable 'Bump'.
 * Millisecond timing is based on Real Time Interrupt, and may be inaccurate (1.024 ms vs 1.000 ms); SEE the
 * macro MS( ) that will round a constant to the nearest 1.024ms ticks.
 */
  int vstop;
  
  vstop=Vcnt+delcount;  // add the specified number of millisecond ticks to Vcnt (current count)...
  while(Vcnt != vstop ){
    Bump = (PTAD&0x03);			 // Stay here and look for any bump contacts
    if(Bump!=0x03) break;
  } // ... and then wait until count expires.


}/* end delay_ms() */

/**********************************************************************************************/
void delay_ms_abs(int delcount) {
/*
 * Delay for specified number of milliseconds.
 * The function DOES NOT check the bumper status.
 * Millisecond timing is based on Real Time Interrupt, and may be inaccurate (1.024 ms vs 1.000 ms); SEE the
 * macro MS( ) that will round a constant to the nearest 1.024ms ticks.
 */
  int vstop;
  
  vstop=Vcnt+delcount;  // add the specified number of millisecond ticks to Vcnt (current count)...
  while(Vcnt != vstop ){
  } // ... and then wait until count expires.


}/* end delay_ms_abs() */



/**********************************************************************************************/
unsigned char read_switches(void){
/* Function reads the DIP switches
 * and returns the byte pattern.
 * NOTE that only the upper 4 bits are connected to DIP 1-4,
 * and also note that 'on' switch is a 0 (active low)
 */
  unsigned char hold_DDRT,switch_pattern;

  hold_DDRT = DDRT;  // Save T data direction
  PTM=0x0F;		       // Make sure display transistors are off 
  DDRT=0x00;	       // Set T to be inputs
  delay_ms_abs(2);
  switch_pattern=PTT;	 // read port T
  switch_pattern=PTT;	 // redundant port T read
  DDRT=hold_DDRT;	 // restore DDRT
  delay_ms_abs(2);
  return(switch_pattern);

}/* end of read_switches() */

/**********************************************************************************************/
void display(int digit,unsigned char four_bit_code, unsigned char decimal) {
/*
 * This function selects which digit to display and which segments
 * to turn on.  Parameter decimal is a boolean: zero means no decimal
 * point, non-zero means display the decimal point.  
 */
  switch(digit) {
    case 0:
            PTM_PTM3 = 0;
            PTM_PTM4 = 0;
            PTM_PTM5 = 0;
			      break;
    case 1:
               //PTM=0x20;
            PTM_PTM3 = 0;
            PTM_PTM4 = 0;
            PTM_PTM5 = 1;
            break;
    case 2:
            //PTM=0x10;
            PTM_PTM3 = 0;
            PTM_PTM4 = 1;
            PTM_PTM5 = 0;
            break;
    case 3:
            //PTM=0x30;
            PTM_PTM3 = 0;
            PTM_PTM4 = 1;
            PTM_PTM5 = 1;
            break;
		default:
            //PTM=0x08; // off
	          PTM_PTM3 = 1;
            PTM_PTM4 = 0;
            PTM_PTM5 = 0;
        		break;
  }
  PTT= (four_bit_code<<4);
  PTM_PTM2 = (decimal)?0:1;
     
}/* end of display() */

/**********************************************************************************************/
void display_decimal(unsigned int val,int decimal_position){
/*
 * This function takes an input val and displays a 4-digit
 * decimal version on the robot display.  Only the least signifcant
 * 4 digits are displayed.  The parameter decimal_position specifies
 * where the decimal point is located:
 *  <0 is no decimal point and leading zero blanking
 *  0  is rightmost (and leading zero blanking)
 *  1  is one decimal place (and blanking)
 *  2  is two  " "
 *  3  is three decimal place
 *  >=4 is no decimal point but NO zero blanking
 */
  int temp,blank;

  temp=(val%10000)/1000;
  if( temp == 0 && decimal_position<3){
    blank=1;
    temp=0x0F;
   }
   else blank=0;

  if(decimal_position==3) display(3,(unsigned char)temp,(unsigned char)1);
  else display(3,(unsigned char)temp,(unsigned char)0);      
  delay_ms_abs(DELAY);

  temp= (val%1000)/100;
  if( temp == 0 && blank==1 && decimal_position<2){
    temp=0x0F;
  }
  else blank=0;

  if(decimal_position==2) display(2,(unsigned char)temp,(unsigned char)1);
  else display(2,(unsigned char)temp,(unsigned char)0);      
  delay_ms_abs(DELAY);

  temp= (val%100)/10;
  if( temp == 0 && blank==1 && decimal_position < 1){
    temp=0x0F;
  }
  else blank=0;

  if(decimal_position==1) display(1,(unsigned char)temp,(unsigned char)1);
  else display(1,(unsigned char)temp,(unsigned char)0);      
  delay_ms_abs(DELAY);

  temp= val%10;
  if(decimal_position==0) display(0,(unsigned char)temp,(unsigned char)1);
  else display(0,(unsigned char)temp,(unsigned char)0);      
  delay_ms_abs(DELAY);

}/* end of display_decimal() */


/**********************************************************************************************/
void display_hexadecimal(unsigned int val,int decimal_position){
/*
 * This function takes an input val and displays a 4-digit
 * hexadecimal version on the robot display.  The decoder shows symbols
 * instead of ABCDEF (refer to 74LS47 datasheet). Only the least signifcant
 * 4 digits are displayed.  The parameter decimal_position specifies
 * where the decimal point is located:
 *  <0 is no decimal point and leading zero blanking
 *  0  is rightmost (and leading zero blanking)
 *  1  is one decimal place (and blanking)
 *  2  is two  " "
 *  3  is three decimal place
 *  >=4 is no decimal point but NO zero blanking
 */
  int temp,blank;

  temp= val/0x1000;
  if( temp == 0 && decimal_position<3){
    blank=1;
    temp=0x0F;
  }
  else blank=0;

  if(decimal_position==3) display(3,(unsigned char)temp,(unsigned char)1);
  else display(3,(unsigned char)temp,(unsigned char)0);      
  delay_ms_abs(DELAY);

  temp= (val%0x1000)/0x100;
  if( temp == 0 && blank==1 && decimal_position<2){
    temp=0x0F;
  }
  else blank=0;

  if(decimal_position==2) display(2,(unsigned char)temp,(unsigned char)1);
  else display(2,(unsigned char)temp,(unsigned char)0);      
  delay_ms_abs(DELAY);

  temp= (val%0x100)/0x10;
  if( temp == 0 && blank==1 && decimal_position < 1){
    temp=0x0F;
  }
  else blank=0;

  if(decimal_position==1) display(1,(unsigned char)temp,(unsigned char)1);
  else display(1,(unsigned char)temp,(unsigned char)0);      
  delay_ms_abs(DELAY);

  temp= val%0x10;
  if(decimal_position==0) display(0,(unsigned char)temp,(unsigned char)1);
  else display(0,(unsigned char)temp,(unsigned char)0);      
  delay_ms_abs(DELAY);

}/* end of display_hexadecimal() */

/**********************************************************************************************/
void display_off(void){
  //PTM=0x08; // off
  PTM_PTM3 = 1;
  PTM_PTM4 = 0;
  PTM_PTM5 = 0;
}/* end display_off() */

/**********************************************************************************************/
void flash_halt(void){
/* Flash the display every half second.
 */		
int i;

  RIGHT_LED_ON(FALSE);
  LEFT_LED_ON(FALSE);
  for(;;){
    for(i=0;i<500/(4*DELAY);i++){
      display_decimal(0,-1);
    }
    display_off();
    delay_ms_abs(500);
  }/* flash forever */
}/* end flash_halt() */


/**********************************************************************************************/
void AtoD(void){
/*
 * A/D for potentiometer
 */
int pot;

  for(;;){
    ATDCTL5 = 0x04;
    while( (ATDSTAT0 & 0x80)==0 ){ };
    pot=ATDDR0H;
    display_decimal(pot,0);
  }
}/* end AtoD() */

/**********************************************************************************************/
void board_test(void){
/* Test program
 * 
 */ 
unsigned char num;
int i,j;

  /*
   * Test of SW2 on module:  PP1 (port p bit 1)
   */

 
  RIGHT_LED_ON(FALSE);
  LEFT_LED_ON(TRUE);
    
  for(;;) {
   
    for(num=0;num<16;++num){
      PTAD= PTAD ^ 0xC0;     /* toggle LED4 and LED5 */
      for(i=0;i<30;i++){		 
        for(j=0;j<4;++j){		 /* show display in all 4 digits */
          display(j,num,(unsigned char)(num%2));
          
          delay_ms_abs(DELAY);
        }
      }
    }

    for(j=0;j<4;++j){
      for(num=0;num<16;++num){
        PTAD= PTAD ^ 0xC0; /* toggle LED4 and LED5 */
        for(i=0;i<60;i++){
          display(j,num,(unsigned char)(num%2));  /* test each digit separately */
          delay_ms_abs(DELAY);
        } /* end i */
      } /* end num */
    }/* end j */
  
//  do{
//  }while ( !scichar_ready() );
  
  }/* end forever loop */
}/* end of board_test() */


/**********************************************************************************************/
void forward_ms(int dur){
/*
 * Engage motors for forward motion of 'dur' milliseconds
 */
  Speed_right = RIGHT_FORWARD;
  Speed_left  = LEFT_FORWARD;
  MOTOR_INTERRUPTS_ON(TRUE);
  LEFT_LED_ON(FALSE);
  RIGHT_LED_ON(FALSE);
  delay_ms(dur);
  MOTOR_INTERRUPTS_ON(FALSE);
}/* end forward_ms() */

/**********************************************************************************************/
void right_ms(int dur){
/*
 * Engage motors for right rotation of 'dur' milliseconds
 */
  Speed_right = RIGHT_BACKWARD;
  Speed_left  = LEFT_FORWARD;
  MOTOR_INTERRUPTS_ON(TRUE);
  LEFT_LED_ON(FALSE);
  RIGHT_LED_ON(TRUE);
  delay_ms(dur);
  MOTOR_INTERRUPTS_ON(FALSE);
}/* end right_ms() */

/**********************************************************************************************/
void left_ms(int dur){
/*
 * Engage motors for left rotation of 'dur' milliseconds
 */
  Speed_right = RIGHT_FORWARD;
  Speed_left  = LEFT_BACKWARD;
  MOTOR_INTERRUPTS_ON(TRUE);
  LEFT_LED_ON(TRUE);
  RIGHT_LED_ON(FALSE);
  delay_ms(dur);
  MOTOR_INTERRUPTS_ON(FALSE);
}/* end left_ms() */

/**********************************************************************************************/
void backward_ms(int dur){
/*
 * Engage motors for backward motion of 'dur' milliseconds
 */
  Speed_right = RIGHT_BACKWARD;
  Speed_left  = LEFT_BACKWARD;
  MOTOR_INTERRUPTS_ON(TRUE);
  LEFT_LED_ON(TRUE);
  RIGHT_LED_ON(TRUE);
  delay_ms(dur);
  MOTOR_INTERRUPTS_ON(FALSE);
}/* end backward_ms() */


/**********************************************************************************************/
void five_seconds_forward(void){
 /*
  * Turns motors on and runs forward for 5 seconds, then stops. Stop if bump (falls through forward_ms() ).
  *
  */
  
  /* Do a countdown on the display each second.
   */
  display(0,5,0);
  forward_ms(MS(1000));
	display(0,4,0);
  forward_ms(MS(1000));
	display(0,3,0);
  forward_ms(MS(1000));
	display(0,2,0);
  forward_ms(MS(1000));
	display(0,1,0);
  forward_ms(MS(1000));

	display(0,0,0);

  flash_halt();
    
}/* end five seconds forward */


/**********************************************************************************************/
void forward_backward(void){
  /*
   * Run alternately forward 2 seconds and backward 2 seconds, repeatedly.  Stop if bump.
   */
       
  for(;;){
    forward_ms(MS(2000));
    if(Bump != 0x03) break;
    delay_ms(200);  // avoid abrupt direction change
    if(Bump != 0x03) break;
    
    backward_ms(MS(2000));
    if(Bump != 0x03) break;
    
    delay_ms(200);  // avoid abrupt direction change
    if(Bump != 0x03) break;
  }/* loop until bump */

  flash_halt();
    
}/* end forward backward */


/**********************************************************************************************/
void dance(void){
  /*
   * Run alternately forward 2 seconds and rotate 2 seconds, repeatedly.  Stop if bump.
   */

  for(;;){
    forward_ms(MS(2000)); // forward
     if(Bump != 0x03) break;
		left_ms(MS(2000)); // rotate
     if(Bump != 0x03) break;
  }/* loop until bump */

  flash_halt();

}/* end dance */

 
/**********************************************************************************************/
void bumper_bot(void){
/*
 * Run forward until switch bump.
 * If left switch hit, back up and turn right.
 * If right switch hit, back up and turn left.
 */
volatile int bump;
int i;

  Speed_right= RIGHT_FORWARD;	 
  Speed_left= LEFT_FORWARD;
  MOTOR_INTERRUPTS_ON(TRUE);	 // Start rollin' !
  
  for(;;){

    do{	 bump=PTAD&0x03;			 // Stay here and look for any bump contacts
      }while( bump==0x03 );

    MOTOR_INTERRUPTS_ON(FALSE);	 // Stop motors on bump

    if( bump == 1){	 // hit right bumper
      RIGHT_LED_ON(TRUE);
      LEFT_LED_ON(FALSE);
    }
    else{						 // hit left bumper
      RIGHT_LED_ON(FALSE);
      LEFT_LED_ON(TRUE);
    }
  
    /* illuminate the display for just 50ms */
    for(i=0;i<50/(4*DELAY);i++){  
      display_decimal(8888,-1);
    }
    RIGHT_LED_ON(FALSE);
    LEFT_LED_ON(FALSE);
    display_off();

    /* Now back up for 2 seconds */
    Speed_right= RIGHT_BACKWARD;
    Speed_left= LEFT_BACKWARD;
    MOTOR_INTERRUPTS_ON(TRUE);
    delay_ms_abs(MS(2000));

    if( bump == 1 ){
    //hit right switch, so turn left
      RIGHT_LED_ON(FALSE);
      LEFT_LED_ON(TRUE);

      Speed_right= RIGHT_FORWARD;
      Speed_left=  LEFT_BACKWARD;
    }
    else{
    //hit left switch, so turn right
      RIGHT_LED_ON(TRUE);
      LEFT_LED_ON(FALSE);

      Speed_right= RIGHT_BACKWARD;
      Speed_left=LEFT_FORWARD;
    }

    delay_ms(MS(675));		   // execute the turn

    RIGHT_LED_ON(FALSE);
    LEFT_LED_ON(FALSE);
    Speed_right= RIGHT_FORWARD;		// now return to forward direction
    Speed_left=  LEFT_FORWARD;
  } /* end for() */

}/* end bumper_bot */

/**********************************************************************************************/
void prog_handler(void){
/*
 * This function is installed as a subroutine called from RTI_handler().  It is called
 * at the real time interrupt rate, currently every 1.024ms.
 * 
 * The function reads the 2-byte commands from the Bunch_o_mem array, decodes the command,
 * and sets up the motor and LED parameters accordingly.
 *
 * The function "uninstalls itself" if it encounters a zero 2-byte command or if it encounters
 * a HALT command in the user RAM buffer (Bunch_o_mem).
 */
static int term_count,first_time=1;
int command,duration;

if( first_time ){
  term_count=Vcnt;
  first_time=0;
}

if( term_count != Vcnt ) return;   // Previous command still going, so continue

if( Step_num >= MAX_STEP){
  MOTOR_INTERRUPTS_ON(FALSE);      // Ran out of command memory
  Rti_func = NULL;
  return;
}

  Step_num++;
/*
 * Command code is upper 3 bits.
 * Duration in milliseconds is lower 13 bits.
 *
 * The command encoding is:
 * 15 14 13 12 11 10  9  8  7  6  5  4  3  2  1  0
 *  |  |  |  |  |  |  |  |  |  |  |  |  |  |  |  |
 *   -----		-----------------------------------
 *    CODE						 DURATION (milliseconds)
 *
 *   000 - go forward
 *   001 - turn left
 *   010 - turn right
 *   011 - go backward
 *   100 - sleep (no motion)
 *   101 -   "
 *   110 -   "
 *   111 - HALT (stop moving and exit)
 *
 *   Duration maximum:  2^13  -1  = 8191 ticks (1 tick = 1.024 milliseconds) ~=   8.39 seconds
 *   Longer durations can be simulated by repeating the same control code on subsequent commands.
 * 
 */
 
  command = (Bunch_o_mem[Step_num] & 0xE000)>> 13;
  duration = Bunch_o_mem[Step_num] & 0x1FFF;
  term_count = Vcnt + duration;
  
if(Bunch_o_mem[Step_num] == 0){  // exit if a 2-byte zero was encountered
  MOTOR_INTERRUPTS_ON(FALSE);
  Rti_func = NULL;
  return;
}

switch(command){
  case 0: // go forward
          Speed_right= RIGHT_FORWARD;
          Speed_left=  LEFT_FORWARD;
          MOTOR_INTERRUPTS_ON(TRUE);
          RIGHT_LED_ON(FALSE);
          LEFT_LED_ON(FALSE);
          break;
  case 1: // turn left
          Speed_right= RIGHT_FORWARD;
          Speed_left=  LEFT_BACKWARD;
          MOTOR_INTERRUPTS_ON(TRUE);
          RIGHT_LED_ON(FALSE);
          LEFT_LED_ON(TRUE);
          break;
  case 2: // turn right
          Speed_right= RIGHT_BACKWARD;
          Speed_left=  LEFT_FORWARD;
          MOTOR_INTERRUPTS_ON(TRUE);
          RIGHT_LED_ON(TRUE);
          LEFT_LED_ON(FALSE);
          break;
  case 3: // go backward
          Speed_right= RIGHT_BACKWARD;	 
          Speed_left=  LEFT_BACKWARD;
          MOTOR_INTERRUPTS_ON(TRUE);
          RIGHT_LED_ON(TRUE);
          LEFT_LED_ON(TRUE);
          break;
  case 4: // sleep  (note that 5 and 6 could be used for other command types)
  case 5:
  case 6:
          Speed_right= RIGHT_FORWARD;
          Speed_left=  LEFT_FORWARD;
          MOTOR_INTERRUPTS_ON(FALSE);
          RIGHT_LED_ON(FALSE);
          LEFT_LED_ON(FALSE);
          break;
  case 7: // HALT
  default:
          MOTOR_INTERRUPTS_ON(FALSE);
          Rti_func = NULL;
          RIGHT_LED_ON(FALSE);
          LEFT_LED_ON(FALSE);
          break;
}/* end switch */
  
} /* end prog_handler() */

/**********************************************************************************************/
void prog_bot(void){
/*
 * This function sets up the user programmable command sequence.
 * The user must load the sequence of 2-byte encoded commands to
 * processor RAM in the 'Bunch_o_mem' location (currently 0x0E00-0x0FFF).
 *
 * The command encoding is:
 * 15 14 13 12 11 10  9  8  7  6  5  4  3  2  1  0
 *  |  |  |  |  |  |  |  |  |  |  |  |  |  |  |  |
 *   -----		-----------------------------------
 *    CODE						 DURATION (milliseconds)
 *
 *   000 - go forward
 *   001 - turn left
 *   010 - turn right
 *   011 - go backward
 *   100 - sleep (no motion)
 *   101 -   "
 *   110 -   "
 *   111 - HALT (stop moving and exit)
 *
 *   Duration maximum:  2^13  -1  = 8191 ticks (1 tick = 1.024 milliseconds) ~=   8.39 seconds
 *   Longer durations can be simulated by repeating the same control code on subsequent commands.
 * 
 * The setup installs a special function that is called from within the RTI_handler() interrupt
 * service routine: the global function pointer Rti_func is assigned prog_handler().
 *
 * This function goes into a loop to display the current step in the command sequence, and monitors
 * the bumper switches for contact.  If the bumper switches close, the robot sequence stops.
 */
int i;
volatile char sw2;
volatile unsigned char cmd, adr_hi, adr_lo, count;
volatile char *bom;

sw2=1;

/* Display the RAM hexadecimal address for 3 seconds.  Note that the display characters for
 * ABCDEF are the 74LS47 display codes, not the letters.
 *
 * IF user presses SW2 on the microprocessor module during the addr display,
 * wait for the serial command download sequence.
 * Otherwise proceed to excecute commands.
 */
  for(i=0;i<3000/(4*DELAY);i++){
    display_hexadecimal((unsigned int) Bunch_o_mem,4);
    if( (PTIP&0x02) == 0 ){ // watch for SW2 press!
    sw2=0;
    break;
    }
  }
  
 if( sw2 == 0){
  // SW2 was pressed, so wait for data download
  /*
   * 0D (cr)
   * A8 (byte download)
   * 3E (dest adr hi)
   * 00 (dest adr lo)
   * FF (count)
   * xx byte
   * xx byte
   * xx byte
   * ...
   */
   bom = (char *) Bunch_o_mem;
    
   cmd=getchar(); // CR
   cmd=getchar(); // A8
   if(cmd != (unsigned char) 0xA8 ) goto ERR_HALT;
   adr_hi=getchar();
   adr_lo=getchar();
   count=getchar();
   for(i=0;i<= (int) count;i++){
      bom[i]=getchar();
   }
   /* Sequence download complete:  flash "1111".
    * User should now reset (and not press SW2) for command sequence execution.
    */
    for(;;){ // TRAP here
     for(i=0;i<500/(4*DELAY);i++){
      display_decimal(1111,-1);
     }
     display_off();
     delay_ms_abs(500);
    }
 }// end SW2 check
  Step_num = -1;
  Rti_func = prog_handler ;  // Install the pointer to the function called from within RTI_handler()

  while( Rti_func != NULL){
    display_decimal(Step_num,-1);		 // Display the step number within the user's program sequence
    if( ((PTAD&0x03) != 0x03)  ){ // check bump
					MOTOR_INTERRUPTS_ON(FALSE); // If bump, shut down motors and halt.
          Rti_func = NULL;
          for(;;){
            RIGHT_LED_ON(FALSE);
            LEFT_LED_ON(FALSE);
            delay_ms_abs(500);
            RIGHT_LED_ON(TRUE);
            LEFT_LED_ON(TRUE);
            delay_ms_abs(150);
          } // trap here forever...
    }

  }/* end while */

/*
 * User 'Halt' command encountered, so just flash final step number (on and off)
 */
ERR_HALT:
  for(;;){
    for(i=0;i<500/(4*DELAY);i++){
      display_decimal(Step_num,-1);
    }
    display_off();
    delay_ms_abs(500);
  }

}/* end prog_bot */


/**********************************************************************************************/
void figure_eight(void){
/*
 * Program to attempt a figure-8 pattern.  Robot starts in the lower left corner, proceeds up,
 * across middle, up to top, across top to upper left, then goes down, across middle, down, and
 * across bottom back to the starting point; then repeats.
 */

  for(;;){
  //forward 4 LL
    forward_ms(4000);
     if(Bump != 0x03) break;
  //right	 mid
    right_ms(675);
     if(Bump != 0x03) break;
  //forward 4 mid
    forward_ms(4000);
     if(Bump != 0x03) break;
  //left UR
    left_ms(675);
     if(Bump != 0x03) break;
  //forward 4  UR
    forward_ms(4000);
     if(Bump != 0x03) break;
  //left	 TOP
    left_ms(675);
     if(Bump != 0x03) break;
  //forward 4 TOP
    forward_ms(4000);
     if(Bump != 0x03) break;
  //left	 UL
    left_ms(675);
     if(Bump != 0x03) break;
  //forward 4 UL
    forward_ms(4000);
     if(Bump != 0x03) break;
  //left mid
    left_ms(675);
     if(Bump != 0x03) break;
  //forward 4 mid
    forward_ms(4000);
     if(Bump != 0x03) break;
  //right	 LR
    right_ms(675);
     if(Bump != 0x03) break;
  //forward 4 LR
    forward_ms(4000);
     if(Bump != 0x03) break;
  //right	 bot
    right_ms(675);
     if(Bump != 0x03) break;
  //forward 4 bot
    forward_ms(4000);
     if(Bump != 0x03) break;
  //right	 LL reorient
    right_ms(675);
     if(Bump != 0x03) break;
  //  repeat!
  } /* loop until bump */

  flash_halt();

}/* end figure_eight() */
 

/**********************************************************************************************/
/**********************************************************************************************/
/*
 * START OF MAIN()
 *
 */
/**********************************************************************************************/
void main(void) {
 
volatile int i;
//volatile int num,dispcnt;
volatile unsigned char pat;
//unsigned char bump;

/* preset test
Bunch_o_mem[0]= 0x0000 | 1191 ; //forward
Bunch_o_mem[1]= 0x2000 | 1191 ; //left
Bunch_o_mem[2]= 0x4000 | 1191 ; //right
Bunch_o_mem[3]= 0x6000 | 1191 ; //back
Bunch_o_mem[4]= 0x8000 | 1191 ; //sleep
Bunch_o_mem[5]= 0xA000 | 1191 ; //sleep
Bunch_o_mem[6]= 0xC000 | 1191 ; //sleep
Bunch_o_mem[7]= 0xE000 | 1191 ; // HALT
*/

/*
 * Set up oscillator phase lock loop (PLL) for 24MHz bus speed.
 * (Procedure taken from HCS12 serial monitor code).
 */

CLKSEL_PLLSEL  = 0;	 // Disengage PLL
PLLCTL_PLLON   = 1;	 // Turn on PLL

//RCM 080915 for CSMB12 module with 4MHz instead of 8MHz on C32, set mult to 6 and div to 1

//SYNR  = 0x02;  // Set loop multiplier to 3 (SYNR = 2, mult=SYNR + 1)
//RCM 070703 comm REFDV = 0x00;	 // Set input osc divider to 1 (REFDV = 0, div = REFDV+1)
//REFDV = 0x01;

SYNR  = 0x05;  // Set loop multiplier to 6 (SYNR = 5, mult=SYNR + 1)
REFDV = 0x00;  // set input osc divider to 1 (REFDV = 0, div =REFDV+1)


_asm("nop"); _asm("nop");   // Delay for stabilization

while( CRGFLG_LOCK != 1){}; // Wait until clock circuit locks on

CLKSEL_PLLSEL  = 1;					// Engage PLL and go!
 

/*
 * Set up initial port configurations.
 */

DDRAD=0xC0;	 /* set AN7 and AN6 to be outputs (LED4 and LED5) */

PERAD_PERAD0 = 1; /* engage pull-up on sensor pins (0 left and 1 right); makes OK if bumper switches not connected */
PERAD_PERAD1 = 1;

//pushbuttons on CSMB12 module
DDRP_DDRP0 = 0;  //Enable as Input (PB1)
DDRP_DDRP1 = 0;  //Enable as Input (PB2)
PERP_PERP0 = 1;  //Enable Internal Pull-ups
PERP_PERP1 = 1;  //Enable Internal Pull-ups


ATDDIEN = 0x0F;			  /* set AN0-AN3 as inputs (bumpers) */

/* set digit select bits of port M to be outputs
 * 
 */
DDRM=0x3C; /* bits 5:4:3 of M register go to decoder, bit 2 is decimal point */
PTM=0xFF;  /*  */

/* set segment select bits of port T to be outputs */
DDRT=0xF1; /* upper 4 bits of T register go to 7-seg decoder */
PTT=0xFF;

LEFT_LED_ON(FALSE);
RIGHT_LED_ON(FALSE);

/*
 * TSCR1_TEN:  bit to enable entire timer system
 * TIOS:  one bit per channel (bit=1 enables output capture)
 * TFLG1: one bit per channel (write 1 to bit to reset flag)
 * TCn:  16 bit register to hold compare value
 * TIE_CnI: bit enables channel interrupt (bit=1 enables)
 * TCTL2: pairs of bits set mode for external pin:
 *        OM3:OL3 OM2:OL2 OM1:OL1 OM0:OL0 11=set; 10=clear; 00=no action; 01=toggle
 */
 
/* setup for output compare on T 0
 */
TIOS=TIOS|0x01; //make channel 0 an output compare
TC0 = 0x100; // load the compare count

TCTL2= 0x03; // 0x03=set; 0x02=clear; 0x00=no action; 0x01 toggle

TSCR1_TEN = 1; //enable timer system
// TIE_C0I = 1; //enable interrupt on timer chan 0
TFLG1 = 0x01; //clear timer ch0 flag

/* setup for output compare on T 1
 */
TIOS= TIOS | 0x02; //make channel 1 an output compare
TC1 = 0x100; // load the compare count

TCTL2= TCTL2 | 0xC0; // 11=set; 10=clear; 00=no action; 01=toggle

// TIE_C1I = 1; //enable interrupt on timer chan 1
TFLG1 = 0x02; //clear timer ch1 flag

Speed_right= RIGHT_FORWARD;	 
Speed_left = LEFT_FORWARD;

MOTOR_INTERRUPTS_ON(FALSE);

/* configure A/D converter, AN4 */
ATDCTL2 |= 0x80;
ATDCTL2 &= (~0x62);
ATDCTL4 &= (~0x61);


/*
 * Start serial port
 */
initsci();

/*
 * Real Time Interrupt control setup
 */
 //RCM 070703 comm RTICTL = 0x17; /* 0x27? 1ms rti period */

//was RCM 080915 RTICTL = 0x27;
RTICTL = 0x13;    //approx 976 microsec with 4MHz clock

 CRGFLG_RTIF = 1;  // Set RTI flag to reset counter
 CRGINT_RTIE = 1;  // Enable RTI interrupt

EnableInterrupts;

/*
 * Read DIP switches and dispatch
 */
pat=read_switches();
pat = (pat>>4)^0x0f;

/*
 * Display revision code, for 1.5 seconds
 */ 
for(i=0;i<1500/(2*DELAY);i++){
  display(1,CODE_REV_MAJOR,1);
  delay_ms(DELAY);
  display(0,CODE_REV_MINOR,0);
  delay_ms(DELAY);
}

/*
 * Display program number (DIP switch pattern), for 1.5 seconds
 */
for(i=0;i<1500/(4*DELAY);i++){
 display_decimal(pat,-1);
}

display_off();


switch(pat){
  case 0:
  default:
          board_test();
          break;
  case 1:
          AtoD();
          break;
  case 2:
          five_seconds_forward();
          break;
  case 3:
          forward_backward();
          break;
  case 4:
          dance();
          break;
  case 5:
          bumper_bot();
          break;
  case 6:
          prog_bot();
          break;
  case 7:
          // RCM figure_eight();
          break;
            
}/* end switch */


  for(;;) {} /* wait forever (should not reach this trap line) */

}/* end of main() */



/**********************************************************************************************/
/*
 * End of file
 *
 */
/**********************************************************************************************/