File content as of revision 4:5f7d38d0e22d:

/*
    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;
    }

    // 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;
}


bool LegInterface::queryStateCompleted()
{
    for(int i = 0; i < NUM_LEGS; ++i) {
        if(!transactionQueryState[i]->completed()) {
            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 sending the estop completed?
    if(!queryStateCompleted()) {
        // No: do not attempt
        return false;
    }

    // Transaction complete: copy the state of the legs
    legState[LEG_W] = this->legState[LEG_W];
    legState[LEG_E] = this->legState[LEG_E];
    legState[LEG_S] = this->legState[LEG_S];

    return true;
}

bool LegInterface::queryStateCopy(float liftPosition[NUM_LEGS], float liftVelocity[NUM_LEGS], float wheelPosition[NUM_LEGS], float wheelVelocity[NUM_LEGS])
{
    // Has the prior attempt at sending the estop completed?
    if(!queryStateCompleted()) {
        // No: do not attempt
        return false;
    }

    // Transaction complete: copy the state of the legs
    for(int i = 0; i < NUM_LEGS; ++i) {
        liftPosition[i] = this->legState[i].lift.pos / LIFT_TICKS_PER_METER;
        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;
    }

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

    return setLegGoal(lift, wheel);
}

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