Code for the COVR project DROPSAW project test rig (previously used for the Heros testing).
Dependencies: SPTE_10Bar_5V mbed AS5048 SDFileSystem MODSERIAL LCM101_DROPSAW LinearActuator
main.cpp
- Committer:
- Technical_Muffin
- Date:
- 2018-06-05
- Revision:
- 2:84d479fe9b5e
- Parent:
- 1:417a5b28ac84
- Child:
- 4:1cdce6c6c94e
File content as of revision 2:84d479fe9b5e:
#include "mbed.h" #include "string.h" #include "bench.h" #include "PinDetect.h" #include "SDFileSystem.h" #include "MODSERIAL.h" #define PI 3.14159265 // 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 torque calculations // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = float getTorque(float theta_hip, float theta_knee, float M0) { float theta_shin = theta_knee - theta_hip; //Values for torque calculation const float Mt = 7.24;//mass of thigh in kg const float Ms = 3.06;//mass of shank in kg const float Marm = 2.0;//mass of load carrying arm in kg const float g = 9.81;//gravity coefficient in m/s2 const float dt = 0.45;//length of thigh in m const float dtm = 0.25755;//distance to mass middle point thigh in m const float ds = 0.45;//length of shank in m const float dsm = 0.197134;//distance to mass middle point shank in m const float shank_angle_correction = 6.04;//corrections for the difference in moment arm angle const float thigh_angle_correction = 3.59;//corrections for the difference in moment arm angle float Mw=M0/g;//M0 is measured in N, but it is calculated as kg in the equation. So instead of changing the whole equation, this is a quick fix. //Calculation of knee torque float Tk = -1*(Ms*g)*(dsm*sin((theta_shin+shank_angle_correction)*PI/180))-((-1*(Ms*g)*(dsm*sin((theta_shin+shank_angle_correction)*PI/180))+(Mw+Marm+Mt+Ms)*g*(ds*sin(theta_shin*PI/180))-(Mt*g)*(dtm*sin((theta_hip-thigh_angle_correction)*PI/180))-(Mw+Marm)*g*(dt*sin(theta_hip*PI/180)))/(ds*cos(theta_shin*PI/180)+dt*cos(theta_hip*PI/180)))*(ds*cos(theta_shin*PI/180))+(Mw+Marm+Mt+Ms)*g*(ds*sin(theta_shin*PI/180)); return Tk; } // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = // 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()); pc.printf("\t%5s %7.2f\r\n","Torque", getTorque(leg.getDegrees(3),leg.getDegrees(2),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, torque"); tick_logging.attach_us(&LogData,timing::kTimeLogDataUs); timer.start(); 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(); timer.stop(); timer.reset(); 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, %+f", leg.getDegrees(0), leg.getDegrees(1), leg.getDegrees(2), leg.getDegrees(3), leg.getForce(), getTorque(leg.getDegrees(3),leg.getDegrees(2),leg.getForce()) ); }