File content as of revision 2:53547eb587fb:

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

dataComm::dataComm():  _len(0), _counter(0), _inMsg(false), _numVars(27), _numReadOnlyParams(12), _escapesNeeded(8)
{
    
    
    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", 
        "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};
    //readData.write(_numReadOnlyParams, temp4);
    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)
{
    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);
    }
    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;
        pc.printf("%s\r\n", var);
        //newval is a short from 0-100, and needs to be converted a more
        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());
        }
        
}
/**
* 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;
}

/**
* Sends error message.
* @param errCode    character representation of the failure-causing error
* @author Michael Ling
* @data 2/4/2015

void dataComm::send_error(char errCode)
{
    _rn42.putc(START);
    _rn42.putc(errCode);
    _rn42.putc(END);
    _rn42.putc(0);
    _rn42.putc(0);
    _rn42.putc(0);
}
*/







char* dataComm::convert_to_char_array(short int* message, int len) {
    char *retval = new char[len*2];
    for (int i = 2; i < len; i+=2) {
        short int val = message[i];
        retval[(i-1)*2] = (val >> 8) & 0xff;
        retval[(i-1)*2+1] = val & 0xff;
    }
    return retval;
}

/**
* 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");
        //send_error(START_ERR);
        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
    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");
          //  send_error((char)(((message[i] & 0x3f) << 3) | RW_ERR));
            return false;
        }
        if (((message[i] & 0x80) != 0x80) && write) {
            //printf("char %x Does not match WRITE format \r\n", message[i]);
            //send_error((char)(((message[i] & 0x3f) << 3) | RW_ERR));
            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");
            //send_error((char) (((message[i] & 0xbf) << 3) | PARITY_ERR));
            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");
            //send_error((char) (((message[i] & 0xbf) << 3) | VAR_ERR));
            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");
            //send_error((char) (((message[i] & 0xbf) << 3) | DATA_ERR));
            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);
     //   send_error(CHECKSUM_ERR);
        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);
    //    send_error(CHECKSUM_ERR);
        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);
      //  send_error(CHECKSUM_ERR);
        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)
//void dataComm::process_write(char *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;
   /*
   for (int i = 0; i < len*2; i += 2) {
       message[i] = msg[i/2] & 0xff;
       message[i+1] = (msg[i/2] >> 8) & 0xff;
    }
   for (int i = 0; i < len*2; i+=1) {
        //if (message[i] != 0) {
        pc.printf("%x, ",message[i]);
        //}
    }
    pc.printf("\r\n");*/

/*
  char *message = convert_to_char_array(msg, len);
 if (message[0] == 0xff) {

    char message[len/2];
    for (int i = 4; i < len; i += 4) {
        message[(i-4)/2] = msg[i];
        message[(i-4)/2+1] = msg[i+1];
    }*/
/*
    for (int j=0;j<len;j+=1){
        //if (message[j] != 0) {
        printf("%d: %x\r\n", j, message[j]);
        //}
    }
    //}
    mbedLED1 = 1;
/*
    if (message[2] == END) {
        _failures += 1;
        if (_failures < 5) {
           send(_lastCmd);
        } else {
            _failures = 0;
        }
        printf("Exited\r\n");

        return;
    }
   
*/

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