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())
           );
}