File content as of revision 7:c164b4869f74:

/*
    Interface for communication to the set of legs for the OU SIPPC Robot (version 3B and beyond).

    Note: this interface is talking to very custom hardware.  As a result, it serves more as an
    example of how to use the I2CTransaction library.

    Author: Andrew H. Fagg  (May, 2014)

    More documentation to come...

*/


#include "LegInterface.h"

const int LegInterface::LegAddress[] = {I2C_ADDR_WEST, I2C_ADDR_EAST, I2C_ADDR_SOUTH};

/**
    Constructor: create all of the packets that can be sent + the associated transactions
*/

LegInterface::LegInterface(Serial *pc)
{
    // ESTOP transactions
    transactionEstop[LEG_W] = new I2CTransaction(I2C_ADDR_WEST, (char*) &legPacketEstop, 4);
    transactionEstop[LEG_E] = new I2CTransaction(I2C_ADDR_EAST, (char*) &legPacketEstop, 4);
    transactionEstop[LEG_S] = new I2CTransaction(I2C_ADDR_SOUTH, (char*) &legPacketEstop, 4);

    // Query state transactions
    legPacketQuery.type = nsPacketsLeg::QUERY_STATE;
    transactionQueryState[LEG_W] = new I2CTransaction(I2C_ADDR_WEST, (char*) &legPacketQuery, 4,
            (char*) &(legState[LEG_W]), sizeof(LegState_t));
    transactionQueryState[LEG_E] = new I2CTransaction(I2C_ADDR_EAST, (char*) &legPacketQuery, 4,
            (char*) &(legState[LEG_E]), sizeof(LegState_t));
    transactionQueryState[LEG_S] = new I2CTransaction(I2C_ADDR_SOUTH, (char*) &legPacketQuery, 4,
            (char*) &(legState[LEG_S]), sizeof(LegState_t));

    // Query leg parameter transactions.  Note: shared outgoing packet structure
    legPacketQueryLiftParams.type = nsPacketsLeg::GET_LIFT_PARAMS;
    transactionQueryLiftParams[LEG_W] = new I2CTransaction(I2C_ADDR_WEST, (char*) &legPacketQueryLiftParams, 4,
            (char *) &legLiftParams, sizeof(LegControlParams_t));
    transactionQueryLiftParams[LEG_E] = new I2CTransaction(I2C_ADDR_EAST, (char*) &legPacketQueryLiftParams, 4,
            (char *) &legLiftParams, sizeof(LegControlParams_t));
    transactionQueryLiftParams[LEG_S] = new I2CTransaction(I2C_ADDR_SOUTH, (char*) &legPacketQueryLiftParams, 4,
            (char *) &legLiftParams, sizeof(LegControlParams_t));

    legPacketQueryWheelParams.type = nsPacketsLeg::GET_WHEEL_PARAMS;
    transactionQueryWheelParams[LEG_W] = new I2CTransaction(I2C_ADDR_WEST, (char*) &legPacketQueryWheelParams, 4,
            (char *) &legWheelParams, sizeof(LegControlParams_t));
    transactionQueryWheelParams[LEG_E] = new I2CTransaction(I2C_ADDR_EAST, (char*) &legPacketQueryWheelParams, 4,
            (char *) &legWheelParams, sizeof(LegControlParams_t));
    transactionQueryWheelParams[LEG_S] = new I2CTransaction(I2C_ADDR_SOUTH, (char*) &legPacketQueryWheelParams, 4,
            (char *) &legWheelParams, sizeof(LegControlParams_t));


    // Set leg parameter transactions
    //  Note: shared packet structure across transactions (since we are setting one at a time)
    legPacketSetLiftParams.type = nsPacketsLeg::SET_LIFT_PARAMS;
    transactionSetLiftParams[LEG_W] = new I2CTransaction(I2C_ADDR_WEST, (char*) &legPacketSetLiftParams, 4+sizeof(LegControlParams_t));
    transactionSetLiftParams[LEG_E] = new I2CTransaction(I2C_ADDR_EAST, (char*) &legPacketSetLiftParams, 4+sizeof(LegControlParams_t));
    transactionSetLiftParams[LEG_S] = new I2CTransaction(I2C_ADDR_SOUTH, (char*) &legPacketSetLiftParams, 4+sizeof(LegControlParams_t));

    legPacketSetWheelParams.type = nsPacketsLeg::SET_WHEEL_PARAMS;
    transactionSetWheelParams[LEG_W] = new I2CTransaction(I2C_ADDR_WEST, (char*) &legPacketSetWheelParams, 4+sizeof(LegControlParams_t));
    transactionSetWheelParams[LEG_E] = new I2CTransaction(I2C_ADDR_EAST, (char*) &legPacketSetWheelParams, 4+sizeof(LegControlParams_t));
    transactionSetWheelParams[LEG_S] = new I2CTransaction(I2C_ADDR_SOUTH, (char*) &legPacketSetWheelParams, 4+sizeof(LegControlParams_t));

    // Goal set
    legPacketSetGoal[LEG_W].type = nsPacketsLeg::SET_GOAL;
    transactionSetGoal[LEG_W] = new I2CTransaction(I2C_ADDR_WEST, (char*) &(legPacketSetGoal[LEG_W]), 4+sizeof(Goal_t));

    legPacketSetGoal[LEG_E].type = nsPacketsLeg::SET_GOAL;
    transactionSetGoal[LEG_E] = new I2CTransaction(I2C_ADDR_EAST, (char*) &(legPacketSetGoal[LEG_E]), 4+sizeof(Goal_t));

    legPacketSetGoal[LEG_S].type = nsPacketsLeg::SET_GOAL;
    transactionSetGoal[LEG_S] = new I2CTransaction(I2C_ADDR_SOUTH, (char*) &(legPacketSetGoal[LEG_S]), 4+sizeof(Goal_t));

    // Other initialziations
    this->pc = pc;
    Leg leg = LEG_S;
    pc->printf("codes: %d %d\n\r", transactionSetLiftParams[leg]->getStatus(0), transactionSetLiftParams[leg]->getStatus(1));

}

/**
    Attempt to send an estop to all legs.

    @param estop true = stop all motion; false = enable motion

    @return true = transaction successfully scheduled, false = prior attempt at sending had not completed.
*/

bool LegInterface::sendEstop(bool estop)
{
    // Has the prior attempt at sending the estop completed?
    if(!transactionEstop[LEG_W]->completed() ||
            !transactionEstop[LEG_E]->completed() ||
            !transactionEstop[LEG_S]->completed()) {
        // No: do not attempt
        return false;
    }

    // Yes - configure the packet (all packets are the same in this case)
    legPacketEstop.type = estop?nsPacketsLeg::ESTOP_ON:nsPacketsLeg::ESTOP_OFF;

    // Schedule the transactions
    transactionEstop[LEG_W]->initiateTransaction();
    transactionEstop[LEG_E]->initiateTransaction();
    transactionEstop[LEG_S]->initiateTransaction();

    // Complete
    return true;
}

bool LegInterface::queryStateInitiate()
{
    /*
    transactionQueryState[LEG_W]->checkTransaction();
    transactionQueryState[LEG_E]->checkTransaction();
    transactionQueryState[LEG_S]->checkTransaction();
    */

    // Has the prior attempt at sending the estop completed?
    if(!queryStateCompleted()) {
        // No: do not attempt
        return false;
    }

    //this->pc->printf("########################## FOO @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@\n\r");
    // Magic numbers
    for(int i= 0; i < NUM_LEGS; ++i) {
        this->legState[i].magic = 0;
    }

    // Query last had completed
    // Schedule the transactions
    transactionQueryState[LEG_W]->initiateTransaction();
    transactionQueryState[LEG_E]->initiateTransaction();
    transactionQueryState[LEG_S]->initiateTransaction();

    return true;
}

bool LegInterface::queryStateWaitForCompletion(int timeout)
{
    for(int i = 0; i < NUM_LEGS; ++i) {
        if(!transactionQueryState[i]->waitForCompletion(timeout)) {
            return false;
        }
    }
    return true;
}

/**
    Check if the query of leg state has been completed (successful or not)

    @return true if completed
*/

bool LegInterface::queryStateCompleted()
{
    for(int i = 0; i < NUM_LEGS; ++i) {
        if(!transactionQueryState[i]->completed()) {
            return false;
        }
    }
    return true;
}

/**
    Check if the query of leg state has been successfully completed

    @return true if successfully completed
*/

bool LegInterface::queryStateSuccess()
{
    for(int i = 0; i < NUM_LEGS; ++i) {
        if(!transactionQueryState[i]->success()) {
            return false;
        }
    }
    return true;
}

/**
    Print out the internal magic numbers
    */

void LegInterface::queryStateReportMagic()
{
    for(int i = 0; i < NUM_LEGS; ++i) {
        pc->printf("%x ", legState[i].magic);
    }
    pc->printf("\n\r");
}
/**
    Check if the query of estop has been completed (successful or not)

    @return true if completed
*/

bool LegInterface::queryEstopCompleted()
{
    for(int i = 0; i < NUM_LEGS; ++i) {
        if(!transactionEstop[i]->completed()) {
            return false;
        }
    }
    return true;
}

/**
    Check if the query of estop has been successfully completed

    @return true if successfully completed
*/

bool LegInterface::queryEstopSuccess()
{
    for(int i = 0; i < NUM_LEGS; ++i) {
        if(!transactionEstop[i]->success()) {
            return false;
        }
    }
    return true;
}

void LegInterface::queryStateTest(Serial *pc)
{
    for(int i = 0; i < NUM_LEGS; ++i) {
        pc->printf("Status %d: %d %d\n\r", i, transactionQueryState[i]->getStatus(0), transactionQueryState[i]->getStatus(1));
    }
}


bool LegInterface::queryStateCopy(LegState_t legState[NUM_LEGS])
{
    // Has the prior attempt at querying the state completed?
    if(!queryStateCompleted()) {
        // No: do not attempt
        return false;
    }

    // Transaction complete: copy the state of the legs only if the magic number is correct
    if(this->legState[LEG_W].magic == I2C_MAGIC_NUMBER)
        legState[LEG_W] = this->legState[LEG_W];
    if(this->legState[LEG_E].magic == I2C_MAGIC_NUMBER)
        legState[LEG_E] = this->legState[LEG_E];
    if(this->legState[LEG_S].magic == I2C_MAGIC_NUMBER)
        legState[LEG_S] = this->legState[LEG_S];

    return true;
}

/**
    Copy the state of the legs from the LegInterface structure to usable structures.  In order for the data to be copied, the
    transaction must be successful, and the leg packet must have a valid magic number (the latter is evalulated on a leg-by-leg
    basis).

    The input parameters are destructively modified only if the data are valid.  The exception is the "magic" parameter.  This is
    only changed if the set of transactions was successful.

    @param liftPosition  Lift position of each of the legs (m above the ground)
    @param liftVelocity  Velocity of the lifts (m/s)
    @param wheelPosition  Position of each of the wheels (m)
    @param wheelVelocity Velocity of the wheels (m/s)
    @param cliff Cliff sensors
    @param limit Limit sensors (lifts)
    @param estop Estop state of each of the legs
    @param magic Magic number from each leg

    @return true if Some data were updated; false if no change

*/

bool LegInterface::queryStateCopy(float liftPosition[NUM_LEGS], float liftVelocity[NUM_LEGS], float wheelPosition[NUM_LEGS], float wheelVelocity[NUM_LEGS],
                                  bool cliff[NUM_LEGS], bool limit[NUM_LEGS], bool estop[NUM_LEGS], uint8_t magic[NUM_LEGS])
{
    // Has the prior attempt at getting state completed?
    if(!queryStateSuccess()) {
        // No: do not attempt the copy
        return false;
    }

    // Transaction complete: copy the state of the legs
    for(int i = 0; i < NUM_LEGS; ++i) {
        if(this->legState[i].magic == I2C_MAGIC_NUMBER) {
            // Only copy if the magic number matches
            liftPosition[i] = this->legState[i].lift.pos / LIFT_TICKS_PER_METER - LIFT_OFFSET;
            liftVelocity[i] = this->legState[i].lift.vel / LIFT_TICKS_PER_METER;
            wheelPosition[i] = this->legState[i].wheel.pos / TICKS_PER_METER;
            wheelVelocity[i] = this->legState[i].wheel.vel / TICKS_PER_METER;
            cliff[i] = this->legState[i].cliff;
            limit[i] = this->legState[i].limit;
            estop[i] = this->legState[i].estop;

        }
        // Also copy the magic number
        magic[i] = this->legState[i].magic;
        //pc->printf("%x ", magic[i]);
    }
    //pc->printf("\n\r");

    return true;
}

void LegInterface::displayLegState(LegState_t *legState)
{
    pc->printf("Cliff = %d, Limit = %d\n\r", legState->cliff, legState->limit);
    pc->printf("Wheel: pos=%d, vel=%d\n\r", legState->wheel.pos, legState->wheel.vel);
    pc->printf("Lift: pos=%d, vel=%d\n\r", legState->lift.pos, legState->lift.vel);
}


bool LegInterface::queryLegParams(Leg leg, LegControlParams_t *liftParams, LegControlParams_t *wheelParams)
{
    /*
    if(pc != NULL) {
        pc->printf("completed: %d %d %d %d\n\r", transactionQueryLiftParams[leg]->completed(), transactionQueryWheelParams[leg]->completed(),
                   transactionSetLiftParams[leg]->completed(), transactionSetWheelParams[leg]->completed());
        pc->printf("codes: %d %d\n\r", transactionSetLiftParams[leg]->getStatus(0), transactionSetLiftParams[leg]->getStatus(1));
    }
    */

    // Has the prior attempt at querying/setting leg parameters completed?
    if(!transactionQueryLiftParams[leg]->completed() || !transactionQueryWheelParams[leg]->completed()
            || !transactionSetLiftParams[leg]->completed() || !transactionSetWheelParams[leg]->completed()) {
        // No:  do not attempt
        return false;
    }

    // Yes: initiate the transactions
    transactionQueryLiftParams[leg]->initiateTransaction();
    transactionQueryWheelParams[leg]->initiateTransaction();
    //pc->printf("codes: %d %d\n\r", transactionQueryWheelParams[leg]->getStatus(0), transactionQueryWheelParams[leg]->getStatus(1));

    if(transactionQueryLiftParams[leg]->waitForCompletion() && transactionQueryWheelParams[leg]->waitForCompletion()) {
        // Completed
        *liftParams = this->legLiftParams;
        *wheelParams = this->legWheelParams;
        return true;
    }
    // A timeout happened (no copy)
    return false;
}

void LegInterface::displayParams(LegControlParams_t *params)
{
    pc->printf("Kp = %d\t Kv = %d\t Ki=%d\n\r", params->Kp, params->Kv, params->Ki);
    pc->printf("deadband = %d\n\r", params->deadband);
    pc->printf("Control signal range = [%d, %d]\n\r", params->min_val, params->max_val);
    pc->printf("Max error = %d, max error accum = %d\n\r", params->max_error, params->max_error_accum);
    pc->printf("Max acceleration = %d\n\r", params->max_accel);
}

bool LegInterface::setLegParams(Leg leg, LegControlParams_t *liftParams, LegControlParams_t *wheelParams)
{


    // Has the prior attempt at querying/setting leg parameters completed?
    if(!transactionQueryLiftParams[leg]->completed() || !transactionQueryWheelParams[leg]->completed()
            || !transactionSetLiftParams[leg]->completed() || !transactionSetWheelParams[leg]->completed()) {
        // No:  do not attempt
        return false;
    }

    // Copy provided parameters into structure to send
    this->legLiftParams = *liftParams;
    this->legWheelParams = *wheelParams;

    // Yes: initiate the transactions
    transactionSetLiftParams[leg]->initiateTransaction();
    transactionSetWheelParams[leg]->initiateTransaction();

    if(transactionQueryLiftParams[leg]->waitForCompletion() && transactionQueryWheelParams[leg]->waitForCompletion()) {
        // Completed
        return true;
    }
    // A timeout happened (no copy)
    return false;
}

bool LegInterface::setLegGoal(int32_t liftPos[NUM_LEGS], int32_t wheelVel[NUM_LEGS])
{
    // Has the previous attempt completed?
    if(!transactionSetGoal[LEG_W]->completed() ||
            !transactionSetGoal[LEG_E]->completed() ||
            !transactionSetGoal[LEG_S]->completed()) {
        // No: refuse to send
        return false;
    }

    // Copy goals into structures to send and initiate
    for(int i = 0; i < LegInterface::NUM_LEGS; ++i) {
        legPacketSetGoal[i].contents.as_goal.liftPos = liftPos[i];
        legPacketSetGoal[i].contents.as_goal.wheelVel = wheelVel[i];
        transactionSetGoal[i]->initiateTransaction();
    }

    // Indicate success
    return true;
}



bool LegInterface::setLegGoal(float liftPos[NUM_LEGS], float wheelVel[NUM_LEGS])
{
    int32_t lift[NUM_LEGS];
    int32_t wheel[NUM_LEGS];

    for(int i = 0; i < NUM_LEGS; ++i) {
        wheel[i] = (int32_t) (wheelVel[i] * TICKS_PER_METER);
        lift[i] = (int32_t) ((liftPos[i] + LIFT_OFFSET) * LIFT_TICKS_PER_METER);
    }

    return setLegGoal(lift, wheel);
}

/**
    Check the completion status of the Set Leg Goal transaction.

    @return true if the last leg goal set was completed.
*/

bool LegInterface::getLegGoalCompleted()
{
    return(transactionSetGoal[LEG_W]->completed() &&
           transactionSetGoal[LEG_E]->completed() &&
           transactionSetGoal[LEG_S]->completed());
}


/**
    Check the completion status of the Set Leg Goal transaction.

    @return true if the last leg goal set was completed successfully.
*/

bool LegInterface::getLegGoalSuccess()
{
    return(transactionSetGoal[LEG_W]->success() &&
           transactionSetGoal[LEG_E]->success() &&
           transactionSetGoal[LEG_S]->success());
}

void LegInterface::displayStatus()
{
    pc->printf("Transaction status:\n\r");
    transactionEstop[LEG_W]->displayStatus(pc, "Estop W:\t");
    transactionEstop[LEG_E]->displayStatus(pc, "Estop E:\t");
    transactionEstop[LEG_S]->displayStatus(pc, "Estop S:\t");
    pc->printf("\n\r");
    transactionQueryState[LEG_W]->displayStatus(pc, "Query State W:\t");
    transactionQueryState[LEG_E]->displayStatus(pc, "Query State E:\t");
    transactionQueryState[LEG_S]->displayStatus(pc, "Query State S:\t");
    pc->printf("\n\r");
    transactionQueryLiftParams[LEG_W]->displayStatus(pc, "Query Lift Params W");
    transactionQueryLiftParams[LEG_E]->displayStatus(pc, "Query Lift Params E");
    transactionQueryLiftParams[LEG_S]->displayStatus(pc, "Query Lift Params S");
    pc->printf("\n\r");
    transactionQueryWheelParams[LEG_W]->displayStatus(pc, "Query Wheel Params W");
    transactionQueryWheelParams[LEG_E]->displayStatus(pc, "Query Wheel Params E");
    transactionQueryWheelParams[LEG_S]->displayStatus(pc, "Query Wheel Params S");
    pc->printf("\n\r");
    transactionSetLiftParams[LEG_W]->displayStatus(pc, "Set Lift Params W");
    transactionSetLiftParams[LEG_E]->displayStatus(pc, "Set Lift Params E");
    transactionSetLiftParams[LEG_S]->displayStatus(pc, "Set Lift Params S");
    pc->printf("\n\r");
    transactionSetWheelParams[LEG_W]->displayStatus(pc, "Set Wheel Params W");
    transactionSetWheelParams[LEG_E]->displayStatus(pc, "Set Wheel Params E");
    transactionSetWheelParams[LEG_S]->displayStatus(pc, "Set Wheel Params S");
    pc->printf("\n\r");
    transactionSetGoal[LEG_W]->displayStatus(pc, "Set Goal W:\t");
    transactionSetGoal[LEG_E]->displayStatus(pc, "Set Goal E:\t");
    transactionSetGoal[LEG_S]->displayStatus(pc, "Set Goal S:\t");
    pc->printf("\n\r");

}

void LegInterface::reset()
{
    I2CTransaction::reset();
}

void LegInterface::resetBus()
{
    I2CTransaction::resetBus();
}

void LegInterface::cycleBus()
{
    I2CTransaction::cycleBus();
}

void LegInterface::clearQueryTransactions()
{
    transactionQueryState[LEG_W]->clearStatus();
    transactionQueryState[LEG_E]->clearStatus();
    transactionQueryState[LEG_S]->clearStatus();
}

void LegInterface::clearSetGoalTransactions()
{
    transactionSetGoal[LEG_W]->clearStatus();
    transactionSetGoal[LEG_E]->clearStatus();
    transactionSetGoal[LEG_S]->clearStatus();
}


/**
    Initialize the I2C pins and other hardware.  If it has already been initialized, then
    we will re-initialize.

    @param sda Name of the pin that is used for data
    @param scl Name of the pin that is used for the clock
*/

void LegInterface::initI2C(PinName sda, PinName scl)
{
    I2CTransaction::initI2C(sda, scl);
}