![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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: main.cpp
- Revision:
- 5:63063a9fa51c
- Parent:
- 4:1cdce6c6c94e
- Child:
- 10:77fcbad99a31
--- a/main.cpp Fri Oct 12 12:12:55 2018 +0000 +++ b/main.cpp Mon Dec 09 10:51:46 2019 +0000 @@ -1,322 +1,120 @@ -#include "mbed.h" -#include <string> - -#include "bench.h" -#include "PinDetect.h" -#include "SDFileSystem.h" -#include "MODSERIAL.h" - -#define PI 3.14159265 - -// User io - -PinDetect forceNull(PTB16,PullUp); -PinDetect press0Null(PTA4,PullUp); - -PinDetect sw2(SW2,PullUp); -PinDetect sw3(SW3,PullUp); -DigitalOut led_g(LED_GREEN); - -void ResetForce(); -void ResetPress(); -void TogglePrinting(); -void ToggleLogging(); -void ShowAlive(); +/** +* Author: Allan Veale +* Date: 27/11/19 +* Purpose: Datalog from the active wearable test rig fitted with the first +* realistic (foam tissue) leg +*/ -// Bench -Bench leg(AS5048_MOSI, AS5048_MISO, AS5048_SCLK, AS5048_CS, LCM101,SPTE_0,SPTE_1); -void Update() -{ - leg.Update(); -} - -// SD Card -SDFileSystem sd(SD_MOSI, SD_MISO, SD_SCK, SD_CS, "sd"); +//Both the general mbed header and the test rig bench header are needed +#include "mbed.h" +#include "bench.h" -void InitSdCard(); -void StartLogging(const char * fname_append = "data"); -void StopLogging(); -void LogData(); - -// Serial -MODSERIAL pc(USBTX,USBRX); +//Example experiment method +void runDemoExperiment0(int cycles, float targetkPa); -void PrintStatus(); -void PrintMenu(); - -// Timing -Ticker tick_update, tick_serial, tick_logging; -Timer timer; -int firstReadMS = 0; //first timer value read -bool startedLogging = false; //in the middle of a logging cycle +// Create bench object - this is used to control the test rig +Bench leg; /** - * Main loop/ + * 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(); +{ + leg.setLoggingFrequency(100); //Set datalogging frequency + + /* Two extra columns of data will be recorded in this experiment. + One for the target pressure, and the other for the number of sit and + stand cycles currently completed in the experiment */ + string colNames[] = {"Target pressure (kPa)","Cycle"}; //add data headings + leg.setExtraColumns(colNames,2); - forceNull.attach_asserted(&ResetForce); - press0Null.attach_asserted(&ResetPress); - - forceNull.setSampleFrequency(); - press0Null.setSampleFrequency(); + float targetP = 200; //Target pressure in kPa + int expCycles = 2; //Number of sit to stand to sit cycles + float vals[] = {targetP,0}; //set initial values of data that will be logged + leg.setExtraData(vals); - sw2.attach_asserted(&TogglePrinting); - sw3.attach_asserted(&ToggleLogging); - - sw2.setSampleFrequency(); - sw3.setSampleFrequency(); + /* Setup all peripherals on rig, display info about SD card and the + user interface menu */ + leg.initialise(); - 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 -// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = - -void ResetForce() -{ - leg.nullForce(); -} - -void ResetPress() -{ - leg.nullPressure0(); - leg.nullPressure1(); -} - -bool is_printing = false; -void TogglePrinting() -{ - if (not is_printing) { - is_printing = true; - } else { - is_printing = false; - PrintMenu(); + + /*Run an experiment when the button is pressed to start datalogging and + stop it if the button is pressed again to stop datalogging + (or when experiment stops - then datalogging stops by itself) */ + while (true) { + if (leg.isLogging()) { + runDemoExperiment0(expCycles,targetP); + } } } -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"}; - -bool sd_card_present = false; -int fname_prepend = 0; - -void PrintStatus() +/** + * Shows how a demo experiment works. This experiment pressurises the leg to + * pressure targetkPa, depressurises it, and then repeats the process cycles + * number of times + * @param cycles: the number of cycles the leg goes up and down + * @param targetkPa: the pressure at which the valve is opened to let the leg go down + */ +void runDemoExperiment0(int cycles, float targetkPa) { - led_g = !led_g; - if (is_printing) { - pc.printf("\r\nLEG STATUS (%s)\r\n",led_g?"+":"*"); - pc.printf("\tSD card detected? %9s\r\n",sd_card_present?"Yes":"No"); - if(sd_card_present) - { - pc.printf("\tFile number %15i\r\n", fname_prepend); - pc.printf("\tLogging? %18s\r\n",is_logging?"Yes":"No"); - } - pc.printf("\n\t%15s %7.2f\r\n","Pressure0 (kPa)", leg.getPressure0()*100); - pc.printf("\t%15s %7.2f\r\n","Pressure1 (kPa)", leg.getPressure1()*100); - - for (int i=0; i<kNumJoints; ++i) - { - string jointName = kJointNames[i]; - jointName = jointName + " (deg)"; - pc.printf("\t%15s %7.2f\r\n",jointName, leg.getDegrees(i)); - } - pc.printf("\t%15s %7.2f\r\n","Force (N)", leg.getForce()); - pc.printf("\t%15s %7.2f\r\n","Torque (Nm)", getTorque(leg.getDegrees(3),leg.getDegrees(2),leg.getForce())); - } -} + //The experiment starts when logging does + bool experimentRunning = leg.isLogging(); + + //Stop the Bench class from printing, so this method can print + leg.pausePrint(); + + if (experimentRunning) { + // Pressurise and depressurise the leg cycles number of times + for (int i=0; i<cycles; i++) { + leg.pc.printf("\r\nCycle: \t%i out of \t%i",i+1,cycles); + + //Update cycles logged + float data[] = {targetkPa,i+1}; + leg.setExtraData(data); + + //Pressurise + leg.setValve(true); -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"); - - pc.printf("\tSD card detected? %9s\r\n",sd_card_present?"Yes":"No"); - if(sd_card_present) - { - pc.printf("\tFile number %15i\r\n", fname_prepend); - pc.printf("\tLogging? %18s\r\n",is_logging?"Yes":"No"); - } -} - -// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = -// IMPLEMENTATION DATA LOGGING -// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = - -FILE * fp_data; - - + //Wait until measured pressure reaches target pressure + float measureP = 0; + while(measureP < targetkPa) { + //Keep checking logging is going + experimentRunning = leg.isLogging(); + + if (experimentRunning) { + measureP = leg.getPressure0()*100;//Conversion of bar to kPa + wait(0.2);//Wait a bit + } else { //Logging stopped + leg.stopLogging(); //Stop logging data + leg.setValve(false); //Depressurise + leg.resumePrint(); //Let the Bench class print + return; + } + } -/** - * 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; + //Depressurise + leg.setValve(false); - // 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; + /*Wait until depressurised (completely depressurised is + around 10-12 kPa due to current sensor calibration)*/ + while(measureP > 15) { + //Keep checking logging is going + experimentRunning = leg.isLogging(); + + if (experimentRunning) { + measureP = leg.getPressure0()*100;//Conversion of bar to kpa + wait(0.2);//Wait a bit + } else { //Logging stopped + leg.stopLogging(); //Stop logging data + leg.resumePrint(); //Let the Bench class print + return; + } } } - 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 (s), theta_toe (deg), theta_ankle (deg), theta_knee (deg), theta_hip (deg), force (N), torque (Nm), pressure0 (kPa), pressure1 (kPa)"); - tick_logging.attach_us(&LogData,timing::kTimeLogDataUs); - 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; + // Logging stopped as experiment is fully completed + leg.stopLogging(); //Stop logging data + leg.resumePrint(); //Let the Bench class print } } -/** - * 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() -{ - 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 - fprintf(fp_data,", %+f, %+f, %+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()), - leg.getPressure0()*100, - leg.getPressure1()*100 - ); -} \ No newline at end of file