Diff: LegInterface.cpp

Revision:

0:346370254254

Child:

1:d3abc0577ebc

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LegInterface.cpp	Wed May 21 21:21:22 2014 +0000
@@ -0,0 +1,292 @@
+
+/*
+    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()
+{
+    // Has the prior attempt at sending the estop completed?
+    if(!transactionQueryState[LEG_W]->completed() ||
+            !transactionQueryState[LEG_E]->completed() ||
+            !transactionQueryState[LEG_S]->completed()) {
+        // 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::queryStateCompleted(int timeout)
+{
+    for(int i = 0; i < NUM_LEGS; ++i) {
+        if(!transactionQueryState[i]->waitForCompletion(timeout)) {
+            return false;
+        }
+    }
+    return true;
+}
+
+bool LegInterface::queryStateCopy(LegState_t legState[NUM_LEGS])
+{
+    // Has the prior attempt at sending the estop completed?
+    if(!transactionQueryState[LEG_W]->completed() ||
+            !transactionQueryState[LEG_E]->completed() ||
+            !transactionQueryState[LEG_S]->completed()) {
+        // 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;
+}
+
+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;
+}
+
+
+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");
+
+}
\ No newline at end of file