test/freqresp.cpp

Go to the documentation of this file.

// This program will run a sine wave with a given amplitude, frequency, and DC
// offset to the motor inputs. It will record the wheel speeds to 
// /tmp/freq_resp.dat 


#include "../truckd/hw_interface.h"
#include <math.h>
#include <unistd.h>
#include <fstream>
#include <iostream>
#include <signal.h>
#include <sys/time.h>
#include <errno.h>

using namespace std;

int quit=0;


// signal handler will set quit the first time ctrl-c is pressed 
// called and will force on exit the second time
void sighandler(int signum) {
  if (signum == SIGINT) {
    if (!quit) {      
      cerr << "caught ctrl-C quitting" << endl;
      quit = 1;
    } else {
      cerr << "second ctrl-C forcing exit" << endl;
      exit (EXIT_FAILURE);
    }
  } else {
    cerr << "caught unknown signal?" << endl;
  }
}

//does error checking on do get time of day
int getTime(struct timeval &time) {
  if (gettimeofday(&time,NULL) == -1) {
    cerr << "error with gettimeofday : " << strerror(errno) << endl;
    return -1;
  }
  return 0;
}

//returns the difference between two timevalues in milliseconds
int timeDiffMS(struct timeval now, struct timeval old) {
  return (now.tv_sec - old.tv_sec) * 1000 + (now.tv_usec - old.tv_usec) / 1000;  
}       


int main(int argc, char *argv[]) {


  if ((argc != 5) && (argc != 6)) {
    cout <<
      "desc  : produces a sine wave input to the motor controller,\n" 
      "        measures motor response, and saves data to a file\n" 
      "usage : freqresp <options> freq P2Pamp DCoff\n" 
      "    freq    = frequecy of input sine wave (should be < 1HZ)\n" 
      "    P2Pamp  = peak to peak amplitude of input sine wave\n" 
      "    DCoff   = average value (DC offset) of sine wave output\n" 
      "    options = -f <file> ; -t <time>\n" 
      "       file = file to save data too (default = /tmp/freqresp.dat\n"
      "       time = number of seconds to run test for (default = 100 seconds)\n";
      return -1;
  }  

  char *filename = "/tmp/freqresp.dat";
  float runTime = 100;

  // parse any command line options
  while(1) {
    opterr = 0;
    int opt = getopt(argc,argv,":t:f:");

    if (opt == -1) 
      break;
    
    switch (opt) {
    case 'f':
      filename = optarg;
      break;
    case 't':
      if (sscanf(optarg,"%f", &runTime) != 1) {
        cerr << "can't convert '" << optarg << "' to number" << endl;
        return -1;
      }
      break;
    case '?':
      cerr << "unknown option '" << (char) optopt << "'" << endl;
      return -1;
    case ':':
      cerr << "missing argument for -" << (char) optopt << endl;
      return -1;
    default:
      cerr << "weight getopt result?" << endl;
      return -1;
    }
  } 
        
  ofstream data(filename);
  if (!data) {
    cerr << "error openning file '" << filename << "'" << endl;
    return -1;
  }
            
  float freq, p2p, dc_off;

  //parse command-line arguments
  if (sscanf(argv[optind], "%f",&freq) != 1) {
    cerr << "error : '" << argv[optind] << "' is not a number" << endl;
    return -1;
  }
  if (sscanf(argv[optind+1], "%f",&p2p) != 1) {
    cerr << "error : '" << argv[optind+1] << "' is not a number" << endl;
    return -1;
  }
  if (sscanf(argv[optind+2], "%f",&dc_off) != 1) {
    cerr << "error : '" << argv[optind+2] << "' is not a number" << endl;
    return -1;
  }

  cout << "running for " << runTime << " seconds" << endl;
  cout << "saving results to '" << filename << "'" << endl;  
  cout << " running with fq=" << freq << " p2p=" << p2p << " off=" << dc_off << endl;
  data << "# running with fq=" << freq << " p2p=" << p2p << " off=" << dc_off << endl;  

  //initialize intreface to truck hardware
  hwInterface hw( 1 /*enable write control*/);
  hw.reset();  

  // install ctrl-c signal handler
  if (signal(SIGINT,sighandler) == SIG_ERR) {
    cerr << "error installing signal handler" << endl;
    return -1;
  }

  // initialize variables for control loop
  float avgTickDelta = 0.0f;
  float avgTickDelta2 = 0.0f;
  float time = 0.0f;
  int cycleCount = 0;
  struct timeval startTime, currentTime;
  hw.readStatus();
  float oldTicks = (float) (hw.encTicks(0) + hw.encTicks(1) + hw.encTicks(2) + hw.encTicks(3));
  if (getTime(startTime))
    return -1;

  //run loop until time runs out or ctrl-c is pushed
  while ((!quit) && (time <= runTime)) {    
    hw.readStatus();
    
    // update stats 
    float ticks = (float) (hw.encTicks(0) + hw.encTicks(1) + hw.encTicks(2) + hw.encTicks(3));
    float tickDelta = ticks-oldTicks;
    oldTicks = ticks;
    avgTickDelta += tickDelta;  
    avgTickDelta2 += tickDelta * tickDelta;
    ++cycleCount;

    // set output power
    float power = dc_off + 0.5f * p2p * sinf(2.0f * M_PI * time * freq);
    hw.setSCPower(power);
    hw.writeControl();

    // write stats to data file
    if (getTime(currentTime)) 
      break;            
    data << timeDiffMS(currentTime,startTime) << '\t' 
         << time << '\t' 
         << power << '\t' 
         << tickDelta << endl;
    time += 0.100;  //use 100ms timing loop    

    // wait 100ms
    for (int i=0; i<10; ++i)
      hw.wait();
  }

  // make sums into averages
  if (cycleCount > 0) {
    avgTickDelta /= (float) cycleCount;
    avgTickDelta2 /= (float) cycleCount;
  }

  data << "# Ran for " << time << " seconds : " 
       << "tick delta : average=" << avgTickDelta
       << " stdev=" << sqrtf(avgTickDelta2 - avgTickDelta*avgTickDelta) << endl;

  cout << " E(X^2) = " << avgTickDelta2 << endl;
  cout << "Ran for " << time << " seconds : "   
       << "tick delta : average=" << avgTickDelta
       << " stdev=" << sqrtf(avgTickDelta2 - avgTickDelta*avgTickDelta) << endl;

  // set power back to 0
  hw.setSCPower(0);

  return 0;
}



// LOST CODE : maybe it will be used someday

/*
class freqTest
{
 protected:
  hwInterface *hw;
  float freq, p2p, dc_off;
  timeval startTime;
  float currentTime;
  float oldSpeed;
  float tickDelta;
  float avgTickDelta;
  float phase;

 public:  
  freqTest(hwInterface &hw, float freq, float p2p, float dc_off) : 
    hw(&hw),
    freq(freq),
    p2p(p2p),
    dc_off(dc_off)    
    currentTime(0.0f),
    oldSpeed(0.0f),
    tickDelta(0),
    avgTickDelta(0),    
  {
    if(getTime(startTime))
      exit(EXIT_FAILURE);    
  }
    
  void step(void) {
    hw->readStatus();
    
    //determine what next speed controller power setting will be
    float power = dc_off + 0.5f * p2p * sinf(2f * M_PI * (time * freq) - phase);
    hw->setSCPower(power);
    
    //update stats
    int ticks = hw.encTicks(0) + hw.encTicks(1) + hw.encTicks(2) + hw.encTicks(3);
    tickDelta = ticks-oldTicks;
    oldTicks = ticks;

    cycleCount

    hw->writeControl();
  };
*/

---