Example Source Code:
CCS PIC-C Test Program
For the WheelCommander WC-132


Description

This example originally came from a customer, Jim Herd, and was written for mikroC. We ported it to CCS PIC-C.
 

Download

wheelcmd2.c - CCS PIC-C source
wheelcmd2.pjt - CCS PIC-C project file

 

Source Code


wheelcmd2.c


//
// Project name: wheelcmd2
//
// Originally Done by:
//   Jim Herd, Feb. 2006
// Adapted to PIC-C by:
//   Pete Skeggs
//
// Description:
//   Test program for WattBot I with attached WheelCommander and
//   WheelWatcher units.
//
// Test configuration:
//   MCU:             P18F452
//   Oscillator:      HS, 40.0000MHz
//   SW:              mikroC v5.003 --> ported to CCS PIC-C
//
//************************************************************************
// Type declarations
//************************************************************************
//

#ifdef MICROCHIP_C
#include <p18f452.h>
#else
#include <18f452.h>
#include <stdio.h>
#use i2c(master, sda=PIN_C4, scl=PIN_C3)
#use delay(clock=20000000)
#use rs232(BAUD=38400,xmit=PIN_C6,rcv=PIN_C7)
#fuses HS, DEBUG, NOWDT
#endif

#include    <delay.h>

#ifdef MICROCHIP_C
typedef    unsigned char  UINT8;
typedef    signed char    INT8;
typedef    unsigned short UINT16;
typedef    signed short   INT16;
typedef    unsigned long  UINT32;
typedef    signed long    INT32;
#else
typedef    unsigned char  UINT8;
typedef    unsigned long UINT16;
typedef    unsigned int32  UINT32;
#endif

//************************************************************************
// Macros
//************************************************************************
//
#define    I2CPAUSE      I2CIdle()
#define    HANG           for(;;) { ; }

//************************************************************************
// Constant declarations
//************************************************************************
//
// I2C baud rate generator constants
//
#define     I2C_100KHZ          0x59
#define     I2C_400KHZ          0x0E

/* SSPCON1 REGISTER */
#define   SSPENB    0x20  /* Enable serial port and configures
                             SCK, SDO, SDI                      */
#define   SLAVE_7   6     /* I2C Slave mode, 7-bit address      */
#define   SLAVE_10  7     /* I2C Slave mode, 10-bit address     */
#define   MASTER    8     /* I2C Master mode                    */

/* SSPSTAT REGISTER */
#define   SLEW_OFF  0xC0  /* Slew rate disabled for 100kHz mode */
#define   SLEW_ON   0x00  /* Slew rate enabled for 400kHz mode  */
//
// Relevant I2C addresses
//
#define     WHEELCMD_ADDRESS    0x10
#define     WHEELCMD_READ       0x21
#define     WHEELCMD_WRITE      0x20
//
// vehicle parameters
//
#define     WHEEL_CIRCUMFERENCE      210      // mm
#define     WHEEL_BASE               150      // mm
//
// Addresses of accessible constants
//
#define     CONST_MODE              2
#define     CONST_WHEELBASE        21
#define     CONST_WHEELCIRCUM      22
//
// Definition of bits in mode byte (constant address 2)
//
#define     MODE_BLANK                   0x00
#define     MODE_SLOW_COMMS              0x01
#define     MODE_NORMAL_COMMS            0x00
#define     MODE_NOTIFY_ON_COMPLETE      0x02
#define     MODE_NOTIFY_ON_RECIEVE       0x00
#define     MODE_NO_ECHO_CMD             0x04
#define     MODE_ECHO_CMD                0x00
#define     MODE_DIRECTION_SENSE_FLIP    0x08
#define     MODE_DIRECTION_SENSE_NO_FLIP 0x00
#define     MODE_PWM                     0x10
#define     MODE_RC                      0x00
#define     MODE_PWM_LOCK_ANTIPHASE      0x20
#define     MODE_PWM_SIGN_MAG            0x00
#define     MODE_PWM_DUAL_DIRECTION      0x40
#define     MODE_PWM_SINGLE_DIRECTION    0x00
#define     MODE_SHAFTS_OPPOSITE         0x80
#define     MODE_SHAFTS_SAME             0x00
//
#define MODE_BITS_WATTBOT_I  (MODE_BLANK | MODE_DIRECTION_SENSE_FLIP | MODE_PWM | MODE_PWM_SIGN_MAG |MODE_SHAFTS_OPPOSITE)
//
// general system constants
//
#define     I2C_Read_NACK          0
#define     I2C_Read_ACK           1

#ifndef MICROCHIP_C
struct SSPCON1_layout
{
   int SSPM : 4;
   int CKP : 1;
   int SSPEN : 1;
   int SSPOV : 1;
   int WCOL : 1;
};
struct SSPCON1_layout SSPCON1bits;
#byte SSPCON1bits = 0xfc6
#byte SSPCON1 = 0xfc6

struct SSPCON2_layout
{
   int SEN : 1;
   int RSEN : 1;
   int PEN : 1;
   int RCEN : 1;
   int ACKEN : 1;
   int ACKDT : 1;
   int ACKSTAT : 1;
   int GCEN : 1;
};
struct SSPCON2_layout SSPCON2bits;
#byte SSPCON2bits = 0xfc5
#byte SSPCON2 = 0xfc5

struct SSPSTAT_layout
{
   int BF : 1;
   int UA : 1;
   int R_W : 1;
   int S : 1;
   int P : 1;
   int DA : 1;
   int CKE : 1;
   int SMP : 1;
};
struct SSPSTAT_layout SSPSTATbits;
#byte SSPSTATbits = 0xfc7
#byte SSPSTAT = 0xfc7

#byte SSPADD  = 0xfc8
#byte SSPBUF  = 0xfc9

#byte TRISA   = 0xf92
#byte TRISB   = 0xf93

struct DDRC_layout
{
   int RC0 : 1;
   int RC1 : 1;
   int RC2 : 1;
   int RC3 : 1;
   int RC4 : 1;
   int RC5 : 1;
   int RC6 : 1;
   int RC7 : 1;
};
struct DDRC_layout DDRCbits;
#byte DDRCbits = 0xf94
#byte TRISC   = 0xf94

#byte TRISD   = 0xf95
#byte TRISE   = 0xf96

#byte PORTA   = 0xf80
#byte PORTB   = 0xf81
#byte PORTC   = 0xf82
#byte PORTD   = 0xf83
#byte PORTE   = 0xf84

struct PIR1_layout
{
   int TMR1IF : 1;
   int TMR2IF : 1;
   int CCP1IF : 1;
   int SSPIF : 1;
   int TXIF : 1;
   int RCIF : 1;
   int ADIF : 1;
   int PSPIF : 1;
};
struct PIR1_layout PIR1bits;
#byte PIR1bits = 0xf9e

#endif


#define     MODE_STOP_START       0x0F
#define     MODE_RESTART          0xF0
//
// commands
//
#define     SYNC_CMD             '.'
#define     CONSTANT_CMD         'F'
#define     GO_CMD               'G'
#define     COAST_CMD            'C'
#define     RESET_CMD            'R'
#define     GET_STATUS_CMD       'S'
#define     ECHO_CMD             'E'
#define     VELOCITY_CMD         'V'
#define     ROTATION_RATE_CMD    'Y'
#define     ANGLE_CMD            'W'
#define     DISTANCE_CMD         'D'

//
// error codes
//
#define     OK                         1
#define     ERROR                    128
#define     SYNC_FAIL            ERROR+1
#define     GET_CONSTANT_FAIL    ERROR+2
#define     SET_CONSTANT_FAIL    ERROR+3
#define     GO_FAIL              ERROR+4
#define     COAST_FAIL           ERROR+5
#define     RESET_FAIL           ERROR+6
#define     ECHO_FAIL            ERROR+7
#define     GET_STATUS_FAIL      ERROR+8
#define     SET_VELOCITY_FAIL    ERROR+9
#define     GET_VELOCITY_FAIL    ERROR+11
#define     SET_ROTATION_RATE_FAIL ERROR+10
#define     GET_ROTATION_RATE_FAIL ERROR+12
#define     SET_ANGLE_FAIL       ERROR+13
#define     GET_ANGLE_FAIL       ERROR+14
#define     SET_DISTANCE_FAIL    ERROR+15
#define     GET_DISTANCE_FAIL    ERROR+16

//************************************************************************
// System functions : prototypes.
//************************************************************************
//
void   sys_init(void);
void   WC_132_init(void);
void   do_sync(void);
void   do_set_constant(UINT8 const_address, UINT8 new_value);
UINT8  do_get_constant(UINT8 const_address);
int    check_set_constants(void);
UINT16 do_get_status(void);
void   debug(UINT8 code);
void   do_go(void);
void   do_coast(void);
void   do_reset(void);
void   do_echo(void);
void   exec_command(UINT8 mode);

// renamed to avoid conflict with CCS C's built in functions
void   I2COpen( unsigned char sync_mode, unsigned char slew );
UINT8  I2CWrite( unsigned char data_out );
UINT8  I2CWriteWait( unsigned char data_out );
UINT8  I2CRead( void );
void   I2CDone(void);
UINT8 I2CAckReceived(void);
void   I2CIdle(void);
void   I2CStop(void);
void   I2CStart(void);
void   I2CNotAck( void );
void   I2CAck( void );


//************************************************************************
// Global data
//************************************************************************
//
// WC_command holds the details of a full I2C command and the returned data.
//
struct
{
    UINT8 cmd[6];                                                                                  // command byte list
    int   send_count;                                                                              // number of bytes to be sent
    UINT8 reply[6];                                                                                // list of byte values received
    int   get_count;                                                                               // number of bytes to be read
    UINT8 delay;                                                                                   // delay in mS between command write and data read
    UINT8 status;                                                                                  // error code or OK
} WC_command;

#define hi(x)  (*(&x+1))

//************************************************************************
//************************************************************************
// System functions : code.
//************************************************************************
//
// sys_init : initialise PIC processor
// ========
//
void sys_init(void)
{
    DelayMs(254);

    TRISB = 0;
    PORTB = 0xFF;

    I2COpen(MASTER, SLEW_OFF);
    SSPADD= I2C_400KHZ;

    DelayMs(1);
}



//************************************************************************
// I2COpen   Open I2C unit
// ========
//
void I2COpen( unsigned char sync_mode, unsigned char slew )
{
    SSPSTAT &= 0x3F;                                                                               // power on state
    SSPCON1 = 0x00;                                                                                // power on state
    SSPCON2 = 0x00;                                                                                // power on state
    SSPCON1 |= sync_mode;                                                                          // select serial mode
    SSPSTAT |= slew;                                                                               // slew rate on/off

    DDRCbits.RC3 = 1;                                                                              // Set SCL (PORTC,3) pin to input
    DDRCbits.RC4 = 1;                                                                              // Set SDA (PORTC,4) pin to input

    SSPCON1 |= SSPENB;                                                                             // enable synchronous serial port
}

//************************************************************************
// I2CWrite   Write a single byte
// =========
//
UINT8 I2CWrite( unsigned char data_out )
{
    SSPBUF = data_out;                                                                             // write single byte to SSPBUF
    if ( SSPCON1bits.WCOL )                                                                        // test if write collision occurred
        return( 0xff );                                                                            // if WCOL bit is set return negative #
    else
    {
        while ( SSPSTATbits.BF );                                                                  // wait until write cycle is complete
        return( 0 );                                                                               // if WCOL bit is not set return non-negative #
    }
}


UINT8 I2CWriteWait(unsigned char data_out)
{
   UINT8 ret;
   
   ret = I2CWrite(data_out);
   while (!I2CAckReceived());
   I2CIdle();
   return ret;
}


//************************************************************************
// I2CRead   Read a single byte
// ========
//
UINT8 I2CRead( void )
{
    UINT8 ret;
    
    SSPCON2bits.RCEN = 1;                                                                          // enable master for 1 byte reception
    while ( !SSPSTATbits.BF );                                                                     // wait until byte received
    ret = SSPBUF;                                                                                  // return with read byte
    I2CIdle();
    return ret;
}

//************************************************************************
// I2CDone   Check I2C action that is completed
// ========
//
void I2CDone(void)
{
    while (!PIR1bits.SSPIF)
        ;                                                                                          // Completed the action when the SSPIF is Hi.
    PIR1bits.SSPIF=0;                                                                              // Clear SSPIF
}


UINT8 I2CAckReceived(void)
{
   return !SSPCON2bits.ACKSTAT ? 1 : 0;
}

//************************************************************************
// I2CIdle   Test and wait until I2C module is idle
// =======
//
void I2CIdle( void )
{
    while ( ( SSPCON2 & 0x1F ) | ( SSPSTATbits.R_W ) )
        continue;
}

//************************************************************************
// I2CStop   Send I2C bus stop condition
// ========
//
void I2CStop( void )
{
    SSPCON2bits.PEN = 1;                                                                           // initiate bus stop condition
    I2CIdle();
}

//************************************************************************
// I2CRestart   initiate bus restart condition
// ===========
//
void I2CRestart( void )
{
    SSPCON2bits.RSEN = 1;                                                                          //
    I2CIdle();
}

//************************************************************************
// I2CStart   Send I2C bus start condition
// ========
//
void I2CStart( void )
{
    SSPCON2bits.SEN = 1;                                                                           // initiate bus start condition
    I2CIdle();
}

//************************************************************************
// I2CNotAck
// ==========
//
void I2CNotAck( void )
{
    SSPCON2bits.ACKDT = 1;                                                                         // set acknowledge bit for not ACK
    SSPCON2bits.ACKEN = 1;                                                                         // initiate bus acknowledge sequence
    I2CIdle();
}

//************************************************************************
// I2CAck
// =======
//
void I2CAck(void)
{
    SSPCON2bits.ACKDT = 0;                                                                         // set acknowledge bit state for ACK
    SSPCON2bits.ACKEN = 1;                                                                         // initiate bus acknowledge sequence
    I2CIdle();
}

//************************************************************************
// exec_command : execute a command to check WheelCommander unit.
// ============
//

void exec_command(UINT8 mode)
{
    int i, count;
// PLS March 9 2006: I2CIdle() must be used instead of I2CDone() to ensure I2C hardware unit is ready
// for next operation.  When I2CDone() is used instead, long delays must be inserted to ensure that
// the unit is ready.  Use of I2CIdle() without delay loops makes exec_command() very fast.

    I2CIdle();
    I2CStart();
    I2CWriteWait(WHEELCMD_WRITE);
    
    for (i=0 ; i<WC_command.send_count ; i++)
        I2CWriteWait(WC_command.cmd[i]);
    
    if (mode == MODE_RESTART)
        I2CRestart();
    else
    {                                                                                              // must be MODE_STOP_START
        I2CStop();
        if (WC_command.delay != 0)
            DelayMs(WC_command.delay);
        else
            DelayUs(10);
        I2CStart();
    }
    
    I2CWriteWait(WHEELCMD_READ);
    
    count = WC_command.get_count - 1;
    for (i = 0; i < count; i++)
    {
        WC_command.reply[i] = I2CRead();
        I2CAck();
    }
    WC_command.reply[WC_command.get_count-1] = I2CRead();
    I2CNotAck();
    I2CStop();
}

#if 0

void i2c_wait_ready(void)
{
    while (SSPCON2 & 0x40);
}

void i2c_idle( void )
{
    while ( ( SSPCON2 & 0x1F ) | ( SSPSTAT & 0x04 ));
}

void i2c_data_ready(void)
{
    while ((SSPSTAT & 0x01) == 0);                                                                    // wait until byte received
}

// CCS C version
void exec_command(UINT8 mode)
{
    int i, count;
// PLS March 9 2006: I2CIdle() must be used instead of I2CDone() to ensure I2C hardware unit is ready
// for next operation.  When I2CDone() is used instead, long delays must be inserted to ensure that
// the unit is ready.  Use of I2CIdle() without delay loops makes exec_command() very fast.
    i2c_start();
    i2c_write(WHEELCMD_WRITE);
    for (i=0 ; i<WC_command.send_count ; i++)
    {
        i2c_write(WC_command.cmd[i]);
    }
    if (mode == MODE_RESTART)
    {
        i2c_start();
    }
    else
    {                                                                                              // must be MODE_STOP_START
        i2c_stop();
        if (WC_command.delay != 0)
        {
            DelayMs(WC_command.delay);
        }
        i2c_start();
    }
    i2c_idle();
    i2c_write(WHEELCMD_READ);
    i2c_wait_ready();
    if (WC_command.get_count)
    {
       for (i = 0; i < (WC_command.get_count - 1); i++)
       {
           i2c_idle();
           DelayUs(20);
           //i2c_data_ready();
           //while (!i2c_poll());
           WC_command.reply[i] = i2c_read(1);   // ack on all but last
       }
       i2c_idle();
       DelayUs(20);
       //i2c_data_ready();
       //while (!i2c_poll());
       WC_command.reply[WC_command.get_count-1] = i2c_read(0); // no ack on last byte
    }
    i2c_stop();
    return;
}
#endif

//************************************************************************
// WC_132_init : initialise the WheelComander unit.
// ===========
//
void WC_132_init(void)
{
    int i;

    printf("WC-132 init:\r\nSync: ");
    for (i=0;i<10;i++)
    {
        printf(".");
        do_sync();
        debug(WC_command.status);
    }
    printf("\r\nEcho: ");
    for (i=0;i<10;i++)
    {
        printf(".");
        do_echo();
        debug(WC_command.status);
    }
    return;
}


//************************************************************************
// EEPROM Test Routines
// ====================
//
BOOLEAN ext_eeprom_ready()
{
   int1 ack;
   I2CIdle();
   I2CStart();            // If the write command is acknowledged,
   I2CWrite(0xa0);        // then the device is ready.
   ack = I2CAckReceived();
   I2CIdle();
   I2CStop();
   return ack;
}

void write_ext_eeprom(long int address, BYTE data) {
   while(!ext_eeprom_ready());
   I2CIdle();

   I2CStart();

   I2CWriteWait(0xa0);
   I2CWriteWait(hi(address));
   I2CWriteWait(address);
   I2CWriteWait(data);

   I2CStop();
}


BYTE read_ext_eeprom(long int address) {
   BYTE data;

   while(!ext_eeprom_ready());

   I2CIdle();
   I2CStart();

   I2CWriteWait(0xa0);
   I2CWriteWait(hi(address));
   I2CWriteWait(address);

   I2CRestart();

   I2CWriteWait(0xa1);
   data=I2CRead();
   I2CNotAck();

   I2CStop();

   return(data);
}


//************************************************************************
// eeprom_test
// ===========
//
void eeprom_test(void)
{
    int i;
    BOOLEAN good = true;
    unsigned int data;

    printf("Testing external EEPROM: ");
    for (i = 0; i < 10; i++)
    {
      write_ext_eeprom(i, 0x5a+i);
      if ((data = read_ext_eeprom(i)) != (0x5a+i))
         good = false;
      write_ext_eeprom(i, 0xa5+i);
      if ((data = read_ext_eeprom(i)) != (0xa5+i))
         good = false;
    }
    if (good)
       printf("PASSED!\r\n");
    else
       printf("FAILED!\r\n");
}


//************************************************************************
// do_sync : execute sync command to check WheelCommander unit.
// =======
//
void do_sync(void)
{

    WC_command.cmd[0] = SYNC_CMD;
    WC_command.send_count = 1;
    WC_command.get_count = 1;
    WC_command.delay = 0;
    exec_command(MODE_RESTART);
    if (WC_command.reply[0] == '.')
    {
        WC_command.status = OK;
    }
    else
    {
        WC_command.status = SYNC_FAIL;
        debug(WC_command.status);
    }
    return;
}

//************************************************************************
// do_get_constant : configure a WheelCommander constant
// ===============
// Description
//    Execute an 'F' command to read a specified constant.
// Parameters
//    const_address : UINT8   : address of constant (2->34 at 01/03/06)
// Return values
//    Function returns read value : UINT8
// Globals
//    WC_command structure to hold command details and returned data.
//
UINT8  do_get_constant(UINT8 const_address)
{
    WC_command.cmd[0] = CONSTANT_CMD;
    WC_command.cmd[1] = const_address;
    WC_command.send_count = 2;
    WC_command.get_count = 3;
    WC_command.delay = 0;
    exec_command(MODE_STOP_START);
    if (WC_command.reply[0] == CONSTANT_CMD)
    {
        WC_command.status = OK;
        return WC_command.reply[2];
    }
    else
    {
        WC_command.status = GET_CONSTANT_FAIL;
        debug(WC_command.status);
    }
    return 0;
}

//************************************************************************
// do_set_constant : configure a WheelCommander constant
// ===============
//
void do_set_constant(UINT8 const_address, UINT8 new_value)
{

    WC_command.cmd[0] = CONSTANT_CMD;
    WC_command.cmd[1] = const_address;
    WC_command.cmd[2] = new_value;
    WC_command.send_count = 3;
    WC_command.get_count = 1;
    WC_command.delay = 0;
    exec_command(MODE_STOP_START);
    if (WC_command.reply[0] == 'a')
    {
        WC_command.status = OK;
    }
    else
    {
        WC_command.status = SET_CONSTANT_FAIL;
        debug(WC_command.status);
    }
    return;
}

//************************************************************************
// do_get_status : get current operating mode of WheelCommander
// =============
// Description
//    Execute an 'S' command to read 16-bit status word.
// Parameters
//    None
// Return values
//    None
// Globals
//    WC_command structure to hold command details and returned status data.
//      WC_command.reply[0] = 'S'
//      WC_command.reply[1] = low byte of status word
//      WC_command.reply[2] = high byte of status word
//
UINT16 do_get_status(void)
{
    WC_command.cmd[0] = GET_STATUS_CMD;
    WC_command.send_count = 1;
    WC_command.get_count = 3;
    WC_command.delay = 0;
    exec_command(MODE_RESTART);
    if (WC_command.reply[0] == GET_STATUS_CMD)
    {
        WC_command.status = OK;
    }
    else
    {
        WC_command.status = GET_STATUS_FAIL;
        debug(WC_command.status);
    }
    return ((UINT16)WC_command.reply[1]) + (((UINT16)WC_command.reply[2]) << 8);
}


//************************************************************************
// do_set_velocity : set the speed of motion
// ===============
//
void do_set_velocity(INT16 velocity)
{

    WC_command.cmd[0] = VELOCITY_CMD;
    WC_command.cmd[1] = velocity & 0x0ff;
    WC_command.cmd[2] = (velocity >> 8) & 0x0ff;
    WC_command.send_count = 3;
    WC_command.get_count = 1;
    WC_command.delay = 0;
    exec_command(MODE_RESTART);
    if (WC_command.reply[0] == 'a')
    {
        WC_command.status = OK;
    }
    else
    {
        WC_command.status = SET_VELOCITY_FAIL;
        debug(WC_command.status);
    }
    return;
}


//************************************************************************
// do_get_velocity : get the speed of motion
// ===============
//
INT16 do_get_velocity(void)
{

    WC_command.cmd[0] = VELOCITY_CMD;
    WC_command.send_count = 1;
    WC_command.get_count = 3;
    WC_command.delay = 0;
    exec_command(MODE_STOP_START);
    if (WC_command.reply[0] == VELOCITY_CMD)
    {
        WC_command.status = OK;
        return(INT16)(((UINT16)WC_command.reply[1]) + (((UINT16)WC_command.reply[2]) << 8));
    }
    else
    {
        WC_command.status = GET_VELOCITY_FAIL;
        debug(WC_command.status);
    }
    return 0;
}


//************************************************************************
// do_set_rotation_rate: set the speed of rotation
// ===============
//
void do_set_rotation_rate(INT16 rotation_rate)
{

    WC_command.cmd[0] = ROTATION_RATE_CMD;
    WC_command.cmd[1] = rotation_rate & 0x0ff;
    WC_command.cmd[2] = (rotation_rate >> 8) & 0x0ff;
    WC_command.send_count = 3;
    WC_command.get_count = 1;
    WC_command.delay = 0;
    exec_command(MODE_STOP_START);
    if (WC_command.reply[0] == 'a')
    {
        WC_command.status = OK;
    }
    else
    {
        WC_command.status = SET_ROTATION_RATE_FAIL;
        debug(WC_command.status);
    }
    return;
}


//************************************************************************
// do_get_rotation_rate: get the speed of rotation
// ===============
//
INT16 do_get_rotation_rate(void)
{

    WC_command.cmd[0] = ROTATION_RATE_CMD;
    WC_command.send_count = 1;
    WC_command.get_count = 3;
    WC_command.delay = 0;
    exec_command(MODE_STOP_START);
    if (WC_command.reply[0] == ROTATION_RATE_CMD)
    {
        WC_command.status = OK;
        return(INT16)(((UINT16)WC_command.reply[1]) + (((UINT16)WC_command.reply[2]) << 8));
    }
    else
    {
        WC_command.status = GET_ROTATION_RATE_FAIL;
        debug(WC_command.status);
    }
    return 0;
}

//************************************************************************
// do_set_angle : set the angle to rotate by
// ===============
//
void do_set_angle(INT16 angle)
{

    WC_command.cmd[0] = ANGLE_CMD;
    WC_command.cmd[1] = angle & 0x0ff;
    WC_command.cmd[2] = (angle >> 8) & 0x0ff;
    WC_command.send_count = 3;
    WC_command.get_count = 1;
    WC_command.delay = 0;
    exec_command(MODE_STOP_START);
    if (WC_command.reply[0] == 'a')
    {
        WC_command.status = OK;
    }
    else
    {
        WC_command.status = SET_ANGLE_FAIL;
        debug(WC_command.status);
    }
    return;
}


//************************************************************************
// do_get_angle : get the angle we have rotated through
// ===============
//
INT16 do_get_angle(void)
{

    WC_command.cmd[0] = ANGLE_CMD;
    WC_command.send_count = 1;
    WC_command.get_count = 3;
    WC_command.delay = 0;
    exec_command(MODE_STOP_START);
    if (WC_command.reply[0] == ANGLE_CMD)
    {
        WC_command.status = OK;
        return(INT16)(((UINT16)WC_command.reply[1]) + (((UINT16)WC_command.reply[2]) << 8));
    }
    else
    {
        WC_command.status = GET_ANGLE_FAIL;
        debug(WC_command.status);
    }
    return 0;
}


//************************************************************************
// do_set_distance : set the distance we have moved
// ===============
//
void do_set_distance(INT32 distance)
{

    WC_command.cmd[0] = DISTANCE_CMD;
    WC_command.cmd[1] = distance & 0x0ff;
    WC_command.cmd[2] = (distance >> 8) & 0x0ff;
    WC_command.cmd[3] = (distance >> 16) & 0x0ff;
    WC_command.cmd[4] = (distance >> 24) & 0x0ff;
    WC_command.send_count = 5;
    WC_command.get_count = 1;
    WC_command.delay = 0;
    exec_command(MODE_STOP_START);
    if (WC_command.reply[0] == 'a')
    {
        WC_command.status = OK;
    }
    else
    {
        WC_command.status = SET_DISTANCE_FAIL;
        debug(WC_command.status);
    }
    return;
}


//************************************************************************
// do_get_distance : get the distance we have moved
// ===============
//
INT32 do_get_distance(void)
{

    WC_command.cmd[0] = DISTANCE_CMD;
    WC_command.send_count = 1;
    WC_command.get_count = 5;
    WC_command.delay = 0;
    exec_command(MODE_STOP_START);
    if (WC_command.reply[0] == DISTANCE_CMD)
    {
        WC_command.status = OK;
        return(INT32)(
        ((UINT32)WC_command.reply[1]) +
        (((UINT32)WC_command.reply[2]) << 8) +
        (((UINT32)WC_command.reply[3]) << 16) +
        (((UINT32)WC_command.reply[4]) << 24));
    }
    else
    {
        WC_command.status = GET_DISTANCE_FAIL;
        debug(WC_command.status);
    }
    return 0;
}



//************************************************************************
// do_go : start movement
// =====
//
void do_go(void)
{
    WC_command.cmd[0] = GO_CMD;
    WC_command.send_count = 1;
    WC_command.get_count = 1;
    WC_command.delay = 100;
    exec_command(MODE_STOP_START);                                                                 // PLS March 9 2006: must release bus before querying status of go command.
    if (WC_command.reply[0] == 'a')
    {
        WC_command.status = OK;
    }
    else
    {
        WC_command.status = GO_FAIL;
        debug(WC_command.status);
    }
    return;
}

//************************************************************************
// do_coast : switch off motor system
// ========
//
void do_coast(void)
{
    WC_command.cmd[0] = COAST_CMD;
    WC_command.send_count = 1;
    WC_command.get_count = 1;
    WC_command.delay = 100;
    exec_command(MODE_STOP_START);                                                                 // PLS March 9 2006: must release bus before querying status of coast command.
    if (WC_command.reply[0] == 'a')
    {
        WC_command.status = OK;
    }
    else
    {
        WC_command.status = COAST_FAIL;
        debug(WC_command.status);
    }
    return;
}

//************************************************************************
// do_reset : reset all motion parameters
// ========
//
void do_reset(void)
{
    WC_command.cmd[0] = RESET_CMD;
    WC_command.send_count = 1;
    WC_command.get_count = 1;
    WC_command.delay = 100;
    exec_command(MODE_STOP_START);
    if (WC_command.reply[0] == 'a')
    {
        WC_command.status = OK;
    }
    else
    {
        WC_command.status = RESET_FAIL;
        debug(WC_command.status);
    }
    return;
}

//************************************************************************
// do_echo : test comms channel
// =======
//
void do_echo(void)
{
    WC_command.cmd[0] = ECHO_CMD;
    WC_command.cmd[1] = 0x0F;
    WC_command.send_count = 2;
    WC_command.get_count = 2;
    WC_command.delay = 0;
    exec_command(MODE_RESTART);
    if (WC_command.reply[1] == 0xF0)
    {
        WC_command.status = OK;
    }
    else
    {
        WC_command.status = ECHO_FAIL;
        debug(WC_command.status);
    }
    return;
}



//************************************************************************
// check_set_constants : Configure WheelCommander constants
// ===================
//
// Description
//    There are number of system constants that need to be set.  However,
//    they need only be set once as they are held in flash memory.  There
//    is also a need to keep flash writes to a minimum.  This function
//    checks each of the constants and only modifies them if they are
//    incorrect.
//
//    The parameters to be checked are
//
//       1. MODE (2)  Vehicle mode settings
//       2. Wb  (21)  Wheelbase distance in mm
//       3. Wc  (22)  Wheel circumference in mm.
//
int  check_set_constants(void)
{
    do_get_constant(CONST_MODE);
//  debug(WC_command.reply[0]);

    if ( do_get_constant(CONST_MODE) != MODE_BITS_WATTBOT_I )
    {
//      debug(WC_command.cmd[0]);
    }
}

//************************************************************************
// debug : output status code on PORTB.
// =====
//
void debug(UINT8 code)
{
    if (code == OK)
    {
#ifdef MICROCHIP_C
        PORTB = 0;
#else
        output_b(0);
#endif
        return;
    }

#ifdef MICROCHIP_C
    PORTB = code;
#else
    output_b(code);
    printf("Fatal error %u.\r\n", (byte)code);
#endif
//  HANG;
}

//************************************************************************
//************************************************************************
// main code.
//************************************************************************
void main()
{
    INT16 v;
    INT16 r;
    INT16 a;
    INT32 d;
    int i;

    sys_init();
    printf("\r\n**************************************\r\n");
    printf("WheelCommander PIC C Test Program v2.0\r\n");
    WC_132_init();
    printf("\r\nInit done.\r\n");
    eeprom_test();
//  check_set_constants();
    printf("Get status: 0x%lx\r\n", do_get_status());
    
    printf("Get constant #2 (MODE):    0x%x\r\n", do_get_constant(2));
    printf("Get constant #3 (BAUD):    0x%x\r\n", do_get_constant(3));
    printf("Get constant #4 (I2CADDR): 0x%x\r\n", do_get_constant(4));
    printf("Get constant #29 (AXSIGN): 0x%x\r\n", do_get_constant(29));
    printf("Get constant #34 (PWM):    0x%x\r\n", do_get_constant(34));
    printf("Get constant #35 (XMODE):  0x%x\r\n", do_get_constant(35));
    printf("Get constant #36 (TPRL):   0x%x\r\n", do_get_constant(36));
    printf("Get constant #37 (TPRH):   0x%x\r\n", do_get_constant(37));
    
    printf("Do coast\r\n");
    do_coast();
    if (WC_command.status == OK)
    {
        printf("WC-132 initialized\r\n");
        printf("\r\nSetting velocity to -6.0 ips\r\n");
        do_set_velocity(-60);
        printf("Go!\r\n");
        do_go();
        DelayMs(254);
        DelayMs(254);
        DelayMs(254);
        DelayMs(254);
        if (WC_command.status == OK)
            printf("Get status: %lx\r\n", do_get_status());
        DelayMs(254);
        DelayMs(254);
        DelayMs(254);
        DelayMs(254);
        v = do_get_velocity();
        r = do_get_rotation_rate();
        a = do_get_angle();
        d = do_get_distance();
        printf("v=%ld, r=%ld, a=%ld, d=%ld\r\n", v, r, a, d);

        do_coast();
        DelayMs(254);
        DelayMs(254);
        DelayMs(254);
        DelayMs(254);
        do_reset();

        printf("\r\nSetting velocity to +5.0 ips\r\n");
        do_set_velocity(50);
        printf("Go!\r\n");
        do_go();
        DelayMs(254);
        DelayMs(254);
        DelayMs(254);
        DelayMs(254);
        if (WC_command.status == OK)
            printf("Get status: %lx\r\n", do_get_status());
        DelayMs(254);
        DelayMs(254);
        DelayMs(254);
        DelayMs(254);
        v = do_get_velocity();
        r = do_get_rotation_rate();
        a = do_get_angle();
        d = do_get_distance();
        printf("v=%ld, r=%ld, a=%ld, d=%ld\r\n", v, r, a, d);

        do_coast();
        DelayMs(254);
        DelayMs(254);
        DelayMs(254);
        DelayMs(254);
        do_reset();

        printf("\r\nSetting rotation rate to 90 dps\r\n");
        do_set_rotation_rate(90);
        printf("Go!\r\n");
        do_go();
        DelayMs(254);
        DelayMs(254);
        DelayMs(254);
        DelayMs(254);
        if (WC_command.status == OK)
            printf("Get status: %lx\r\n", do_get_status());
        DelayMs(254);
        DelayMs(254);
        DelayMs(254);
        DelayMs(254);
        v = do_get_velocity();
        r = do_get_rotation_rate();
        a = do_get_angle();
        d = do_get_distance();
        printf("v=%ld, r=%ld, a=%ld, d=%ld\r\n", v, r, a, d);

        do_coast();
        printf("\r\nDone.\r\n");
    }
    debug(WC_command.status);
    printf("Halting.\r\n");
    HANG;
}