File content as of revision 15:a84d54e25775:

/*  Copyright 2020 Allan Joshua Veale
 
    Licensed under the Apache License, Version 2.0 (the "License");
    you may not use this file except in compliance with the License.
    You may obtain a copy of the License at
 
        http://www.apache.org/licenses/LICENSE-2.0
 
    Unless required by applicable law or agreed to in writing, software
    distributed under the License is distributed on an "AS IS" BASIS,
    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
    See the License for the specific language governing permissions and
    limitations under the License.
*/

#include "bench.h"

/** constructor
 * 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 p_laDir: digital output for linear actuator direction (retract/extend)
 * @param p_laPwm: pwm output for linear actuator speed
 * @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
 */
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 p_laDir, PinName p_laPwm,
             PinName sd_mosi, PinName sd_miso, PinName sd_sck, PinName sd_cs,
             PinName tx, PinName rx) :
    pc(tx,rx),
    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),
    LinAct(p_laDir,p_laPwm),
    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;//have not checked for an sd card
    fname_prepend = 0;//have not checked what is on the sd card
    is_logging = false;//not datalogging
    is_printing = false;//not printing anything
    was_printing = false;//was not printing
        
    usedExtraCols = 0;//default
    
    firstReadMS = 0; //first timer value read
    startedLogging = false; //not in the middle of a logging cycle
    
    keyIntOffset = timing::minKeyInt;//any right keypress is ok at the start
    k = '\0'; //keyboard value starts off as a null value
    
    //set data logging frequency
    loggingUS = timing::kTimeLogDataUs;    
    loggingHz = (int)(1000000/timing::kTimeLogDataUs); 
    
    //set all angle sensor calibration data
    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 for the control loop (which only reads the angle sensors)
    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 timer to read key presses
    Bench::pc.attach(this,&Bench::ToggleState,MODSERIAL::RxIrq);
    
    //display welcome message
    pc.printf("\r\n**Hello!**\r\n");
    pc.printf("Version: V4\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 - when automatic datalogging is used
 * @param hertz: logging frequency in Hz
 */
void Bench::setLoggingFrequency(float hertz)
{
    //set data logging frequency
    if (hertz > 0) {
        loggingUS = 1000000/hertz;
        loggingHz = hertz;
    } else { //default if bad value supplied
        loggingUS = timing::kTimeLogDataUs;
        loggingHz = (int)(1000000/timing::kTimeLogDataUs);
    }
}

/**
 * Sets names of extra columns of data to be recorded (those more than maxCols are 
 * ignored)
 * @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;
    // save the names   
    for(int i=0; i<maxCols; i++) {
        if (i<numCols) {
            extraColNames[i] = extraColumnNames[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];
    }    
}

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

/**
 * Update routine for the test rig
 * - Updates the angle buffer of the sensor array
 * 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 (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;
}

/**
 * @param joint: the joint for which the angle is requested (a number starting 
 * from 0 indicating the position in the sensor daisy chain)
 * @return the name of the joint number
 */
const char* Bench::getJointName(int i_joint) 
{
    return sensors::kJointNames[i_joint];
}

/**
 * @return the encoder object reference
 */
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();
    }
    
}

/**
 * zero the force reading (like a tare function)
 */
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();
}

/**
 * zero the pressure reading (like a tare function)
 */
void Bench::nullPressure0()
{
    return spte0.nullPressure();
}

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

/**
 * zero the pressure reading (like a tare function)
 */
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();   
}

/**
 * The linear actuator state (true is retracting and false is extending)
 * @return: linear actuator state
 */
bool Bench::getDir() {
    return LinAct.getDirection();   
}

/**
 * Control the linear actuator direction (true retracts and false extends)
 * @param set: direction
 */
void Bench::setDir(bool dir)
{
    LinAct.setDirection(dir);
}

/**
 * Control the linear actuator speed (0-100% integer of pwm. generally need 
 * above 50% to get it moving)
 * @param pwm: pwm duty cycle percentage
 */
void Bench::setPWM(int pwm)
{
    LinAct.setPWM(pwm);
}

// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
// 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) {//if there is an SD card
        sd_card_present = true;
        
        //print existing files
        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 (now manual, meaning user has to call a method to record a datapoint
 * but if tick_logging.attach...(...) is uncommmented
 * then recording becomes automatic)
 * 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)";//default data points recorded
            
            for (int i=0; i<usedExtraCols; i++) {
                if (extraColNames[i] != "") {
                    fHeader = fHeader + "," + extraColNames[i];
                }
            }
            fprintf(fp_data, "%s", fHeader.c_str());
            //tick_logging.attach_us(this,&Bench::LogData,loggingUS); - uncomment to automatically log data
            timer.start();//enable a time stamp for the logging file
            startedLogging = true;            
            is_logging = true;
        }
    } else {
        pc.printf("\t> No SD Card; no data will be logged.\r\n");
        startedLogging = false;
    }
}

/**
 * Restart logging data (opens data logging file)
 * @param fname_append: a string describing the name appended to each logged file
 * (along with its unique loggging number).
 */
void Bench::RestartLogging(const char * fname_append)
{    
    if (sd_card_present) {
 
        // file name carries on from what we last had
        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 {            
            timer.start();
            startedLogging = true;
            
            pc.printf("\t> Logging file opened.\r\n");
            
            is_logging = true;
        }
    } else {
        pc.printf("\t> No SD Card; no data will be logged.\r\n");
        startedLogging = false;
    }
}

/**
 * Stop automatically logging data.
 */
void Bench::StopLogging()
{
    Bench::pc.printf("\r\nDATA LOGGING:");
    if (sd_card_present) {
        // close data file, stop logging, reset time stamp
        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 (record all the data points, default and custom with a timestamp)
 */
void Bench::LogData()
{
    int currTime = timer.read_ms();
    if(startedLogging) {//the first time this is called, save the timer offset
        firstReadMS = currTime;
        startedLogging = false;
    }
    double currTimeS = ((double)currTime - firstReadMS)/1000;//time stamp
    
    // time
    fprintf(fp_data,"\n%f",currTimeS);

    // bench default datapoints: joint angles and force sensor and pressure sensor values
    fprintf(fp_data,",%f,%f,%f",
            getDegrees(0),
            getForce(),            
            getPressure0()*100
           );
    //custom datapoints
    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)
        {
            //file name and whether logging is happening
            Bench::pc.printf("\tFile number %15i\r\n", fname_prepend);
            Bench::pc.printf("\tLogging? %18s\r\n",is_logging?"Yes":"No");
        }
        
        //angles printed
        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()); //force reading        
        Bench::pc.printf("\t%15s %7.2f\r\n","Pressure0 (kPa)",  getPressure0()*100); //pressure reading 
    }
}

/**
 * Prints the user choices to be made
 */
void Bench::PrintMenu()
{
    Bench::pc.printf("\r\nMENU\r\n");
    Bench::pc.printf("\t> Press s to toggle printing leg status\r\n");
    Bench::pc.printf("\t> Press l 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
// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =

/**
 * s toggles printing state of sensors and l toggles logging of data
 * use the keyboard on the pc
 * do not press longer than a minKeyInt or it will toggle back
 */
void Bench::ToggleState(MODSERIAL_IRQ_INFO *q)
{
    //if we have data and it was not too soon since the last data
    if(Bench::pc.readable() && (((Bench::keyIntT.read() + keyIntOffset) >= timing::minKeyInt))) {
        k = Bench::pc.getc();//read the keystroke
        Bench::pc.rxBufferFlush();//delete all recorded keystrokes
        
        if(k == 's') {//toggle printing of data
            Bench::pc.printf("\tInput: %c\r\n",k);
            TogglePrinting();
            keyIntT.reset();
            keyIntT.start();
            keyIntOffset = 0;
        } else if (k == 'l') {//toggle status of logging
            Bench::pc.printf("\tInput: %c\r\n",k);
            ToggleLogging();
            keyIntT.reset();
            keyIntT.start();
            keyIntOffset = 0;
        }
        k = '\0';
    }
}

/**
 * toggles printing of data
 */
void Bench::TogglePrinting()
{
    if (not is_printing) {
        is_printing = true;
    } else {
        is_printing = false;
        PrintMenu();//data no longer printed, print home menu
    }
    was_printing = is_printing;
}

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

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