Version 3 is with update to the test rig with a linear actuator
Dependencies: SPTE_10Bar_5V mbed AS5048 SDFileSystem MODSERIAL PinDetect LCM101 LinearActuator
bench.cpp
- Committer:
- cnckiwi31
- Date:
- 2019-12-09
- Revision:
- 7:edb876c98565
- Parent:
- 6:02507d7a6f51
- Child:
- 8:bc467f5fe7c3
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; }