test/circle.cpp

Go to the documentation of this file.

// This program will run the truck to produce a cicular path at a certain speed
// and record the encoder values to a file


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

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;
        }
}



float calcAdjustedSteeringValue(float value) {   
        static float valueErr = 0.0f;   
        float intValue = roundf(value + valueErr);
        valueErr += value - intValue;   
        return intValue;
}



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

        if ((argc != 4) && (argc != 3)) {
                cout <<
                        "desc  : move the truck in a circle at a given SC power\n"
                        "        and record encoder values to a file \n"
                        "usage : circle <steering value> <SC power> [filename]\n"
                        "     steering value - value for steering servo (0-255)\n"
            "        if the value is fractional the steering controller will\n"
                        "        alternate between two closest integer values\n"
                        "     SC power - power setting for speed controller (-1 to 1)\n"
                        "     filename - name of file to save matlab data script to (default 'circ.m')\n";
                        return -1;
        }  

        errno = 0;
        char *temp;
        float value = strtof(argv[1], &temp);
        if (errno != 0) {
                cerr << "could not convert '" << argv[1] << "' to float :"
                         << strerror(errno) << endl;
                return -1;
        }
        
        float power = strtof(argv[2], &temp);
        if (errno != 0) {
                cerr << "could not convert '" << argv[2] << "' to float :"
                         << strerror(errno) << endl;
                return -1;
        }

        if ((power > 0.1) || (power < -0.1)) {
                cerr << "power of " << power << " is just too much to be safe!" << endl
                         << "change circle.cpp to allow this if it is really what you want" << endl;
                return -1;
        }

        
        const char *filename = "/tmp/circ.m";
        if (argc>=4)
                filename = argv[3];

        ofstream f(filename);
        if (!f) {
                cerr << "could not open file '" << filename << "' for writing" << endl;
                return -1;
        }
        
        // initialize intreface to truck hardware and set steering servo value
        hwInterface hw( 1 /*enable write control*/);
        hw.reset();
        int intValue = (int) calcAdjustedSteeringValue(value);
        hw.setSteeringValue(intValue);
        hw.writeControl();
        
        // install ctrl-c signal handler
        if (signal(SIGINT,sighandler) == SIG_ERR) {
                cerr << "error installing signal handler" << endl;
                return -1;
        }
        
        // write debugging info to a file that can be run by matlab
        f << "% circle data" << endl
          << "steeringValue=" << value << endl
          << "power=" << power << endl
          << "% [time, encoder fr, fl, br, bl, steering A2d, steering value]" << endl
          << "data=[" << endl; 
        
        // Let the truck be adusted before starting, if nessasary
        cout << "press ENTER when truck is ready" << endl;
        getchar();

        float time = 0.0f;
        hw.setAdjustedSCPower(power);
        hw.writeControl();
        
        while (!quit) {         

                for (int j = 1; (j<33) && (!quit); ++j) {
                        for (int i = 1; i<3; ++i) {
                                hw.readStatus();
                                f << '['
                                  << time << ",\t" 
                                  << hw.encTicksFR() << ",\t"
                                  << hw.encTicksFL() << ",\t"
                                  << hw.encTicksBR() << ",\t"
                                  << hw.encTicksBL() << ",\t"
                                  << hw.getSteeringA2DValue() << ",\t"
                                  << intValue << " ]" << endl;                                                          
                                hw.wait();
                                time += 0.01;
                        }               

                        intValue = (int) calcAdjustedSteeringValue(value);
                        hw.setSteeringValue(intValue);
                        hw.setAdjustedSCPower(power);
                        hw.writeControl();
                }
                        
                cout  << hw.encTicksFR() << '\t'
                          << hw.encTicksFL() << '\t'
                          << hw.encTicksBR() << '\t'
                          << hw.encTicksBL() << endl;           
        }

        f << "];" << endl;
        
        hw.setAdjustedSCPower(0);
        hw.writeControl();
        
        return 0;
}

---