Version 3 is with update to the test rig with a linear actuator
Dependencies: SPTE_10Bar_5V mbed AS5048 SDFileSystem MODSERIAL PinDetect LCM101 LinearActuator
Diff: bench.cpp
- Revision:
- 5:63063a9fa51c
- Parent:
- 4:1cdce6c6c94e
- Child:
- 6:02507d7a6f51
diff -r 1cdce6c6c94e -r 63063a9fa51c bench.cpp --- a/bench.cpp Fri Oct 12 12:12:55 2018 +0000 +++ b/bench.cpp Mon Dec 09 10:51:46 2019 +0000 @@ -7,31 +7,154 @@ * @param miso: miso pin for sensor chain * @param sck: clock pin for sensor chain * @param cs: chip select pin for sensor chain + * @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, - PinName p_lcm101, PinName p_spte0, PinName p_spte1) : + 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), - lcm101_(p_lcm101, sensors::kLcm101Offset, sensors::kLcm101Factor), + 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) -{ + 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 + */ +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]); + } + +} + +/** * Update routine for the testbench. * - 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() +void Bench::update() { as5048_.UpdateAngleBuffer(); } @@ -45,6 +168,253 @@ return ang; } +// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = +// 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 + */ +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 +// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = + +/** + * button lower toggles printing of data + */ +void Bench::TogglePrinting() +{ + if (not is_printing) { + is_printing = true; + } else { + is_printing = false; + PrintMenu(); + } + was_printing = is_printing; +} + +/* + * button lower toggles printing of data + */ +void Bench::ToggleLogging() +{ + if (not is_logging) { + StartLogging(); + } else { + is_logging = false; + StopLogging(); + } + PrintMenu(); +} + +/* + * Indicates if we are now data logging + */ +bool Bench::isLogging() +{ + return is_logging; +} + /** * Obtain the joint angle in degrees. * These are the angles at the time of two Update() calls back @@ -94,12 +464,21 @@ float Bench::getForce() { - return lcm101_.getForce(); + if (Bench::use5kN) { + return loadCell5kN.getForce(); + } else { + return loadCell1kN.getForce(); + } + } void Bench::nullForce() { - return lcm101_.nullForce(); + if (use5kN) { + return loadCell5kN.nullForce(); + } else { + return loadCell1kN.nullForce(); + } } float Bench::getPressure0() @@ -122,4 +501,11 @@ return spte1.nullPressure(); } +void Bench::setValve(bool set) +{ + valveFesto.setValve(set); +} +bool Bench::getValve() { + return valveFesto.getValve(); +} \ No newline at end of file