HeRoS: read out and log joint angles and force sensor data from the leg test bench. Now with additional features to read pressure sensors and set the null values of the pressure and force sensors
Dependencies: SPTE_10Bar_5V mbed AS5048 SDFileSystem MODSERIAL PinDetect Valve LCM101
Fork of heros_leg_readout_torque_addition by
main.cpp
- Committer:
- megrootens
- Date:
- 2017-12-01
- Revision:
- 0:3855d4588f76
- Child:
- 1:417a5b28ac84
File content as of revision 0:3855d4588f76:
#include "mbed.h" #include "string.h" #include "bench.h" #include "PinDetect.h" #include "SDFileSystem.h" #include "MODSERIAL.h" // User io PinDetect sw2(SW2,PullUp); PinDetect sw3(SW3,PullUp); DigitalOut led_g(LED_GREEN); void TogglePrinting(); void ToggleLogging(); void ShowAlive(); // Bench Bench leg(AS5048_MOSI, AS5048_MISO, AS5048_SCLK, AS5048_CS, LCM101); void Update() { leg.Update(); } // SD Card SDFileSystem sd(SD_MOSI, SD_MISO, SD_SCK, SD_CS, "sd"); void InitSdCard(); void StartLogging(const char * fname_append = "data"); void StopLogging(); void LogData(); // Serial MODSERIAL pc(USBTX,USBRX); void PrintStatus(); void PrintMenu(); // Timing Ticker tick_update, tick_serial, tick_logging; Timer timer; /** * Main loop/ */ int main() { pc.baud(timing::kSerialBaudrate); pc.printf("**Hello!**\r\n"); InitSdCard(); tick_update.attach_us(&Update,timing::kTimeControlUs); tick_serial.attach_us(&PrintStatus,timing::kTimeSerialPrintUs); PrintMenu(); sw2.attach_asserted(&TogglePrinting); sw3.attach_asserted(&ToggleLogging); sw2.setSampleFrequency(); sw3.setSampleFrequency(); while (true); } // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = // IMPLEMENTATION USER IO // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = bool is_printing = false; void TogglePrinting() { if (not is_printing) { is_printing = true; } else { is_printing = false; PrintMenu(); } } bool is_logging = false; void ToggleLogging() { if (not is_logging) { StartLogging(); } else { is_logging = false; StopLogging(); } PrintMenu(); } // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = // IMPLEMENTATION SERIAL COM // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = const int kNumJoints = 4; const char *kJointNames[kNumJoints] = {"Toes","Ankle","knee","Hip"}; void PrintStatus() { led_g = !led_g; if (is_printing) { pc.printf("\r\nLEG STATUS (%s)\r\n",led_g?"+":"*"); for (int i=0; i<kNumJoints; ++i) pc.printf("\t%5s %7.2f\r\n",kJointNames[i], leg.getDegrees(i)); pc.printf("\t%5s %7.2f\r\n","Force", leg.getForce()); } } void PrintMenu() { pc.printf("\r\nMENU\r\n"); pc.printf("\t> Press SW2 to toggle printing leg status\r\n"); pc.printf("\t> Press SW3 to toggle data logging\r\n"); } // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = // IMPLEMENTATION DATA LOGGING // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = FILE * fp_data; bool sd_card_present = false; int fname_prepend = 0; /** * Check contents of SD card and count files in order * to ensure unique file name for logging data */ void InitSdCard() { pc.printf("INITIALIZING 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:"); 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("\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 StartLogging(const char * fname_append) { pc.printf("DATA 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 { fprintf(fp_data, "time_ms, theta_toe, theta_ankle, theta_knee, theta_hip, force"); tick_logging.attach_us(&LogData,timing::kTimeLogDataUs); pc.printf("\t> Logging started.\r\n"); is_logging = true; } } else { pc.printf("\t> No SD Card; no data will be logged.\r\n"); } } /** * Stop logging data */ void StopLogging() { pc.printf("DATA LOGGING:"); if (sd_card_present) { // close data file, stop logging fclose(fp_data); tick_logging.detach(); pc.printf("\r> Stopped."); } else { pc.printf("\t> No data was logged."); } is_logging = false; } /** * Log data */ void LogData() { // time fprintf(fp_data,"\n%d", timer.read_ms()); // bench: joint angles and force sensor fprintf(fp_data,", %+f, %+f, %+f, %+f, %+f", leg.getDegrees(0), leg.getDegrees(1), leg.getDegrees(2), leg.getDegrees(3), leg.getForce() ); }