int main(int argc, char* argv[]) {
struct bParamList * paramList = NULL;
const char *strPtr;
int ii;
for (ii=0; ii<TCX_MAX_CLIENTS; ii++) {
CLIENT[ii] = NULL;
}
/*
* Set some parameters
*/
/* add some defaults */
paramList = bParametersAddEntry(paramList, "*", "base.bps", "9600");
paramList = bParametersAddEntry(paramList, "*", "base.dev", "/dev/cur0");
paramList = bParametersAddEntry(paramList, "", "TCXHOST", "localhost");
paramList = bParametersAddEntry(paramList, "", "fork", "yes");
/* add some parameter files */
paramList = bParametersAddFile(paramList, "etc/beeSoft.ini");
/* add some enviroment variables */
paramList = bParametersAddEnv(paramList, "", "TCXHOST");
/* add command line arguements */
paramList = bParametersAddArray(paramList, "", argc, argv);
/* here is where we should add non "parameter format" options */
bParametersFillParams(paramList);
strPtr = bParametersGetParam(paramList, "robot", "name");
if (!strPtr) {
fprintf(stderr,
"Robot name not set. Check the value in beeSoft.ini.\n");
exit(1);
}
else if (!strcmp("none", strPtr)) {
fprintf(stderr,
"Robot name set to 'none'. Check value in beeSoft.ini.\n");
exit(1);
}
strPtr = bParametersGetParam(paramList, strPtr, "base.type");
if (!strPtr) {
fprintf(stderr,
"Base type not set. Check the value in beeSoft.ini.\n");
exit(1);
}
else if (
strcmp("B14", strPtr) &&
strcmp("B21", strPtr) &&
strcmp("Ramona", strPtr)
) {
fprintf(stderr,
"Arm type set to unrecognized type '%s'. Check the\n"
"value in beeSoft.ini.\n", strPtr);
exit(1);
}
/*
*
*/
argv0 = argv[0];
if (makeLock(BASE_SERVER_LOCK)<0) {
fprintf(stderr,"%s: Already running base server\n",argv[0]);
exit(-1);
}
if (findArg(argc,argv,"-v")) {
VERBOSE = TRUE;
fprintf(stderr,"baseServer started in verbose mode\n");
}
if (findArg(argc,argv,"-noIrs")) {
IRS = FALSE;
fprintf(stderr,"baseServer started without IRs\n");
}
if (findArg(argc,argv,"-noTactiles")) {
TACTILES = FALSE;
fprintf(stderr,"baseServer started without Tactiles\n");
}
if (findArg(argc,argv,"-noSonars")) {
SONARS= FALSE;
fprintf(stderr,"baseServer started without Sonars\n");
}
if (findArg(argc,argv,"-simulator")) {
useSimulator=TRUE;
fprintf(stderr,"baseServer connecting to simulator.\n");
}
RaiInit();
BaseInit(bRobot.base_dev, bRobot.base_bps);
catchInterrupts();
initBaseServerTCX(bRobot.TCXHOST);
if (useSimulator==TRUE) {
simulatorHandle = tcxConnectModule(TCX_SIMULATOR_MODULE_NAME);
}
registerWatchdogCallback(watchdogToClient);
/*
* GRUMBLE - It is going to be a bit ugly to get the sensor
* structures initialized without opening /dev/abus. tds
*/
if (SONARS && !useSimulator) {
sonarInit();
registerSonarCallback(sonarToClient);
SIM_sonarStop(); /* turn off sonar until we have a client */
}
if (IRS && !useSimulator) {
irInit();
registerIrCallback(irToClient);
SIM_irStop(); /* turn off ir until we have a client */
}
if (TACTILES && !useSimulator) {
tactileInit();
registerTactileCallback(tactileToClient);
tactileStop();
}
registerStatusCallback(statusToClient);
registerBaseCallback(baseToClient);
initBaseServerModules();
if (bRobot.fork) {
bDaemonize("baseServer.log");
}
rotateLimp();
translateLimp();
RaiStart();
exit(0);
}
int main() {
// Processor Initialization
SetupClock();
SwitchClocks();
SetupPorts();
sclockSetup();
LED_1 = 1;
LED_2 = 1;
LED_3 = 1;
// Message Passing
fun_queue = carrayCreate(FUN_Q_LEN);
cmdSetup();
// Radio setup
radioInit(RADIO_RXPQ_MAX_SIZE, RADIO_TXPQ_MAX_SIZE);
radioSetChannel(RADIO_CHANNEL);
radioSetSrcAddr(RADIO_SRC_ADDR);
radioSetSrcPanID(RADIO_PAN_ID);
uart_tx_packet = NULL;
uart_tx_flag = 0;
//uartInit(&cmdPushFunc);
tactileInit();
// Need delay for encoders to be ready
delay_ms(100);
amsEncoderSetup();
mpuSetup();
tiHSetup();
dfmemSetup();
telemSetup();
adcSetup();
pidSetup();
LED_1 = 0;
LED_3 = 1;
while(1){
// Send outgoing radio packets
radioProcess();
/*
// Send outgoing uart packets
if(uart_tx_flag) {
uartSendPacket(uart_tx_packet);
uart_tx_flag = 0;
}*/
checkTactileBuffer();
// move received packets to function queue
while (!radioRxQueueEmpty()) {
// Check for unprocessed packet
rx_packet = radioDequeueRxPacket();
if(rx_packet != NULL) {
cmdPushFunc(rx_packet);
}
}
// process commands from function queue
while(!carrayIsEmpty(fun_queue)) {
rx_packet = carrayPopHead(fun_queue);
unsigned int rx_src_addr = rx_packet->src_addr.val;
if(rx_packet != NULL) {
rx_payload = macGetPayload(rx_packet);
if(rx_payload != NULL) {
rx_function = (test_function)(rx_payload->test);
if(rx_function != NULL) {
LED_2 = ~LED_2;
(rx_function)(payGetType(rx_payload), payGetStatus(rx_payload), payGetDataLength(rx_payload), payGetData(rx_payload), rx_src_addr);
}
}
ppoolReturnFullPacket(rx_packet);
}
}
}
return 0;
}
