Torque calculation added to leg readout
Dependencies: AS5048 LCM101 MODSERIAL PinDetect SDFileSystem mbed
Fork of heros_leg_readout by
Diff: main.cpp
- Revision:
- 0:3855d4588f76
- Child:
- 1:417a5b28ac84
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Dec 01 11:14:38 2017 +0000 @@ -0,0 +1,237 @@ +#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() + ); +} \ No newline at end of file