Commit 4ad62556 authored by Daniel Flickinger's avatar Daniel Flickinger

(2004/12/06) Initial add by Dan Flickinger

parent 3125b8a8
# Makefile for Garcia robot applications
#
# Dan Flickinger
#
# 2004/10/04
# 2004/11/19
HOSTPATH=/z/garcia/brainstem
ARCH=ARM
CC=/usr/local/arm/3.4.1/bin/arm-linux-g++
#ARCH=i686
#CC=g++
INCLUDES=-I$(HOSTPATH)/aCommon \
-I$(HOSTPATH)/aUnix \
-I$(HOSTPATH)/aIO/aCommon \
-I$(HOSTPATH)/aIO/aUnix \
-I$(HOSTPATH)/aUI/aCommon \
-I$(HOSTPATH)/aUI/aUnix \
-I$(HOSTPATH)/aStem/aCommon \
-I$(HOSTPATH)/aStem/aUnix \
-I$(HOSTPATH)/aGarcia/aGarcia \
-I$(HOSTPATH)/aGarcia/aUnix \
-I$(HOSTPATH)/aGarcia/aCommon \
-I./
LIBRARIES=\
-laGarcia \
-laStem \
-laIO \
-laUI \
-lpthread \
-lm \
-I.
VPATH=$(HOSTPATH)/aCommon:$(HOSTPATH)/aUnix
CFLAGS=-DaUNIX -s -static
#CFLAGS=-DaUNIX -g
LDFLAGS=-L$(HOSTPATH)/aRelease/aUnix/$(ARCH)/
OBJECTSREQ=grobot gcallbacks gbehaviors simplepath
OBJECTS=grobot.o gcallbacks.o gbehaviors.o simplepath.o
all: commotion pathmotion circle gorobot
commotion: $(OBJECTSREQ)
$(CC) -o commotion $(OBJECTS) $(INCLUDES) $(CFLAGS) $(LDFLAGS) commotion.cc $(LIBRARIES)
pathmotion: $(OBJECTSREQ)
$(CC) -o pathmotion $(OBJECTS) $(INCLUDES) $(CFLAGS) $(LDFLAGS) pathmotion.cc $(LIBRARIES)
circle: $(OBJECTSREQ)
$(CC) -o circle $(OBJECTS) $(INCLUDES) $(CFLAGS) $(LDFLAGS) circle.cc $(LIBRARIES)
gorobot: $(OBJECTSREQ)
$(CC) -o gorobot $(OBJECTS) $(INCLUDES) $(CFLAGS) $(LDFLAGS) gorobot.cc $(LIBRARIES)
# %.o : %.c
# $(CC) $(CFLAGS) -c -o $@ $< $(INCLUDES)
#
# %.oo : %.cpp
# $(CC) $(CFLAGS) -c -o $@ $< $(INCLUDES)
grobot:
$(CC) -c $(INCLUDES) $(CFLAGS) dgrobot/grobot.cc
gcallbacks:
$(CC) -c $(INCLUDES) $(CFLAGS) dgrobot/gcallbacks.cc
gbehaviors:
$(CC) -c $(INCLUDES) $(CFLAGS) dgrobot/gbehaviors.cc
simplepath:
$(CC) -c $(INCLUDES) $(CFLAGS) dgrobot/simplepath.cc
clean :
rm -f *.o
rm -f primotion
rm -f square
rm -f commotion
rm -f shell
rm -f pathmotion
rm -f circle
rm -f batt
rm -f gorobot
/* circle: garcia robot simple circle path
* (Tests grobot::setvPath(float, float))
*
* Dan Flickinger
*
* 2004/11/10
* 2004/11/11
*/
#include "dgrobot/commotion.h"
int main(int argc, char **argv)
{
// terminal setup
struct termios old_mode;
struct termios new_mode;
tcgetattr(fileno(stdout),&old_mode);
new_mode = old_mode;
new_mode.c_lflag &= ~(ECHOKE | ECHOK | ECHOE | ECHO);
new_mode.c_lflag &= ~(ICANON);
tcsetattr(fileno(stdout),TCSANOW,&new_mode);
setbuf(stdin,(char*)NULL);
char keypress = '~';
int status = 0;
grobot robot1;
robot1.setvPath(0.2f, 0.5f);
robot1.sleepy();
cout << "Set path to 1.0 m diameter circle at 0.2 m/s" << endl;
do {
robot1.sleepy();
fread(&keypress, sizeof(char), 1, stdin);
status = robot1.getGstatus();
} while ((keypress == '~') && (status == 0));
robot1.sleepy();
cout << "Garcia status: " << status << endl;
cout << "Setting wheels to zero" << endl;
robot1.setWheels(0.0f, 0.0f);
cout << "DONE..." << endl;
robot1.sleepy();
return 0;
}
/* Garcia robot motion program (command and interactive mode)
*
* Dan Flickinger
*
* 2004/09/13
* 2004/11/17
*/
#include "dgrobot/commotion.h"
void interact(grobot & Robot);
void drive(char *filename, grobot & Robot);
void print_acpValue(acpValue *v);
void printHELP();
int main(int argc, char **argv)
{
char commandIN [64]; // command in
char callbackMSG[128]; // callback message
char * pCPRIM; // pointer to Command PRIMitive character
char * pCARG; // pointer to Commmand ARGument string
int fileMode = 0; // file in mode for commands
// battery parameters
float battLevel, battVoltage;
// wheel odometry parameters (distance and velocity)
float dLeft, dRight;
float vLeft, vRight;
// GOTO displacement (Dx, Dy) and final orientation angle (Rf)
float Dx, Dy, Rf;
ifstream fileIN;
ofstream fileOUT;
strcpy(commandIN, "");
grobot robot1;
fileOUT.open("vpos.log", ios::out | ios::app);
if (!(fileOUT.is_open())) {
// file not opened for some reason
cout << "ERROR opening log file!" << endl;
} else {
cout << "Appending wheel position and velocity data to 'vpos.log'" << endl;
}
cout << "COMMAND: (h for help)" << endl;
while (strcmp(commandIN, "q") != 0) {
cout << endl << "? ";
// get a command
if (fileMode) {
// read a line from file
if (fileIN.is_open()) {
if (fileIN.eof()) {
// done reading, drop back to command mode
cout << "Finished reading command file." << endl;
fileIN.close();
fileMode = 0;
} else {
fileIN.getline(commandIN, 64);
cout << commandIN << endl;
}
} else {
// file not open
cout << "FILE CLOSED." << endl;;
fileMode = 0;
}
} else {
// read from console input
cin.getline(commandIN, 64);
}
if (strcmp(commandIN, "h") == 0) {
// print HELP
printHELP();
} else {
// not help
if (strcmp(commandIN, "") !=0) {
pCPRIM = strtok(commandIN, " ");
pCARG = strtok(NULL, " ");
if (strcmp(pCPRIM, "q") == 0) {
// quit
cout << "QUIT..." << endl;
} else if (strcmp(pCPRIM, "s") == 0) {
// set wheelspeed
// (This value is persistent for all primitives.)
acpValue wheelSpeed((float)(atof(pCARG)));
robot1.garcia.setNamedValue("speed", &wheelSpeed);
cout << "Set wheelspeed to " << pCARG << endl;
} else if (strcmp(pCPRIM, "a") == 0) {
// set stall threshold
acpValue stallThreshold((float)(atof(pCARG)));
robot1.garcia.setNamedValue("stall-threshold", &stallThreshold);
cout << "Set stall threshold to " << pCARG << endl;
} else if (strcmp(pCPRIM, "b") == 0) {
// get battery level
battLevel = robot1.garcia.getNamedValue("battery-level")->getFloatVal();
battVoltage = robot1.garcia.getNamedValue("battery-voltage")->getFloatVal();
cout << "Battery: " << 100 * battLevel << "%, "
<< battVoltage << " V" << endl;
} else if (strcmp(pCPRIM, "o") == 0) {
// read wheel odometry
dLeft = robot1.garcia.getNamedValue("distance-left")->getFloatVal();
dRight = robot1.garcia.getNamedValue("distance-right")->getFloatVal();
cout << "Odometry: left = " << dLeft
<< ", right = " << dRight << endl;
} else if (strcmp(pCPRIM, "i") == 0) {
// enter interactive mode
interact(robot1);
} else if (strcmp(pCPRIM, "v") == 0) {
// get acpValue
acpValue *retValue = robot1.garcia.getNamedValue(pCARG);
if (retValue == NULL) {
cout << "Invalid property" << endl;
} else {
print_acpValue(retValue);
}
} else if (strcmp(pCPRIM, "f") == 0) {
// set file read mode
fileIN.open(pCARG, ios::in);
if (fileIN.is_open()) {
cout << "Opened command file: " << pCARG << endl;
fileMode = 1;
} else {
cout << "FAILED to open command file: " << pCARG << endl;
fileMode = 0;
}
} else if (strcmp(pCPRIM, "d") == 0) {
// set file read mode for driving
drive(pCARG, robot1);
} else {
// parse a motion command
if (strcmp(pCPRIM, "m") == 0) {
// move primitive
cout << "Executing MOVE (distance: " << pCARG << ")" << endl;
fileOUT << "# MOVE " << pCARG << ":" << endl;
robot1.pbMove((float)(atof(pCARG)));
}
if (strcmp(pCPRIM, "p") == 0) {
// pivot primitive
cout << "Executing PIVOT (angle: " << pCARG << ")" << endl;
fileOUT << "# PIVOT " << pCARG << ":" << endl;
robot1.pbPivot((float)(atof(pCARG)));
}
if (strcmp(pCPRIM, "g") == 0) {
// goto displacement-orientation (3 primitives)
cout << "Executing GOTO (Dx=";
fileOUT << "# GOTO Dx=";
Dx = atof(pCARG);
cout << Dx << ", Dy=";
fileOUT << Dx << " Dy=";
pCARG = strtok(NULL, " ");
Dy = atof(pCARG);
cout << Dy << ", Rf=";
fileOUT << Dy << " Rf=";
pCARG = strtok(NULL, " ");
Rf = atof(pCARG);
cout << Rf << ")" << endl;
fileOUT << Rf << ":" << endl;
robot1.dgoto(Dx, Dy, Rf);
}
while (!robot1.garcia.getNamedValue("idle")->getBoolVal()) {
robot1.garcia.handleCallbacks(100);
// wheel speed is only accessible while the NULL primitive is running
dLeft = robot1.garcia.getNamedValue("distance-left")->getFloatVal();
dRight = robot1.garcia.getNamedValue("distance-right")->getFloatVal();
// vLeft = robot1.garcia.getNamedValue("damped-speed-left")->getFloatVal();
// vRight = robot1.garcia.getNamedValue("damped-speed-right")->getFloatVal();
// use numerical differentiation instead
// FIXME
vLeft = 0.0f;
vRight = 0.0f;
// log data to outfile
fileOUT << dLeft << " "
<< dRight << endl;
} // end while garcia is idle
} // end parse a motion command
} // end not a blank command ("")
} // end not help
} // end while command in is not 'q'
fileOUT.close();
return 0;
}
/**********************
* FUNCTIONS *
**********************/
void interact(grobot & Robot) {
// Keyboard interactive mode
// terminal interaction
struct termios old_mode;
struct termios new_mode;
tcgetattr(fileno(stdout),&old_mode);
new_mode = old_mode;
new_mode.c_lflag &= ~(ECHOKE | ECHOK | ECHOE | ECHO);
//new_mode.c_lflag |= ECHONL;
new_mode.c_lflag &= ~(ICANON);
tcsetattr(fileno(stdout),TCSANOW,&new_mode);
setbuf(stdin,(char*)NULL);
cout << "Interactive mode: " <<
"use numeric keypad,PgUP/DOWN or ijkm,ol,<space>" << endl;
float Vleft = 0.0f;
float Vright = 0.0f;
// speed change increment
float Vdelta = 0.02f;
grobot robot1;
int local_quit = 0;
char c;
while (!local_quit) {
// interactive loop
// handle the callbacks (check and see if a behavior terminated)
robot1.garcia.handleCallbacks(10);
fread(&c,sizeof(char),1,stdin);
switch(c) {
case '9':
case 'o':
// PigUp, inrease speed change
Vdelta += 0.02f;
break;
case '3':
case 'l':
// PigDown, descrease speed change
Vdelta -= 0.02f;
break;
case '8':
case 'i':
// UP arrow, increase forward speed
Vleft += Vdelta;
Vright += Vdelta;
break;
case '2':
case 'm':
// DOWN arrow, decrease forward speed
Vleft -= Vdelta;
Vright -= Vdelta;
break;
case '4':
case 'j':
// LEFT arrow, turn left
Vleft -= Vdelta;
Vright += Vdelta;
break;
case '6':
case 'k':
// RIGHT arrow, turn right
Vleft += Vdelta;
Vright -= Vdelta;
break;
case '5':
case ' ':
// CENTER, stop!!
Vleft = 0.0f;
Vright = 0.0f;
break;
case 'q':
local_quit = 1;
// Don't forget to stop the robot~!
Vleft = 0.0f;
Vright = 0.0f;
break;
default:
;
}
// send new speed information to the robot
Robot.setWheels(Vleft, Vright);
}
Robot.garcia.flushQueuedBehaviors();
tcsetattr(fileno(stdout),TCSANOW,&old_mode);
}
void drive(char *filename, grobot & Robot) {
// drive with path from a file
float Vleft = 0.0f;
float Vright = 0.0f;
// open the file
ifstream fileDRIVE;
fileDRIVE.open(filename, ios::in);
if (!(fileDRIVE.is_open())) {
cout << "FAILED to open drive file: " << filename << endl;
} else {
cout << "Opened drive file: " << filename << endl;
while (!(fileDRIVE.eof())) {
// read through file line by line
fileDRIVE >> Vleft >> Vright;
// send new speed information to the robot
Robot.setWheels(Vleft, Vright);
Robot.sleepy();
}
fileDRIVE.close();
Robot.garcia.flushQueuedBehaviors();
}
}
void print_acpValue(acpValue *v) {
acpValue::eType t = v->getType();
if (t == acpValue::kInt) { cout << v->getIntVal(); }
else if (t == acpValue::kFloat) { cout << v->getFloatVal(); }
else if (t == acpValue::kString) { cout << v->getStringVal(); }
else if (t == acpValue::kBoolean) {
const char *s = (v->getBoolVal())?"true":"false";
cout << s;
} else {
cout << "<UNRECOGNIZED acpValue TYPE>";
}
cout << endl;
}
void printHELP() {
// print HELP
cout << "HELP:" << endl
<< "q : QUIT" << endl
<< "f <name> : Read in command file" << endl
<< "d <name> : Drive from file" << endl
<< "b : Battery level, voltage" << endl
<< "m 0.00 : MOVE (distance)" << endl
<< "p 0.00 : PIVOT (angle)" << endl
<< "s 0.00 : set wheel speed" << endl
<< "a 0.00 : set stall threshold" << endl
<< "o : get wheel odometry" << endl
<< "g 0.00 0.00 0.00 : goto displacement/orientation" << endl
<< "i : enter interactive mode" << endl
<< "v <name> : print acpValue" << endl
<< endl;
}
/* Garcia motion program (command and interactive mode)
*
* Dan Flickinger
*
* 2004/09/13
* 2004/11/17
*/
#include <sys/time.h>
#include <unistd.h>
#include <stdio.h>
#include <iostream>
#include <fstream>
#include <stdlib.h>
#include <string>
#include <cmath>
#include <ctype.h>
#include <termios.h>
#include <sys/types.h>
#include <sys/uio.h>
#include "acpGarcia.h"
#include "acpValue.h"
using namespace std;
#include "gcallbacks.h"
#include "grobot.h"
// path generators:
#ifdef PATH_SIMPLE
#include "simplepath.h"
#endif
#ifdef PATH_CUBIC
#include "cubicpath.h"
#endif
/* cubicpath.h
*
* class with methods for generating cubic paths
* (moved from grobot class in grobot.h)
*
* Dan Flickinger
* 2004/10/26
* 2004/10/26
*/
// time constant between waypoints
#define PATH_QUANTUM 0.1f
// define these to keep the pathvars more organized
#define Upi_x pathvars[0]
#define Upi_y pathvars[1]
#define Upf_x pathvars[2]
#define Upf_y pathvars[3]
#define Uvi_x pathvars[4]
#define Uvi_y pathvars[5]
#define Uvf_x pathvars[6]
#define Uvf_y pathvars[7]
#define Uti pathvars[8]
#define Utf pathvars[9]
/* Upi_x: initial position, x
* Upi_y: initial position, y
* Upf_x: final position, x
* Upf_y: final position, y
* Uvi_x: initial velocity, x
* Uvi_y: initial velocity, y
* Uvf_x: final velocity, x
* Uvf_y: final velocity, y