// @(#) $Header: /usr/local/scalawags_cvs/Scalawags/Frc2006/main.c,v 1.56 2006/03/07 02:31:50 abrown Exp $
/*****************************************************************************
* FILE NAME: main.c
*
* DESCRIPTION:
* This file contains the main program loop.
*
* USAGE:
* You should not need to modify this file.
*****************************************************************************
*/
#define MAIN
#include "p18f8722.h"
#include "ifi_aliases.h"
#include "ifi_default.h"
#include "ifi_utilities.h"
#include "user_routines_alt.h"
#include "user_SerialDrv.h"
tx_data_record txdata;
rx_data_record rxdata;
packed_struct statusflag;
extern int msgxfer_fd;
#if defined(ABSIMULATION_COPROCESSOR) \
|| defined(ABSIMULATION_COPRSIM) \
|| defined(COPROCESSOR_STANDALONE) \
|| defined(COPROCESSOR_SIMULATION)
#include <stdio.h>
#include <pty.h>
#include <utmp.h>
#include <sys/types.h>
#include <unistd.h>
#include <signal.h>
#include <string.h>
#include <stdlib.h>
#include "sim/simfuncs.h"
#define EXTERN
#endif
#if defined(FRC_COPROCESSOR) || defined(ABSIMULATION_COPROCESSOR) || defined(ABSIMULATION_COPRSIM)
#include "msgxfer.h"
#endif
#include "supportfuncs.h"
int message, cyclecount=0;
/*****************************************************************************
* FUNCTION NAME: main
* PURPOSE: Main program function.
* CALLED FROM: ifi_startup.c
* ARGUMENTS: none
* RETURNS: void
*****************************************************************************
*/
#if defined(ABSIMULATION_NOCOPROCESSOR) || defined(ABSIMULATION_COPROCESSOR) || defined(ABSIMULATION_COPRSIM)
char Revision[]="$Revision: 1.56 $";
#endif
#if defined(ABSIMULATION_COPRSIM)
char coprname[] = "coprocessor"; // Name of simulation binary.
char *coprocessor;
#endif
// enum {INITIAL, DISABLED, MANUAL, AUTO} runstatetype;
#define INITIAL 0
#define DISABLED 1
#define MANUAL 2
#define AUTO 3
typedef int runstatetype;
runstatetype RunMode;
#if defined(FRC_NOCOPROCESSOR) || defined(FRC_COPROCESSOR)
// MPLab
#define MAINTYPE void
#define MAINARGS void
#else
// Standard C
#define MAINTYPE int
#define MAINARGS int argc, char **argv
#endif
MAINTYPE main ( MAINARGS )
{ // main
runstatetype OldRunMode;
#if defined(FRC_COPROPCESSOR)
// Nothing to do
#else
# if defined(ABSIMULATION_COPROCESSOR) || defined(ABSIMULATION_COPRSIM)
unsigned char inbuf[9], outbuf[10];
int inlen;
msgxfer_state outstate;
msgxfer_state instate;
# endif
# if defined(ABSIMULATION_COPRSIM)
int slave;
pid_t childpid;
char slavename[257];
if (openpty(&msgxfer_fd, &slave, slavename, NULL, NULL) != 0)
{
fprintf(stderr, "Unable to create pty\n");
return -1;
}
close(slave);
fprintf(stderr, "Slave pty is %s\n", slavename);
set_raw_mode(msgxfer_fd);
switch (childpid=fork())
{
case 0: // The child
{
char *coprind, *coprlast = coprocessor;
coprocessor = malloc(sizeof(coprname)+strlen(argv[0]));
if( !coprocessor )
{
fprintf(stderr,"main robot code: malloc failed.\n");
return(3);
}
coprocessor[0] = 0;
strcat(coprocessor,argv[0]);
for(coprind=coprocessor; (*coprind); coprind++)
{
if( *coprind == '/' ) coprlast = coprind+1;
}
*coprlast = (char)0;
strcat(coprocessor,coprname);
execl(coprocessor, coprocessor, slavename, (char *) NULL);
fprintf(stderr, "main robot code: Exec '%s' failed.\n", coprname);
return(-1);
}
break;
case -1: // An error
{
fprintf(stderr, "Fork error\n");
return -1;
}
break;
default: // The parent
msgxfer_init_read(&instate, &msg_reader);
msgxfer_init_write(&outstate, &msg_writer);
break;
}
# endif
#endif
RunMode=INITIAL;
#ifdef UNCHANGEABLE_DEFINITION_AREA
IFI_Initialization ();
#endif
DisableBot();
User_Initialization(); // You edit this in user_routines_new.c
Putdata(&txdata);
statusflag.NEW_SPI_DATA = 0;
while(1) // This loop will repeat indefinitely.
{
#if defined(FRC_NOCOPROCESSOR) || defined(FRC_COPROCESSOR)
#ifdef _SIMULATOR
statusflag.NEW_SPI_DATA = 1;
#endif
message = statusflag.NEW_SPI_DATA ? MSG_TSLOW : MSG_TFAST;
#endif
#if defined(ABSIMULATION_NOCOPROCESSOR) || defined(ABSIMULATION_COPROCESSOR) || defined(ABSIMULATION_COPRSIM)
int reportval;
message = GetInput();
statusflag.NEW_SPI_DATA = (message == MSG_TSLOW);
#endif
OldRunMode = RunMode;
if (disabled_mode)
{
RunMode = DISABLED;
}
else
{
if (autonomous_mode)
{
RunMode = AUTO;
}
else
{
RunMode = MANUAL;
}
}
if (OldRunMode != RunMode)
{
reportstart(__FILE__,__LINE__,NOTE,"Mode=");
switch(RunMode)
{
case DISABLED:
reportend("DISABLED.\n");
break;
case MANUAL:
reportend("MANUAL.\n");
DisableBot();
break;
case AUTO:
DisableBot();
reportend("AUTO.\n");
autonomous_init();
break;
default:
reportend("UNDEFINED.\n");
reportstart(__FILE__,__LINE__,ERROR,"Internal error in mode switch");
reportend(".\n");
break;
}
}
if (RunMode == DISABLED)
{
DisableBot();
cyclecount++;
Putdata(&txdata); // Write the results
} // if (RunMode == DISABLED)
else
{
switch(message)
{
case MSG_TSLOW: // --------------------------------------------
Getdata(&rxdata);
User_Slow_Pre();
if (autonomous_mode && cyclecount)
{
User_Slow_Auto();
}
else
{
User_Slow_Manual();
}
User_Slow_Post();
cyclecount++;
Putdata(&txdata); // Write the results
break;
case MSG_TFAST: // --------------------------------------------
User_Fast_Pre();
if (autonomous_mode && cyclecount)
{
User_Fast_Auto();
}
else
{
User_Fast_Manual();
}
User_Fast_Post();
#if defined(ABSIMULATION_NOCOPROCESSOR) || defined(ABSIMULATION_COPROCESSOR) || defined(ABSIMULATION_COPRSIM)
PutFast();
#endif
Putdata(&txdata); // Write the results
break;
#if defined(ABSIMULATION_NOCOPROCESSOR) || defined(ABSIMULATION_COPROCESSOR) || defined(ABSIMULATION_COPRSIM)
case MSG_TINTERRUPT: // --------------------------------------------
InterruptVectorLow();
break;
case MSG_TREPORT: // --------------------------------------------
reportsum();
break;
case MSG_TEXIT: // --------------------------------------------
reportval=reportsum();
// printf("Done\n");
#if defined(ABSIMULATION_COPRSIM)
kill(childpid,SIGQUIT);
#endif
return(reportval);
break;
#endif
#if defined (ABSIMULATION_COPROCESSOR) || defined(ABSIMULATION_COPRSIM)
case MSG_TCOPDAT: // --------------------------------------------
outbuf[0] = rxdata.oi_analog01;
outbuf[1] = rxdata.oi_analog02;
outbuf[2] = rxdata.oi_analog03;
outbuf[3] = rxdata.oi_analog04;
outbuf[4] = rxdata.oi_analog05;
outbuf[5] = rxdata.oi_analog06;
outbuf[6] = rxdata.oi_analog07;
outbuf[7] = rxdata.oi_analog08;
outbuf[8] = rxdata.oi_swA_byte.allbits;
outbuf[9] = rxdata.oi_swB_byte.allbits;
msgxfer_reset_state(&outstate);
msgxfer_send(outbuf, sizeof(outbuf), &outstate);
inlen = msgxfer_receive(inbuf, sizeof(inbuf), &instate);
if (inlen != 0)
{
if (inlen < 0)
{
fprintf(stderr, "Error %d from msgxfer_receive\n", inlen);
}
else
{
txdata.rc_pwm01 = inbuf[0];
txdata.rc_pwm02 = inbuf[1];
txdata.rc_pwm03 = inbuf[2];
txdata.rc_pwm04 = inbuf[3];
txdata.rc_pwm13 = inbuf[4];
txdata.rc_pwm14 = inbuf[5];
txdata.rc_pwm15 = inbuf[6];
txdata.rc_pwm16 = inbuf[7];
LATEbits.LATE0 = (inbuf[8] >> 0) && 1;
LATEbits.LATE1 = (inbuf[8] >> 1) && 1;
LATEbits.LATE2 = (inbuf[8] >> 2) && 1;
LATEbits.LATE3 = (inbuf[8] >> 3) && 1;
LATEbits.LATE4 = (inbuf[8] >> 4) && 1;
LATEbits.LATE5 = (inbuf[8] >> 5) && 1;
LATEbits.LATE6 = (inbuf[8] >> 6) && 1;
LATEbits.LATE7 = (inbuf[8] >> 7) && 1;
fprintf(stdout, "coprsend: %02x %02x %02x %02x %02x %02x %02x %02x %02x\n",
inbuf[0], inbuf[1], inbuf[2], inbuf[3],
inbuf[4], inbuf[5], inbuf[6], inbuf[7], inbuf[8]);
fflush(stdout);
}
msgxfer_reset_state(&instate);
}
break;
#endif
case 0: // Do nothing for blank lines or comment lines.
break;
default:
reportstart(__FILE__,__LINE__,NOTE,"Undefined message=");
reportint(message);
reportend(".\n");
break;
} // switch(RunMode)
} // else (RunMode == DISABLED)
} // while (1)
#if defined(ABSIMULATION_NOCOPROCESSOR) || defined(ABSIMULATION_COPROCESSOR) || defined(ABSIMULATION_COPRSIM)
return(0);
#endif
} // main()
/****************************************************************************
* $Log: main.c,v $
* Revision 1.56 2006/03/07 02:31:50 abrown
* Correctly ignore blank lines and comment lines.
*
* Revision 1.55 2006/03/06 20:23:51 abrown
* Fix logging during state changes.
*
* Revision 1.54 2006/02/17 20:24:01 abrown
* Clean up whitespace.
*
* Revision 1.53 2006/02/17 20:08:11 abrown
* Change the way the main() function is declared, making it easier to read.
* Chnage almost all comments to the // type, making it easier to comment
* out blocks of code. Move DisableBot() to before User_Initialization().
* Move if(!disabled_mode) to outside the switch(message).
*
* Revision 1.52 2006/02/08 18:58:19 abrown
* Move the "do not change" code from user_routines_new.c to main.c.
* Remove some scruff and improve formatting.
*
* Revision 1.51 2006/02/01 00:43:45 abrown
* Replace the original IFI functions with ones that are more sensibly defined.
*
* Revision 1.50 2005/12/24 01:36:49 abrown
* Split the old target ABSIMULATION_COPROCESSOR into two. Now
* ABSIMULATION_COPROCESSOR includes a physical coprocessor. The new
* target, ABSIMULATION_COPRSIM, includes a simulated coprocessor.
*
* Revision 1.49 2005/12/17 03:46:19 asumu
* Replaced calls for msgxfer_init() with msgxfer_init_read/write(). Removed calls for msgxfer_init_type().
*
* Revision 1.48 2005/09/24 22:19:20 asumu
* Modified to use new/modified functions in msgxfer.c and new msgxfer state structure.
*
* Revision 1.47 2005/09/21 13:09:56 abrown
* Consolidate a couple of declarations at the beginning of their code
* block. Provides better compatibility with older compilers.
*
* Revision 1.46 2005/05/14 03:31:40 go
* Fixing up msgxfer linkages and general conditionals for different version.
*
* Revision 1.45 2005/05/01 19:53:08 abrown
* Typo'ed the new #if's.
*
* Revision 1.44 2005/04/30 17:06:20 abrown
* Change from using the old defines to the new ones.
*
* Revision 1.43 2005/03/13 20:44:13 abrown
* Move call to autonomous_init() from user_routines.c to main.c and execute
* it each time we enter RunMode=AUTO state.
* In user_routines.c User_Initialization() call DisableBot() early.
*
* Revision 1.42 2005/02/20 21:34:25 abrown
* Stop using pwm05.
*
* Revision 1.41 2005/02/20 00:31:41 jeff
* Call Disable bot when switching between manual and auto, prevents motors from continuing to run after auto.
*
* Revision 1.40 2005/02/17 04:50:41 abrown
* Free up User_Mode_byte by using pwm05 instead.
*
* Revision 1.39 2005/02/17 02:33:19 abrown
* Move simfuncs into the sim directory.
*
* Revision 1.38 2005/02/16 16:31:40 abrown
* Remove some scruff.
*
* Revision 1.37 2005/02/16 05:30:01 abrown
* Move the call to Getdata() to more closely match the orig FRC code.
*
* Revision 1.36 2005/02/15 04:28:02 abrown
* Don't execute Getdata() the first time thru the while loop. It
* would override statusflag.NEW_SPI_DATA=0
*
* Revision 1.35 2005/02/14 05:01:38 abrown
* Include a Putdata() after the slow loop call.
*
* Revision 1.34 2005/02/14 04:52:03 abrown
* Move the call to Getdata() from user_routines to main.c.
*
* Revision 1.33 2005/02/13 20:36:41 abrown
* Run the manual code the first time even if auto enabled. This
* more closely matches the FRC code.
*
* Revision 1.32 2005/02/13 20:18:56 abrown
* The merge effort between FRC and simulator in main. It doesn't
* work on the FRC yet. But it got checked in anyway, so now I
* need to make it work quickly.
*
* Revision 1.30 2005/02/12 17:19:00 abrown
* Clean up some comments.
*
* Revision 1.29 2005/02/12 06:11:16 jeff
* Modified to drive in a straight line using hall sensors during autonomous.
*
* Revision 1.28 2005/02/12 03:57:37 jeff
* Only calling Putdata(&txdata) in one place. Appears second (or later) calls
* do nothing.
*
* Revision 1.27 2005/02/09 16:26:32 abrown
* Trivial comment change.
*
* Revision 1.26 2005/02/06 23:15:44 abrown
* Get distance_left/distance_right working globally. Also formating tweaks.
*
* Revision 1.25 2005/02/05 02:43:30 jeff
* Modified to compile under MPLab (fixed misplaced ifndefs and endifs)
* Added comment about pwm position on robot
*
* Revision 1.24 2005/02/03 16:39:25 abrown
* Make sure all switch()s have defaults.
*
* Revision 1.23 2005/02/03 01:57:23 abrown
* Detect state change and log it: DISABLED, MANUAL, AUTO.
*
* Revision 1.22 2005/01/31 23:17:45 go
* Added "set_raw_mode" function to supportfuncs
* Added call to "set_raw_mode" to main and coprocessor
*
* Revision 1.21 2005/01/31 06:02:52 abrown
* Fix problems pointing to msg_reader/msg_writer.
*
* Revision 1.20 2005/01/30 22:23:02 abrown
* Merge in changes to msgxfer. Unfortunately it now core dumps.
* And there are some odd warnings.
*
* Revision 1.19 2005/01/30 20:34:57 abrown
* Make coprocessor activities visible to TCL.
*
* Revision 1.18 2005/01/30 00:45:13 abrown
* Compact some declarations.
*
* Revision 1.17 2005/01/29 20:30:32 abrown
* The coprocessor now computes pwms from joystick vals. It's time to
* deploy the code.
*
* Revision 1.16 2005/01/29 18:05:50 abrown
* Make the inbuf and outbuf sizes different.
*
* Revision 1.15 2005/01/29 17:42:33 abrown
* Remove experimental code (was commented out) to pass the main/coprocessor
* pty port by number rather than name. It didn't work and the real bug
* in main's use of msgxfer was found. Try to clean up ifdefs so that
* no coprocessor code shows up if we don't have one.
*
* Revision 1.14 2005/01/29 04:29:14 abrown
* Only kill the sim/coprocessor child if we are in simulation mode.
*
* Revision 1.13 2005/01/29 04:11:54 go
* Missing initialization
*
* Revision 1.12 2005/01/29 03:56:09 abrown
* Kill child on exit.
*
* Revision 1.11 2005/01/29 03:28:09 abrown
* Minor cleanups. But messages are still not getting thru.
*
* Revision 1.10 2005/01/28 03:13:40 abrown
* Add the time code in so that packets would send some real data, if
* they were sending at all. Nothing is getting to sim/coprocessor.
*
* Revision 1.9 2005/01/28 02:50:00 abrown
* Add some debug code. Why isn't coprocessor binary doing anything?
*
* Revision 1.8 2005/01/27 16:07:16 abrown
* coprocessor.c is much closer. I put in the slave code from Gary's
* example test program. I used a boption shell and then stripped
* out some of the cruft.
*
* Revision 1.7 2005/01/26 05:53:00 abrown
* Some of the coprocessor code.
*
* Revision 1.6 2005/01/14 04:36:25 abrown
* Use the Makefile to indicate if there is a coprocessor.
*
* Revision 1.5 2005/01/14 03:35:27 abrown
* Added rudimentary com link to a coprocessor.
*
* Revision 1.4 2005/01/10 01:21:23 jeff
* Changed return(0) to be compatible with MPLAB
*
* Revision 1.3 2005/01/09 05:14:59 abrown
* Update to 2005 default code base. -- Allen Brown
*
* Revision 1.2 2004/12/29 05:22:02 abrown
* Add MAIN define to work with ifi_picdefs.h. Add ABSIMULATION
* conditional code including the message switch().
*/
/****************************************************************************/
syntax highlighted by Code2HTML, v. 0.9.1