Torque calculation added to leg readout
Dependencies: AS5048 LCM101 MODSERIAL PinDetect SDFileSystem mbed
Fork of heros_leg_readout 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() ); }