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

robots/primotion/Makefile

+# 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

robots/primotion/circle.cc

+/* 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;
+}

robots/primotion/commotion.cc

+/* 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;
+       
+}
+

robots/primotion/dgrobot/commotion.h

+/* 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

robots/primotion/dgrobot/cubicpath.h

+/* 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