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;
}
© 2004-2011 Noetic Design, Inc. All Rights Reserved. Nubotics and WheelWatcher are trademarks of Noetic Design, Inc.