File content as of revision 14:b2dd17623a3c:

#include "mbed.h"

#include "dataComm.h"
#include "initExoVars.h"
#include "gaitGenerator.h"
#include <string>
#include <map>

//Much code is shared with BluetoothComm class, on the control bed
dataComm::dataComm():  _len(0), _counter(0), _inMsg(false), _numVars(34), _numReadOnlyParams(12), _escapesNeeded(8)
{
    mbedLED1 = 1;
    int temp1[] = {0,1,2,3,4,8,9,10,-1};

    for (int i = 0; i < _escapesNeeded+1; i++) {
        _escapeNeeded[i] = temp1[i];
    }


    std::string temp2[] = {"KPStance", "KPSwing", "KPStanding", "KPSitting", "KPStandUp", "KPSitdown", "KDStance", "KDSwing",
                           "KDStanding", "KDSitting", "KDStandUp", "KDSitDown", "StandingAngle", "SittingAngle", "BentForwardAngle", "ForwardAngle", "RearAngle",
                           "IMUAngle", "KneeFullRetract", "KneeFullExtend", "LockTime", "Rate", "StandupAsst", "StandupTime", "SitdownAsst", "SitdownTime", "WalkAngle",
                           "StepLength", "StepTime", "HipFlex", "PhaseShift", "MaxAmplitude", "StanceStart", "StanceEnd",
                           "TorsoAng", "LKneeAng", "RKneeAng", "LHipAng", "RHipAng", "LHipTorque", "RHipTorque", "ExoAndKneeStates", "TorsoRefAngle", "LHipRefAngle",
                           "RHipRefAngle", "Charge"
                          };
    //Populate the map of indices to param names
    for (int j = 0; j < (_numVars + _numReadOnlyParams); j += 1) {
        _indexMap[j] = temp2[j];
    }

    for (int j = 0; j < _numVars; j += 1) {
        _paramMap[_indexMap[j]] = generic_get(_indexMap[j]);
    }

    //  pc.printf("Initialized PARAM \r\n");
    int temp4[] = {0x01fe, 0x02ac, 0x02ff, 0x0180, 0x0012, 0x0010, 0x0020, 0x00bf, 0x023f, 0x0123, 0x03a2, 0x10};

    for (int k = 0; k < _numReadOnlyParams; k += 1) {
        _paramMap[_indexMap[_numVars + k]] = temp4[k];
    }
    //pc.printf("Test: %x\r\n", _paramMap["TorsoAng"]);

}

/**
* Returns the stored value of the specified variable
* @param var    The variable to be gotten (as a string).
* @author Michael Ling
* @date 3/31/2015
*/
short dataComm::generic_get(std::string var)
{
    //Translate the raw value of the parameter to a short between 0-100
    if (var.compare("SittingAngle") == 0) {
        return (short)((sittingAngle-MIN_SIT)/(MAX_SIT-MIN_SIT)*100);
    } else if(var.compare( "BentForwardAngle")==0) {
        return (short)((bentAngle-MIN_BENT)/(MAX_BENT-MIN_BENT)*100);
    } else if (var.compare("StandupAsst")==0) {
        return (short)((fsm.get_standup_asst()-MIN_SUASST)/(MAX_SUASST-MIN_SUASST)*100);
    } else if (var.compare("SitdownAsst")==0) {
        return (short)((fsm.get_sitdown_asst()-MIN_SDASST)/(MAX_SDASST-MIN_SDASST)*100);
    } else if (var.compare("SitdownTime")==0) {
        return (short)((tSittingDown-MIN_SDTIME)/(MAX_SDTIME-MIN_SDTIME)*100);
    } else if (var.compare( "StandingAngle")==0) {
        return (short)((stand_adjust-MIN_STAND)/(MAX_STAND-MIN_STAND)*100);
    } else if (var.compare("WalkAngle")==0) {
        return (short)((fsm.get_backbias()-MIN_WALK)/(MAX_WALK-MIN_WALK)*100);
    } else if (var.compare("StepTime")==0) {
        return (short) ((mm_gait_params.time_steps-MIN_STEPTIME)/(MAX_STEPTIME-MIN_STEPTIME)*100);
    } else if (var.compare("PhaseShift") == 0) {
        return (short) ((mm_gait_params.peak_time-MIN_PHASESHIFT)/(MAX_PHASESHIFT-MIN_PHASESHIFT)*100);
    } else if (var.compare("StanceEnd") == 0) {
        return (short) ((mm_gait_params.stance_end-MIN_STANCEEND)/(MAX_STANCEEND-MIN_STANCEEND)*100);
    } else if (var.compare("StanceStart") == 0) {
        return (short) ((mm_gait_params.stance_start-MIN_STANCESTART)/(MAX_STANCESTART-MIN_STANCESTART)*100);
    } else if (var.compare("StepLength") == 0) {
        return (short) ((mm_gait_params.stance_end-MIN_STEPLEN)/(MAX_STEPLEN-MIN_STEPLEN)*100);
    } else if (var.compare("HipFlex") == 0) {
        return (short) ((mm_gait_params.max_angle-MIN_HIPFLEX)/(MAX_HIPFLEX-MIN_HIPFLEX)*100);
    }
    return 0;

}

/**
* Sets a new value for the given variable
* @param var   The variable to be set, as a string
* @param newval   New value for the variable
* @author Michael Ling
* @date 3/31/2015
*/
void dataComm::generic_set(std::string var, short newval)
{
    mbedLED3 = 1;
    //newval is a short from 0-100, and needs to be converted to a raw value.
    //We do this by making a 100-point scale between MIN and MAX param values
    if (var.compare( "SittingAngle")==0) {
        sittingAngle = MIN_SIT + (float)newval/100*(MAX_SIT-MIN_SIT);
        //   pc.printf("%d\r\n", (short)((sittingAngle-70)/40*100));
    } else if (var.compare( "BentForwardAngle")==0) {
        bentAngle = MIN_BENT+(float)newval/100*(MAX_BENT-MIN_BENT);
        // pc.printf("%d\r\n", (short)((bentAngle-90)/50*100));
    } else if (var.compare("StandupAsst")==0) {
        fsm.set_standup_asst(MIN_SUASST+(float)newval/100*(MAX_SUASST-MIN_SUASST));
        //  pc.printf("%d\r\n", (short)fsm.get_standup_asst());
    } else if (var.compare("SitdownAsst")==0) {
        fsm.set_sitdown_asst(MIN_SDASST+(float)newval/100*(MAX_SDASST-MIN_SDASST));
        //  pc.printf("%d\r\n", (short)fsm.get_sitdown_asst());
    } else if (var.compare("SitdownTime")==0) {
        tSittingDown = MIN_SDTIME + (float)newval/100*(MAX_SDTIME-MIN_SDTIME);
        // pc.printf("%d\r\n", (short)tSittingDown);
    } else if (var.compare("StandingAngle")==0) {
        //pc.printf("LED\r\n");
        motorLED = 1;
        stand_adjust = MIN_STAND + float(newval)/100*(MAX_STAND-MIN_STAND);
        encoder_L.init(zero_ang_L+stand_adjust);
        encoder_R.init(zero_ang_R+stand_adjust);
        //pc.printf("%d\r\n", (short)((stand_adjust+15)/30*100));
    } else if (var.compare("WalkAngle")==0) {
        fsm.set_backbias(MIN_WALK+(float)newval/100*(MAX_WALK-MIN_WALK));
        // pc.printf("%d\r\n", (short)fsm.get_backbias());
    } else if (var.compare("StepTime")==0) {
        mm_gait_params.time_steps = MIN_STEPTIME+(float)newval/100*(MAX_STEPTIME-MIN_STEPTIME);
    } else if (var.compare("PhaseShift") == 0) {
        mm_gait_params.peak_time = MIN_PHASESHIFT+(float)newval/100*(MAX_PHASESHIFT-MIN_PHASESHIFT);
    } else if (var.compare("StanceStart") == 0) {
        mm_gait_params.stance_start = MIN_STANCESTART+(float)newval/100*(MAX_STANCESTART-MIN_STANCESTART);
    } else if (var.compare("StanceEnd") == 0) {
        mm_gait_params.stance_end = MIN_STANCEEND+(float)newval/100*(MAX_STANCEEND-MIN_STANCEEND);
    } else if (var.compare("StepLength") == 0) {
        mm_gait_params.stance_end = MIN_STEPLEN+(float)newval/100*(MAX_STEPLEN-MIN_STEPLEN);
    } else if (var.compare("HipFlex") == 0) {
        mm_gait_params.max_angle = MIN_HIPFLEX+(float)newval/100*(MAX_HIPFLEX-MIN_HIPFLEX);
    }
    //MORE else blocks for gait params

}
/**
* Calculates parity--0 if c even, 1 if odd
* @param c    The short that we want to calculate parity of
* @author Michael Ling
* @date 2/4/2015
*/
bool dataComm::parity(short c)
{
    bool p = false;
    while (c != 0) {
        p = !p;
        c = (short) (c & (c-1));
    }
    return p;
}

/**
* Calculates checksum of char array, sum of all chars in array
* @param b    The char array to be summed up
* @param len    Length of the array b
* @author Michael Ling
* @date 2/4/2015
*/
char* dataComm::get_checksum(char* b, int len)
{
    char* checksum = (char*)malloc(3*sizeof(char));
    short sum = 0;
    for (int i = 0; i < len; i += 1) {
        sum += ((short) b[i] & 0xff);
    }
    checksum[1] = (char) ((sum >> 8) & 0xff);
    checksum[2] = (char) (sum & 0xff);
    char check = (char) (sum & 0xff);
    //Sets escape character if the checksum contains START or END
    if (check == START || check == END) {
        checksum[0] = 1;
        checksum[2] = (char) (checksum[2] & 0x1);
    } else {
        checksum[0] = 0;
    }
    return checksum;
}



/**
* Checks the message with length len. Checks for START and END in correct spots, parity, all data and variable byte in proper range, and correct checksum
* @param message    The received message to check
* @param len    Length of the message
* @author Michael Ling
* @date 2/4/2015
*/
bool dataComm::msg_check(char* message, int len)
{
    //Checks the message starts with 0xff
    if (message[0] != START) {
        //printf("Improper START or END \r\n");

        return false;
    }
    //printf("got a start\r\n");
    //Unlike on databed, the message is not guaranteed to be exactly LEN long, hence we're allowed to backtrack looking for END
    //Otherwise, the msg_check procedure is the same
    while (message[len-4] != END && len >= 6) {
        len -= 1;
    }
    if (message[len-4] != END) {
        //send_error(END_ERR);
        return false;
    }
    bool write = message[2] & 0x80;
    for (int i=2; i < len-5; i+=2) {
        if (i == 2 && message[i] == READONLY_IND) {
            break;
        }
        if (i == (len-5) && message[i] == READONLY_IND) {
            break;
        }
        if (((message[i] & 0x80) !=0) && !write) {
            //printf("Does not match READ format \r\n");
            return false;
        }
        if (((message[i] & 0x80) != 0x80) && write) {
            //printf("char %x Does not match WRITE format \r\n", message[i]);
            return false;
        }
        short s;
        if (write) {
            s = (short) ((message[i] << 8) | message[i+1]);
        } else {
            s = (short) (message[i] << 8);
        }
        //Checks the parity bit of the var/data pair.
        bool parity1 = (s & 0x4000) != 0;

        if (parity1 != parity(s & 0x4000)) {
            //printf("Parity error in VAR char \r\n");
            return false;
        }

        char c = message[i] & 0x3f;
        char c2 = message[i+1];
        //Makes sure the var is a valid one
        if ((int) c < 0 || (int) c > _numVars) {
            //printf("VAR char out of range \r\n");
            return false;
        }
        //Makes sure the new value is in the range 0-100
        if ((int) c2 < 0 || (int) c2 > 100) {
            //printf("DATA char out of range");
            return false;
        }
    }
    char* checksum = get_checksum(message, len-3);
    if (checksum[0] != message[len-3]) {
        //printf("checksum error in char 0, expected %x but got %x \r\n", checksum[0], message[len-3]);
        free(checksum);
        return false;
    }
    if (checksum[1] != message[len-2]) {
        //printf("Checksum error in char 1, expected %x but got %x \r\n", checksum[1], message[len-2]);
        free(checksum);
        return false;
    }
    if (checksum[2] != message[len-1]) {
        //printf("Checksum error in char 2, expected %x but got %x \r\n", checksum[2], message[len-1]);
        free(checksum);
        return false;
    }
    free(checksum);

    return true;
}



/**
* Checks received WRITE message and writes to paramMap/SDCard
* @param message    The received WRITE message
* @param len    Length of the WRITE message
* @author Michael Ling
* @date 2/4/2015
*/
void dataComm::process_write(short int* msg, int len)
{

    if (msg[0] == 0) {
        return;
    }
    //dataIn is formatted so that a direct cast to char* will reproduce the original message from the phone.
    char *message = (char*) msg;


    if (!msg_check(message, len*2)) {
        return;
    }
    mbedLED2 = 1;
    //If the msgCheck passes, we change the local paramMap and set variables to their new values
    for (int i=2; i < len*2-5; i+=2) {
        //printf("Loop %d\r\n", i);
        if (message[i] == END) {
            return;
        }
        char msgc = message[i] & 0xbf;
        if ((msgc & 0x80) != 0x80) {
            return;
        }
        int index = msgc & 0x7f;

        _paramMap[_indexMap[index]] = message[i+1];
        generic_set(_indexMap[index], message[i+1]);

    }
}