File content as of revision 7:edb876c98565:

#include "bench.h"

/**
 * Create an object representing the test rig
 * @param mosi: mosi pin for sensor chain (SPI angle sensors)
 * @param miso: miso pin for sensor chain (SPI angle sensors)
 * @param sck: clock pin for sensor chain (SPI angle sensors)
 * @param cs: chip select pin for sensor chain (SPI angle sensors)
 * @param use5kNLoadCell: if 5kN load cell is used (if false, 1kN sensor is used)
 * @param p_lcm101: analog input pin for load cell
 * @param p_spte0: analog input pin for pressure sensor 0
 * @param p_spte1: analog input pin for pressure sensor 1
 * @param p_valve: digital output for valve (on/off) that inflates leg actuator
 * @param mosi: mosi pin for sd card
 * @param miso: miso pin for sd card
 * @param sck: clock pin for sd card
 * @param cs: chip select pin for sd card
 * @param tx: serial transmission line
 * @param rx: serial receive line
 * @param but0: first input button 
 * @param but1: second input button
 */
Bench::Bench(PinName mosi, PinName miso, PinName sck, PinName cs,
             bool use5kNLoadCell,PinName p_lcm101, PinName p_spte0, PinName p_spte1, PinName p_valve,
             PinName sd_mosi, PinName sd_miso, PinName sd_sck, PinName sd_cs,
             PinName tx, PinName rx,
             PinName but0, PinName but1) :
    pc(tx,rx),
    lowerBut(but0,PullUp),
    upperBut(but1,PullUp),
    as5048_(mosi, miso, sck, cs, sensors::kNumJoints),
    loadCell5kN(p_lcm101, sensors::kGen5kNOffset, sensors::kGen5kNFactor),
    loadCell1kN(p_lcm101, sensors::kLcm101Offset, sensors::kLcm101Factor),
    use5kN(use5kNLoadCell),
    spte0(p_spte0, sensors::kSPTE0Offset, sensors::kSPTE0Factor),
    spte1(p_spte1, sensors::kSPTE1Offset, sensors::kSPTE1Factor),
    valveFesto(p_valve),
    sd(sd_mosi, sd_miso, sd_sck, sd_cs, "sd")
{    
    //init serial things
    pc.baud(timing::kSerialBaudrate);//set the serial rate
    
    sd_card_present = false;
    fname_prepend = 0;
    is_logging = false;
    is_printing = false;
    was_printing = false;
        
    usedExtraCols = 0;
    
    firstReadMS = 0; //first timer value read
    startedLogging = false; //in the middle of a logging cycle
    
    //set data logging frequency
    loggingUS = timing::kTimeLogDataUs;    
    loggingHz = (int)(1000000/timing::kTimeLogDataUs); 
    
    for (int i=0; i<sensors::kNumJoints; ++i) {
        as5048_.setOffsetDegrees(i,sensors::kOffsetsDegrees[i]);
        as5048_.setDirection(i,sensors::kDirections[i]);
    }   
}

/**
 * Initialises: timers, SD card
 * Prints a welcome message and information on the logging frequency and sensor
 * reading rate
 */
void Bench::initialise()
{
    //setup the timing
    tick_update.attach_us(this,&Bench::update,timing::kTimeControlUs);
            
    // set rate at which data is printed
    tick_serial.attach_us(this,&Bench::PrintStatus,timing::kTimeSerialPrintUs);
     
    //setup the buttons with debouncing    
    lowerBut.attach_asserted(this,&Bench::TogglePrinting);
    upperBut.attach_asserted(this,&Bench::ToggleLogging);
    
    lowerBut.setSampleFrequency();
    upperBut.setSampleFrequency();
    
    //display welcome message
    pc.printf("\r\n**Hello!**\r\n");
    pc.printf("Version: 3/12/2019 - 00:00\r\n\n");
    
    Bench::pc.printf("5kN load cell? %s\r\n\n",sensors::use5kN?"Yes":"No");
    
    pc.printf("Bench update rate (Hz): %i\r\n",timing::TimeControlHertz);
    pc.printf("Logging rate (Hz): %i\r\n\n",(int)loggingHz);
    
    //startup the SD card
    InitSdCard();
    
    //Display the menu
    PrintMenu();    
}

/**
 * Sets the rate at which data is logged in an experiment
 * @param hertz: logging frequency in Hz
 */
void Bench::setLoggingFrequency(float hertz)
{
    //set data logging frequency
    if (hertz > 0) {
        loggingUS = 1000000/hertz;
        loggingHz = hertz;
    } else {
        loggingUS = timing::kTimeLogDataUs;
        loggingHz = (int)(1000000/timing::kTimeLogDataUs);
    }
}

/**
 * Sets names of extra columns of data to be recorded
 * @param extraColumnNames: string array of the column names
 * @param numCols: number of elements in the string array
 */
void Bench::setExtraColumns(string extraColumnNames[], int numCols)
{
    usedExtraCols = numCols;
    //pc.printf("Length: %i\r\n",usedExtraCols);
    // save the names   
    for(int i=0; i<maxCols; i++) {
        if (i<numCols) {
            extraColNames[i] = extraColumnNames[i];
            //pc.printf("\r\nS%d: %s\r\n",i,extraColNames[i]);
        } else {
            extraColNames[i] = ""; //less columns than the max possible are filled with a null space
        }
    }
    
}

/**
 * Sets values of extra columns of data to be recorded
 * @param data: data to be logged with the extra columns
 */
void Bench::setExtraData(float data[])
{
    for (int i=0; i<usedExtraCols; i++) {
        extraColValues[i] = data[i];
        //pc.printf("\r\nD%d: %f\r\n",i,extraColValues[i]);
    }
    
}

// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
// IMPLEMENTATION HARDWARE INTERFACE
// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =

/**
 * Update routine for the test rig
 * - Updates the angle buffer of the sensor array
 * - ... that's it for now (add filtering?)
 * Note that angles lag one update() behind, due to the way the SPI
 * protocol works.
 */
void Bench::update() 
{
    as5048_.UpdateAngleBuffer();
}

/**
 * Obtain the joint angle in degrees
 * These are the angles at the time of two Update() calls back
 * @param joint: the joint for which the angle is requested (as enumerated in 
 * class header - TOES, KNEE, ANKLE, HIP
 * @return: joint angle
 */
float Bench::getDegrees(Joint joint) 
{
    return getDegrees(joint);
}

/**
 * Obtain the joint angle in degrees
 * These are the angles at the time of two Update() calls back
 * @param joint: the joint for which the angle is requested (a number starting 
 * from 0 indicating the position in the sensor daisy chain)
 * @return: joint angle
 */
float Bench::getDegrees(int i_joint) 
{
    float ang = as5048_.getAngleDegrees(i_joint);
    if (ang>kCutOffDegrees) {
        return ang-As5048::kDegPerRev;
    }
    return ang;
}

/**
 * Obtain the joint angle in radians
 * These are the angles at the time of two Update() calls back
 * @param joint: the joint for which the angle is requested (as enumerated in 
 * class header - TOES, KNEE, ANKLE, HIP
 * @return: joint angle
 */
float Bench::getRadians(Joint joint) 
{
    return getRadians(joint);
}

/**
 * Obtain the joint angle in radians
 * These are the angles at the time of two Update() calls back
 * @param joint: the joint for which the angle is requested (a number starting 
 * from 0 indicating the position in the sensor daisy chain)
 * @return: joint angle
 */
float Bench::getRadians(int i_joint) 
{
    float ang = as5048_.getAngleRadians(i_joint);
    if (ang>kCutOffRadians) {
        return ang-As5048::kRadPerRev;
    }
    return ang;
}


const char* Bench::getJointName(int i_joint) 
{
    return sensors::kJointNames[i_joint];
}

const char* Bench::getJointName(Joint joint) 
{
    return getJointName(joint);
}

As5048* Bench::get_as5048() 
{
    return &as5048_;
}

/**
 * The force applied vertically at the hip joint in N
 * @return: force
 */
float Bench::getForce() 
{
    if (Bench::use5kN) {
        return loadCell5kN.getForce();
    } else {
        return loadCell1kN.getForce();
    }
    
}

void Bench::nullForce()
{
    if (use5kN) {
        return loadCell5kN.nullForce();
    } else {
        return loadCell1kN.nullForce();
    }
}

/**
 * The pressure measured by pressure sensor 0 in bar
 * @return: pressure
 */
float Bench::getPressure0() 
{
    return spte0.getPressure();
}

void Bench::nullPressure0()
{
    return spte0.nullPressure();
}

float Bench::getPressure1() 
{
    return spte1.getPressure();
}

void Bench::nullPressure1()
{
    return spte1.nullPressure();
}

/**
 * Control the valve (true turns the valve on to pressurise, false turns 
 * the valve off to depressurise)
 * @param set: valve state
 */
void Bench::setValve(bool set)
{
    valveFesto.setValve(set);
}

/**
 * The valve state (true is on/pressurising and false is off/depressurising).
 * @return: valve state
 */
bool Bench::getValve() {
    return valveFesto.getValve();   
}

// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
// IMPLEMENTATION DATA LOGGING
// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =

/**
 * Check contents of SD card and count files in order
 * to ensure unique file name for logging data.
 */
void Bench::InitSdCard()
{
    pc.printf("\r\nINITIALISING SD CARD\r\n");

    int num_files = 0;

    // scan dir
    DIR *d;
    struct dirent *p;

    d = opendir("/sd");
    if (d != NULL) {
        sd_card_present = true;

        pc.printf("\t> Contents of SD Card:\r\n");
        while ((p = readdir(d)) != NULL) {
            if (p->d_name[0] != '.') {
                // skip files starting with '.'
                pc.printf("\t  %s",p->d_name);
                ++num_files;
            }
        }
        pc.printf("\r\n\t> Counted %d visible files.\r\n",num_files);

        closedir(d);
    } else {
        sd_card_present = false;

        pc.printf("\t> No SD Card present. Data cannot be logged.\r\n");
    }

    // id to be appended to logged data files
    fname_prepend = num_files;
}

/**
 * Start logging data
 * param fname_append: a string describing the name appended to each logged file
 * (along with its unique loggging number).
 */
void Bench::StartLogging(const char * fname_append)
{    
    pc.printf("\r\nDATA LOGGING");
    if (sd_card_present) {

        // create unique file name
        ++fname_prepend;
        char fname[50];
        sprintf(fname, "/sd/%d_%s.csv",fname_prepend,fname_append);

        pc.printf("\t> Opening data log file '%s'...\r\n",fname);

        // open file for writing and start logging after success
        fp_data = fopen(fname,"w");
        if (fp_data==NULL) {
            pc.printf("\t> ERROR: failed to open log file (t=%d ms)\r\n",
                      timer.read_ms());
        } else {
            string fHeader = "time (s),theta_knee (deg),force (N),pressure (kPa)";
            
            for (int i=0; i<usedExtraCols; i++) {
                if (extraColNames[i] != "") {
                    fHeader = fHeader + "," + extraColNames[i];
                }
            }
            //pc.printf("%s", fHeader.c_str());
            fprintf(fp_data, "%s", fHeader.c_str());
            tick_logging.attach_us(this,&Bench::LogData,loggingUS);
            timer.start();
            startedLogging = true;
            
            pc.printf("\t> Logging started.\r\n");
            
            is_logging = true;
        }
    } else {
        pc.printf("\t> No SD Card; no data will be logged.\r\n");
        startedLogging = false;
    }
}


/**
 * Stop logging data.
 */
void Bench::StopLogging()
{
    Bench::pc.printf("\r\nDATA LOGGING:");
    if (sd_card_present) {
        // close data file, stop logging
        fclose(fp_data);
        tick_logging.detach();
        timer.stop();
        timer.reset();
        Bench::pc.printf("\r> Stopped.");
    } else {
        Bench::pc.printf("\t> No data was logged.");
    }
    
    is_logging = false;
}

/**
 * Stop logging data and print the menu
 */
void Bench::stopLogging()
{
    if(is_logging) {
        is_logging = false;
        StopLogging();
        PrintMenu();
    }    
}

/**
 * Log data
 */
void Bench::LogData()
{
    int currTime = timer.read_ms();
    if(startedLogging) {
        firstReadMS = currTime;
        startedLogging = false;
    }
    double currTimeS = ((double)currTime - firstReadMS)/1000;
    
    // time
    fprintf(fp_data,"\n%f",currTimeS);

    // bench: joint angles and force sensor and pressure sensor values
    fprintf(fp_data,",%f,%f,%f",
            getDegrees(0),
            getForce(),            
            getPressure0()*100
           );
    for (int i=0; i<usedExtraCols; i++) {
        fprintf(fp_data,",%f",extraColValues[i]);
    }            
}

// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
// IMPLEMENTATION SERIAL COM
// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =      
/**
 * Prints the values of the key measurements in the setup
 */
void Bench::PrintStatus()
{    
    if (is_printing) {
        if(sd_card_present)
        {
            Bench::pc.printf("\tFile number %15i\r\n", fname_prepend);
            Bench::pc.printf("\tLogging? %18s\r\n",is_logging?"Yes":"No");
        }
        
        for (int i=0; i<sensors::kNumJoints; ++i)
        {
            string jointName = sensors::kJointNames[i];
            jointName = jointName + " (deg)";
            Bench::pc.printf("\t%15s %7.2f\r\n",jointName, getDegrees(i));
        }
        Bench::pc.printf("\t%15s %7.2f\r\n","Force (N)",  getForce()); 
        
        Bench::pc.printf("\t%15s %7.2f\r\n","Pressure0 (kPa)",  getPressure0()*100);   
    }
}

/**
 * Prints the user choices to be made
 */
void Bench::PrintMenu()
{
    Bench::pc.printf("\r\nMENU\r\n");
    Bench::pc.printf("\t> Press SW2 to toggle printing leg status\r\n");
    Bench::pc.printf("\t> Press SW3 to toggle data logging\r\n");
    
    Bench::pc.printf("\tSD card detected? %9s\r\n",sd_card_present?"Yes":"No");    
    if(sd_card_present)
    {
        Bench::pc.printf("\tFile number %15i\r\n", fname_prepend);
        Bench::pc.printf("\tLogging? %18s\r\n",is_logging?"Yes":"No");
    }
}

/**
 * Stops the Bench class from printing data if it is
 */
void Bench::pausePrint()
{
    was_printing = is_printing;
    is_printing = false;
}

/**
 * Resumes printing in the Bench class
 */
void Bench::resumePrint()
{
    is_printing = was_printing;
}

// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
// IMPLEMENTATION USER IO
// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =

/**
 * SW2 toggles printing of data
 */
void Bench::TogglePrinting()
{
    if (not is_printing) {
        is_printing = true;
    } else {
        is_printing = false;
        PrintMenu();
    }
    was_printing = is_printing;
}

/*
 * SW3 toggles logging of data
 */
void Bench::ToggleLogging()
{
    if (not is_logging) {
        StartLogging();
    } else {
        is_logging = false;
        StopLogging();
    }        
    PrintMenu();
}

/*
 * Indicates if we are now data logging
 * return: true if logging
 */
bool Bench::isLogging()
{
    return is_logging;
}