Andrew Fagg / SIPPCLegInterface

Example use of I2CTransaction class. In this example, the master mbed is talking to 3 legs of the SIPPC3B robot.

LegInterface.cpp@6:a52ded837184, 2014-08-05 (annotated)

Committer:
symbiotic
Date:
Tue Aug 05 21:53:59 2014 +0000
Revision:
6:a52ded837184
Parent:
5:86f1cd6657de
Child:
7:c164b4869f74
added new state query functions

Who changed what in which revision?

UserRevisionLine numberNew contents of line
symbiotic 0:346370254254 1
symbiotic 0:346370254254 2 /*
symbiotic 0:346370254254 3 Interface for communication to the set of legs for the OU SIPPC Robot (version 3B and beyond).
symbiotic 1:d3abc0577ebc 4
symbiotic 0:346370254254 5 Note: this interface is talking to very custom hardware. As a result, it serves more as an
symbiotic 0:346370254254 6 example of how to use the I2CTransaction library.
symbiotic 0:346370254254 7
symbiotic 0:346370254254 8 Author: Andrew H. Fagg (May, 2014)
symbiotic 0:346370254254 9
symbiotic 0:346370254254 10 More documentation to come...
symbiotic 1:d3abc0577ebc 11
symbiotic 0:346370254254 12 */
symbiotic 0:346370254254 13
symbiotic 0:346370254254 14
symbiotic 0:346370254254 15 #include "LegInterface.h"
symbiotic 0:346370254254 16
symbiotic 0:346370254254 17 const int LegInterface::LegAddress[] = {I2C_ADDR_WEST, I2C_ADDR_EAST, I2C_ADDR_SOUTH};
symbiotic 0:346370254254 18
symbiotic 0:346370254254 19 /**
symbiotic 0:346370254254 20 Constructor: create all of the packets that can be sent + the associated transactions
symbiotic 0:346370254254 21 */
symbiotic 0:346370254254 22
symbiotic 0:346370254254 23 LegInterface::LegInterface(Serial *pc)
symbiotic 0:346370254254 24 {
symbiotic 0:346370254254 25 // ESTOP transactions
symbiotic 0:346370254254 26 transactionEstop[LEG_W] = new I2CTransaction(I2C_ADDR_WEST, (char*) &legPacketEstop, 4);
symbiotic 0:346370254254 27 transactionEstop[LEG_E] = new I2CTransaction(I2C_ADDR_EAST, (char*) &legPacketEstop, 4);
symbiotic 0:346370254254 28 transactionEstop[LEG_S] = new I2CTransaction(I2C_ADDR_SOUTH, (char*) &legPacketEstop, 4);
symbiotic 0:346370254254 29
symbiotic 0:346370254254 30 // Query state transactions
symbiotic 0:346370254254 31 legPacketQuery.type = nsPacketsLeg::QUERY_STATE;
symbiotic 0:346370254254 32 transactionQueryState[LEG_W] = new I2CTransaction(I2C_ADDR_WEST, (char*) &legPacketQuery, 4,
symbiotic 0:346370254254 33 (char*) &(legState[LEG_W]), sizeof(LegState_t));
symbiotic 0:346370254254 34 transactionQueryState[LEG_E] = new I2CTransaction(I2C_ADDR_EAST, (char*) &legPacketQuery, 4,
symbiotic 0:346370254254 35 (char*) &(legState[LEG_E]), sizeof(LegState_t));
symbiotic 0:346370254254 36 transactionQueryState[LEG_S] = new I2CTransaction(I2C_ADDR_SOUTH, (char*) &legPacketQuery, 4,
symbiotic 0:346370254254 37 (char*) &(legState[LEG_S]), sizeof(LegState_t));
symbiotic 0:346370254254 38
symbiotic 0:346370254254 39 // Query leg parameter transactions. Note: shared outgoing packet structure
symbiotic 0:346370254254 40 legPacketQueryLiftParams.type = nsPacketsLeg::GET_LIFT_PARAMS;
symbiotic 0:346370254254 41 transactionQueryLiftParams[LEG_W] = new I2CTransaction(I2C_ADDR_WEST, (char*) &legPacketQueryLiftParams, 4,
symbiotic 0:346370254254 42 (char *) &legLiftParams, sizeof(LegControlParams_t));
symbiotic 0:346370254254 43 transactionQueryLiftParams[LEG_E] = new I2CTransaction(I2C_ADDR_EAST, (char*) &legPacketQueryLiftParams, 4,
symbiotic 0:346370254254 44 (char *) &legLiftParams, sizeof(LegControlParams_t));
symbiotic 0:346370254254 45 transactionQueryLiftParams[LEG_S] = new I2CTransaction(I2C_ADDR_SOUTH, (char*) &legPacketQueryLiftParams, 4,
symbiotic 0:346370254254 46 (char *) &legLiftParams, sizeof(LegControlParams_t));
symbiotic 0:346370254254 47
symbiotic 0:346370254254 48 legPacketQueryWheelParams.type = nsPacketsLeg::GET_WHEEL_PARAMS;
symbiotic 0:346370254254 49 transactionQueryWheelParams[LEG_W] = new I2CTransaction(I2C_ADDR_WEST, (char*) &legPacketQueryWheelParams, 4,
symbiotic 0:346370254254 50 (char *) &legWheelParams, sizeof(LegControlParams_t));
symbiotic 0:346370254254 51 transactionQueryWheelParams[LEG_E] = new I2CTransaction(I2C_ADDR_EAST, (char*) &legPacketQueryWheelParams, 4,
symbiotic 0:346370254254 52 (char *) &legWheelParams, sizeof(LegControlParams_t));
symbiotic 0:346370254254 53 transactionQueryWheelParams[LEG_S] = new I2CTransaction(I2C_ADDR_SOUTH, (char*) &legPacketQueryWheelParams, 4,
symbiotic 0:346370254254 54 (char *) &legWheelParams, sizeof(LegControlParams_t));
symbiotic 0:346370254254 55
symbiotic 0:346370254254 56
symbiotic 0:346370254254 57 // Set leg parameter transactions
symbiotic 0:346370254254 58 // Note: shared packet structure across transactions (since we are setting one at a time)
symbiotic 0:346370254254 59 legPacketSetLiftParams.type = nsPacketsLeg::SET_LIFT_PARAMS;
symbiotic 0:346370254254 60 transactionSetLiftParams[LEG_W] = new I2CTransaction(I2C_ADDR_WEST, (char*) &legPacketSetLiftParams, 4+sizeof(LegControlParams_t));
symbiotic 0:346370254254 61 transactionSetLiftParams[LEG_E] = new I2CTransaction(I2C_ADDR_EAST, (char*) &legPacketSetLiftParams, 4+sizeof(LegControlParams_t));
symbiotic 0:346370254254 62 transactionSetLiftParams[LEG_S] = new I2CTransaction(I2C_ADDR_SOUTH, (char*) &legPacketSetLiftParams, 4+sizeof(LegControlParams_t));
symbiotic 0:346370254254 63
symbiotic 0:346370254254 64 legPacketSetWheelParams.type = nsPacketsLeg::SET_WHEEL_PARAMS;
symbiotic 0:346370254254 65 transactionSetWheelParams[LEG_W] = new I2CTransaction(I2C_ADDR_WEST, (char*) &legPacketSetWheelParams, 4+sizeof(LegControlParams_t));
symbiotic 0:346370254254 66 transactionSetWheelParams[LEG_E] = new I2CTransaction(I2C_ADDR_EAST, (char*) &legPacketSetWheelParams, 4+sizeof(LegControlParams_t));
symbiotic 0:346370254254 67 transactionSetWheelParams[LEG_S] = new I2CTransaction(I2C_ADDR_SOUTH, (char*) &legPacketSetWheelParams, 4+sizeof(LegControlParams_t));
symbiotic 0:346370254254 68
symbiotic 0:346370254254 69 // Goal set
symbiotic 0:346370254254 70 legPacketSetGoal[LEG_W].type = nsPacketsLeg::SET_GOAL;
symbiotic 0:346370254254 71 transactionSetGoal[LEG_W] = new I2CTransaction(I2C_ADDR_WEST, (char*) &(legPacketSetGoal[LEG_W]), 4+sizeof(Goal_t));
symbiotic 0:346370254254 72
symbiotic 0:346370254254 73 legPacketSetGoal[LEG_E].type = nsPacketsLeg::SET_GOAL;
symbiotic 0:346370254254 74 transactionSetGoal[LEG_E] = new I2CTransaction(I2C_ADDR_EAST, (char*) &(legPacketSetGoal[LEG_E]), 4+sizeof(Goal_t));
symbiotic 0:346370254254 75
symbiotic 0:346370254254 76 legPacketSetGoal[LEG_S].type = nsPacketsLeg::SET_GOAL;
symbiotic 0:346370254254 77 transactionSetGoal[LEG_S] = new I2CTransaction(I2C_ADDR_SOUTH, (char*) &(legPacketSetGoal[LEG_S]), 4+sizeof(Goal_t));
symbiotic 0:346370254254 78
symbiotic 0:346370254254 79 // Other initialziations
symbiotic 0:346370254254 80 this->pc = pc;
symbiotic 0:346370254254 81 Leg leg = LEG_S;
symbiotic 0:346370254254 82 pc->printf("codes: %d %d\n\r", transactionSetLiftParams[leg]->getStatus(0), transactionSetLiftParams[leg]->getStatus(1));
symbiotic 0:346370254254 83 }
symbiotic 0:346370254254 84
symbiotic 0:346370254254 85 /**
symbiotic 0:346370254254 86 Attempt to send an estop to all legs.
symbiotic 0:346370254254 87
symbiotic 0:346370254254 88 @param estop true = stop all motion; false = enable motion
symbiotic 0:346370254254 89
symbiotic 0:346370254254 90 @return true = transaction successfully scheduled, false = prior attempt at sending had not completed.
symbiotic 0:346370254254 91 */
symbiotic 0:346370254254 92
symbiotic 0:346370254254 93 bool LegInterface::sendEstop(bool estop)
symbiotic 0:346370254254 94 {
symbiotic 0:346370254254 95 // Has the prior attempt at sending the estop completed?
symbiotic 0:346370254254 96 if(!transactionEstop[LEG_W]->completed() ||
symbiotic 0:346370254254 97 !transactionEstop[LEG_E]->completed() ||
symbiotic 0:346370254254 98 !transactionEstop[LEG_S]->completed()) {
symbiotic 0:346370254254 99 // No: do not attempt
symbiotic 0:346370254254 100 return false;
symbiotic 0:346370254254 101 }
symbiotic 0:346370254254 102
symbiotic 0:346370254254 103 // Yes - configure the packet (all packets are the same in this case)
symbiotic 0:346370254254 104 legPacketEstop.type = estop?nsPacketsLeg::ESTOP_ON:nsPacketsLeg::ESTOP_OFF;
symbiotic 0:346370254254 105
symbiotic 0:346370254254 106 // Schedule the transactions
symbiotic 0:346370254254 107 transactionEstop[LEG_W]->initiateTransaction();
symbiotic 0:346370254254 108 transactionEstop[LEG_E]->initiateTransaction();
symbiotic 0:346370254254 109 transactionEstop[LEG_S]->initiateTransaction();
symbiotic 0:346370254254 110
symbiotic 0:346370254254 111 // Complete
symbiotic 0:346370254254 112 return true;
symbiotic 0:346370254254 113 }
symbiotic 0:346370254254 114
symbiotic 0:346370254254 115 bool LegInterface::queryStateInitiate()
symbiotic 0:346370254254 116 {
symbiotic 3:8e7471af3453 117 /*
symbiotic 3:8e7471af3453 118 transactionQueryState[LEG_W]->checkTransaction();
symbiotic 3:8e7471af3453 119 transactionQueryState[LEG_E]->checkTransaction();
symbiotic 3:8e7471af3453 120 transactionQueryState[LEG_S]->checkTransaction();
symbiotic 3:8e7471af3453 121 */
symbiotic 4:5f7d38d0e22d 122
symbiotic 0:346370254254 123 // Has the prior attempt at sending the estop completed?
symbiotic 3:8e7471af3453 124 if(!queryStateCompleted()) {
symbiotic 0:346370254254 125 // No: do not attempt
symbiotic 0:346370254254 126 return false;
symbiotic 0:346370254254 127 }
symbiotic 0:346370254254 128
symbiotic 0:346370254254 129 // Query last had completed
symbiotic 0:346370254254 130 // Schedule the transactions
symbiotic 0:346370254254 131 transactionQueryState[LEG_W]->initiateTransaction();
symbiotic 0:346370254254 132 transactionQueryState[LEG_E]->initiateTransaction();
symbiotic 0:346370254254 133 transactionQueryState[LEG_S]->initiateTransaction();
symbiotic 0:346370254254 134
symbiotic 0:346370254254 135 return true;
symbiotic 0:346370254254 136 }
symbiotic 0:346370254254 137
symbiotic 2:17c7a02f8401 138 bool LegInterface::queryStateWaitForCompletion(int timeout)
symbiotic 0:346370254254 139 {
symbiotic 0:346370254254 140 for(int i = 0; i < NUM_LEGS; ++i) {
symbiotic 0:346370254254 141 if(!transactionQueryState[i]->waitForCompletion(timeout)) {
symbiotic 0:346370254254 142 return false;
symbiotic 0:346370254254 143 }
symbiotic 0:346370254254 144 }
symbiotic 0:346370254254 145 return true;
symbiotic 0:346370254254 146 }
symbiotic 0:346370254254 147
symbiotic 6:a52ded837184 148 /**
symbiotic 6:a52ded837184 149 Check if the query of leg state has been completed (successful or not)
symbiotic 6:a52ded837184 150
symbiotic 6:a52ded837184 151 @return true if completed
symbiotic 6:a52ded837184 152 */
symbiotic 2:17c7a02f8401 153
symbiotic 2:17c7a02f8401 154 bool LegInterface::queryStateCompleted()
symbiotic 2:17c7a02f8401 155 {
symbiotic 2:17c7a02f8401 156 for(int i = 0; i < NUM_LEGS; ++i) {
symbiotic 2:17c7a02f8401 157 if(!transactionQueryState[i]->completed()) {
symbiotic 2:17c7a02f8401 158 return false;
symbiotic 2:17c7a02f8401 159 }
symbiotic 2:17c7a02f8401 160 }
symbiotic 2:17c7a02f8401 161 return true;
symbiotic 2:17c7a02f8401 162 }
symbiotic 2:17c7a02f8401 163
symbiotic 6:a52ded837184 164 /**
symbiotic 6:a52ded837184 165 Check if the query of leg state has been successfully completed
symbiotic 6:a52ded837184 166
symbiotic 6:a52ded837184 167 @return true if successfully completed
symbiotic 6:a52ded837184 168 */
symbiotic 6:a52ded837184 169
symbiotic 6:a52ded837184 170 bool LegInterface::queryStateSuccess()
symbiotic 6:a52ded837184 171 {
symbiotic 6:a52ded837184 172 for(int i = 0; i < NUM_LEGS; ++i) {
symbiotic 6:a52ded837184 173 if(!transactionQueryState[i]->success()) {
symbiotic 6:a52ded837184 174 return false;
symbiotic 6:a52ded837184 175 }
symbiotic 6:a52ded837184 176 }
symbiotic 6:a52ded837184 177 return true;
symbiotic 6:a52ded837184 178 }
symbiotic 6:a52ded837184 179
symbiotic 6:a52ded837184 180 /**
symbiotic 6:a52ded837184 181 Check if the query of estop has been completed (successful or not)
symbiotic 6:a52ded837184 182
symbiotic 6:a52ded837184 183 @return true if completed
symbiotic 6:a52ded837184 184 */
symbiotic 6:a52ded837184 185
symbiotic 6:a52ded837184 186 bool LegInterface::queryEstopCompleted()
symbiotic 6:a52ded837184 187 {
symbiotic 6:a52ded837184 188 for(int i = 0; i < NUM_LEGS; ++i) {
symbiotic 6:a52ded837184 189 if(!transactionEstop[i]->completed()) {
symbiotic 6:a52ded837184 190 return false;
symbiotic 6:a52ded837184 191 }
symbiotic 6:a52ded837184 192 }
symbiotic 6:a52ded837184 193 return true;
symbiotic 6:a52ded837184 194 }
symbiotic 6:a52ded837184 195
symbiotic 6:a52ded837184 196 /**
symbiotic 6:a52ded837184 197 Check if the query of estop has been successfully completed
symbiotic 6:a52ded837184 198
symbiotic 6:a52ded837184 199 @return true if successfully completed
symbiotic 6:a52ded837184 200 */
symbiotic 6:a52ded837184 201
symbiotic 6:a52ded837184 202 bool LegInterface::queryEstopSuccess()
symbiotic 6:a52ded837184 203 {
symbiotic 6:a52ded837184 204 for(int i = 0; i < NUM_LEGS; ++i) {
symbiotic 6:a52ded837184 205 if(!transactionEstop[i]->success()) {
symbiotic 6:a52ded837184 206 return false;
symbiotic 6:a52ded837184 207 }
symbiotic 6:a52ded837184 208 }
symbiotic 6:a52ded837184 209 return true;
symbiotic 6:a52ded837184 210 }
symbiotic 6:a52ded837184 211
symbiotic 3:8e7471af3453 212 void LegInterface::queryStateTest(Serial *pc)
symbiotic 3:8e7471af3453 213 {
symbiotic 3:8e7471af3453 214 for(int i = 0; i < NUM_LEGS; ++i) {
symbiotic 3:8e7471af3453 215 pc->printf("Status %d: %d %d\n\r", i, transactionQueryState[i]->getStatus(0), transactionQueryState[i]->getStatus(1));
symbiotic 3:8e7471af3453 216 }
symbiotic 3:8e7471af3453 217 }
symbiotic 3:8e7471af3453 218
symbiotic 3:8e7471af3453 219
symbiotic 0:346370254254 220 bool LegInterface::queryStateCopy(LegState_t legState[NUM_LEGS])
symbiotic 0:346370254254 221 {
symbiotic 6:a52ded837184 222 // Has the prior attempt at querying the state completed?
symbiotic 3:8e7471af3453 223 if(!queryStateCompleted()) {
symbiotic 0:346370254254 224 // No: do not attempt
symbiotic 0:346370254254 225 return false;
symbiotic 0:346370254254 226 }
symbiotic 0:346370254254 227
symbiotic 6:a52ded837184 228 // Transaction complete: copy the state of the legs only if the magic number is correct
symbiotic 6:a52ded837184 229 if(this->legState[LEG_W].magic == I2C_MAGIC_NUMBER)
symbiotic 6:a52ded837184 230 legState[LEG_W] = this->legState[LEG_W];
symbiotic 6:a52ded837184 231 if(this->legState[LEG_E].magic == I2C_MAGIC_NUMBER)
symbiotic 6:a52ded837184 232 legState[LEG_E] = this->legState[LEG_E];
symbiotic 6:a52ded837184 233 if(this->legState[LEG_S].magic == I2C_MAGIC_NUMBER)
symbiotic 6:a52ded837184 234 legState[LEG_S] = this->legState[LEG_S];
symbiotic 0:346370254254 235
symbiotic 0:346370254254 236 return true;
symbiotic 0:346370254254 237 }
symbiotic 0:346370254254 238
symbiotic 6:a52ded837184 239 /**
symbiotic 6:a52ded837184 240 Copy the state of the legs from the LegInterface structure to usable structures. In order for the data to be copied, the
symbiotic 6:a52ded837184 241 transaction must be successful, and the leg packet must have a valid magic number (the latter is evalulated on a leg-by-leg
symbiotic 6:a52ded837184 242 basis).
symbiotic 6:a52ded837184 243
symbiotic 6:a52ded837184 244 The input parameters are destructively modified only if the data are valid. The exception is the "magic" parameter. This is
symbiotic 6:a52ded837184 245 only changed if the set of transactions was successful.
symbiotic 6:a52ded837184 246
symbiotic 6:a52ded837184 247 @param liftPosition Lift position of each of the legs (m above the ground)
symbiotic 6:a52ded837184 248 @param liftVelocity Velocity of the lifts (m/s)
symbiotic 6:a52ded837184 249 @param wheelPosition Position of each of the wheels (m)
symbiotic 6:a52ded837184 250 @param wheelVelocity Velocity of the wheels (m/s)
symbiotic 6:a52ded837184 251 @param cliff Cliff sensors
symbiotic 6:a52ded837184 252 @param limit Limit sensors (lifts)
symbiotic 6:a52ded837184 253 @param estop Estop state of each of the legs
symbiotic 6:a52ded837184 254 @param magic Magic number from each leg
symbiotic 6:a52ded837184 255
symbiotic 6:a52ded837184 256 @return true if Some data were updated; false if no change
symbiotic 6:a52ded837184 257
symbiotic 6:a52ded837184 258 */
symbiotic 6:a52ded837184 259
symbiotic 6:a52ded837184 260 bool LegInterface::queryStateCopy(float liftPosition[NUM_LEGS], float liftVelocity[NUM_LEGS], float wheelPosition[NUM_LEGS], float wheelVelocity[NUM_LEGS],
symbiotic 6:a52ded837184 261 bool cliff[NUM_LEGS], bool limit[NUM_LEGS], bool estop[NUM_LEGS], uint8_t magic[NUM_LEGS])
symbiotic 2:17c7a02f8401 262 {
symbiotic 6:a52ded837184 263 // Has the prior attempt at getting state completed?
symbiotic 6:a52ded837184 264 if(!queryStateSuccess()) {
symbiotic 6:a52ded837184 265 // No: do not attempt the copy
symbiotic 2:17c7a02f8401 266 return false;
symbiotic 2:17c7a02f8401 267 }
symbiotic 2:17c7a02f8401 268
symbiotic 2:17c7a02f8401 269 // Transaction complete: copy the state of the legs
symbiotic 2:17c7a02f8401 270 for(int i = 0; i < NUM_LEGS; ++i) {
symbiotic 6:a52ded837184 271 if(this->legState[i].magic == I2C_MAGIC_NUMBER) {
symbiotic 6:a52ded837184 272 // Only copy if the magic number matches
symbiotic 6:a52ded837184 273 liftPosition[i] = this->legState[i].lift.pos / LIFT_TICKS_PER_METER - LIFT_OFFSET;
symbiotic 6:a52ded837184 274 liftVelocity[i] = this->legState[i].lift.vel / LIFT_TICKS_PER_METER;
symbiotic 6:a52ded837184 275 wheelPosition[i] = this->legState[i].wheel.pos / TICKS_PER_METER;
symbiotic 6:a52ded837184 276 wheelVelocity[i] = this->legState[i].wheel.vel / TICKS_PER_METER;
symbiotic 6:a52ded837184 277 cliff[i] = this->legState[i].cliff;
symbiotic 6:a52ded837184 278 limit[i] = this->legState[i].limit;
symbiotic 6:a52ded837184 279 estop[i] = this->legState[i].estop;
symbiotic 6:a52ded837184 280
symbiotic 6:a52ded837184 281 }
symbiotic 6:a52ded837184 282 // Also copy the magic number
symbiotic 6:a52ded837184 283 magic[i] = this->legState[i].magic;
symbiotic 2:17c7a02f8401 284 }
symbiotic 2:17c7a02f8401 285
symbiotic 2:17c7a02f8401 286 return true;
symbiotic 2:17c7a02f8401 287 }
symbiotic 2:17c7a02f8401 288
symbiotic 0:346370254254 289 void LegInterface::displayLegState(LegState_t *legState)
symbiotic 0:346370254254 290 {
symbiotic 0:346370254254 291 pc->printf("Cliff = %d, Limit = %d\n\r", legState->cliff, legState->limit);
symbiotic 0:346370254254 292 pc->printf("Wheel: pos=%d, vel=%d\n\r", legState->wheel.pos, legState->wheel.vel);
symbiotic 0:346370254254 293 pc->printf("Lift: pos=%d, vel=%d\n\r", legState->lift.pos, legState->lift.vel);
symbiotic 0:346370254254 294 }
symbiotic 0:346370254254 295
symbiotic 0:346370254254 296
symbiotic 0:346370254254 297 bool LegInterface::queryLegParams(Leg leg, LegControlParams_t *liftParams, LegControlParams_t *wheelParams)
symbiotic 0:346370254254 298 {
symbiotic 0:346370254254 299 /*
symbiotic 0:346370254254 300 if(pc != NULL) {
symbiotic 0:346370254254 301 pc->printf("completed: %d %d %d %d\n\r", transactionQueryLiftParams[leg]->completed(), transactionQueryWheelParams[leg]->completed(),
symbiotic 0:346370254254 302 transactionSetLiftParams[leg]->completed(), transactionSetWheelParams[leg]->completed());
symbiotic 0:346370254254 303 pc->printf("codes: %d %d\n\r", transactionSetLiftParams[leg]->getStatus(0), transactionSetLiftParams[leg]->getStatus(1));
symbiotic 0:346370254254 304 }
symbiotic 0:346370254254 305 */
symbiotic 0:346370254254 306
symbiotic 0:346370254254 307 // Has the prior attempt at querying/setting leg parameters completed?
symbiotic 0:346370254254 308 if(!transactionQueryLiftParams[leg]->completed() || !transactionQueryWheelParams[leg]->completed()
symbiotic 0:346370254254 309 || !transactionSetLiftParams[leg]->completed() || !transactionSetWheelParams[leg]->completed()) {
symbiotic 0:346370254254 310 // No: do not attempt
symbiotic 0:346370254254 311 return false;
symbiotic 0:346370254254 312 }
symbiotic 0:346370254254 313
symbiotic 0:346370254254 314 // Yes: initiate the transactions
symbiotic 0:346370254254 315 transactionQueryLiftParams[leg]->initiateTransaction();
symbiotic 0:346370254254 316 transactionQueryWheelParams[leg]->initiateTransaction();
symbiotic 0:346370254254 317 //pc->printf("codes: %d %d\n\r", transactionQueryWheelParams[leg]->getStatus(0), transactionQueryWheelParams[leg]->getStatus(1));
symbiotic 0:346370254254 318
symbiotic 0:346370254254 319 if(transactionQueryLiftParams[leg]->waitForCompletion() && transactionQueryWheelParams[leg]->waitForCompletion()) {
symbiotic 0:346370254254 320 // Completed
symbiotic 0:346370254254 321 *liftParams = this->legLiftParams;
symbiotic 0:346370254254 322 *wheelParams = this->legWheelParams;
symbiotic 0:346370254254 323 return true;
symbiotic 0:346370254254 324 }
symbiotic 0:346370254254 325 // A timeout happened (no copy)
symbiotic 0:346370254254 326 return false;
symbiotic 0:346370254254 327 }
symbiotic 0:346370254254 328
symbiotic 0:346370254254 329 void LegInterface::displayParams(LegControlParams_t *params)
symbiotic 0:346370254254 330 {
symbiotic 0:346370254254 331 pc->printf("Kp = %d\t Kv = %d\t Ki=%d\n\r", params->Kp, params->Kv, params->Ki);
symbiotic 0:346370254254 332 pc->printf("deadband = %d\n\r", params->deadband);
symbiotic 0:346370254254 333 pc->printf("Control signal range = [%d, %d]\n\r", params->min_val, params->max_val);
symbiotic 0:346370254254 334 pc->printf("Max error = %d, max error accum = %d\n\r", params->max_error, params->max_error_accum);
symbiotic 0:346370254254 335 pc->printf("Max acceleration = %d\n\r", params->max_accel);
symbiotic 0:346370254254 336 }
symbiotic 0:346370254254 337
symbiotic 0:346370254254 338 bool LegInterface::setLegParams(Leg leg, LegControlParams_t *liftParams, LegControlParams_t *wheelParams)
symbiotic 0:346370254254 339 {
symbiotic 0:346370254254 340
symbiotic 0:346370254254 341
symbiotic 0:346370254254 342 // Has the prior attempt at querying/setting leg parameters completed?
symbiotic 0:346370254254 343 if(!transactionQueryLiftParams[leg]->completed() || !transactionQueryWheelParams[leg]->completed()
symbiotic 0:346370254254 344 || !transactionSetLiftParams[leg]->completed() || !transactionSetWheelParams[leg]->completed()) {
symbiotic 0:346370254254 345 // No: do not attempt
symbiotic 0:346370254254 346 return false;
symbiotic 0:346370254254 347 }
symbiotic 0:346370254254 348
symbiotic 0:346370254254 349 // Copy provided parameters into structure to send
symbiotic 0:346370254254 350 this->legLiftParams = *liftParams;
symbiotic 0:346370254254 351 this->legWheelParams = *wheelParams;
symbiotic 0:346370254254 352
symbiotic 0:346370254254 353 // Yes: initiate the transactions
symbiotic 0:346370254254 354 transactionSetLiftParams[leg]->initiateTransaction();
symbiotic 0:346370254254 355 transactionSetWheelParams[leg]->initiateTransaction();
symbiotic 0:346370254254 356
symbiotic 0:346370254254 357 if(transactionQueryLiftParams[leg]->waitForCompletion() && transactionQueryWheelParams[leg]->waitForCompletion()) {
symbiotic 0:346370254254 358 // Completed
symbiotic 0:346370254254 359 return true;
symbiotic 0:346370254254 360 }
symbiotic 0:346370254254 361 // A timeout happened (no copy)
symbiotic 0:346370254254 362 return false;
symbiotic 0:346370254254 363 }
symbiotic 0:346370254254 364
symbiotic 0:346370254254 365 bool LegInterface::setLegGoal(int32_t liftPos[NUM_LEGS], int32_t wheelVel[NUM_LEGS])
symbiotic 0:346370254254 366 {
symbiotic 0:346370254254 367 // Has the previous attempt completed?
symbiotic 0:346370254254 368 if(!transactionSetGoal[LEG_W]->completed() ||
symbiotic 0:346370254254 369 !transactionSetGoal[LEG_E]->completed() ||
symbiotic 0:346370254254 370 !transactionSetGoal[LEG_S]->completed()) {
symbiotic 0:346370254254 371 // No: refuse to send
symbiotic 0:346370254254 372 return false;
symbiotic 0:346370254254 373 }
symbiotic 0:346370254254 374
symbiotic 0:346370254254 375 // Copy goals into structures to send and initiate
symbiotic 0:346370254254 376 for(int i = 0; i < LegInterface::NUM_LEGS; ++i) {
symbiotic 0:346370254254 377 legPacketSetGoal[i].contents.as_goal.liftPos = liftPos[i];
symbiotic 0:346370254254 378 legPacketSetGoal[i].contents.as_goal.wheelVel = wheelVel[i];
symbiotic 0:346370254254 379 transactionSetGoal[i]->initiateTransaction();
symbiotic 0:346370254254 380 }
symbiotic 0:346370254254 381
symbiotic 0:346370254254 382 // Indicate success
symbiotic 0:346370254254 383 return true;
symbiotic 0:346370254254 384 }
symbiotic 0:346370254254 385
symbiotic 0:346370254254 386
symbiotic 1:d3abc0577ebc 387
symbiotic 1:d3abc0577ebc 388 bool LegInterface::setLegGoal(float liftPos[NUM_LEGS], float wheelVel[NUM_LEGS])
symbiotic 1:d3abc0577ebc 389 {
symbiotic 2:17c7a02f8401 390 int32_t lift[NUM_LEGS];
symbiotic 2:17c7a02f8401 391 int32_t wheel[NUM_LEGS];
symbiotic 1:d3abc0577ebc 392
symbiotic 2:17c7a02f8401 393 for(int i = 0; i < NUM_LEGS; ++i) {
symbiotic 2:17c7a02f8401 394 wheel[i] = (int32_t) (wheelVel[i] * TICKS_PER_METER);
symbiotic 5:86f1cd6657de 395 lift[i] = (int32_t) ((liftPos[i] + LIFT_OFFSET) * LIFT_TICKS_PER_METER);
symbiotic 2:17c7a02f8401 396 }
symbiotic 2:17c7a02f8401 397
symbiotic 2:17c7a02f8401 398 return setLegGoal(lift, wheel);
symbiotic 1:d3abc0577ebc 399 }
symbiotic 1:d3abc0577ebc 400
symbiotic 6:a52ded837184 401 /**
symbiotic 6:a52ded837184 402 Check the completion status of the Set Leg Goal transaction.
symbiotic 6:a52ded837184 403
symbiotic 6:a52ded837184 404 @return true if the last leg goal set was completed.
symbiotic 6:a52ded837184 405 */
symbiotic 6:a52ded837184 406
symbiotic 6:a52ded837184 407 bool LegInterface::getLegGoalCompleted()
symbiotic 6:a52ded837184 408 {
symbiotic 6:a52ded837184 409 return(transactionSetGoal[LEG_W]->completed() &&
symbiotic 6:a52ded837184 410 transactionSetGoal[LEG_E]->completed() &&
symbiotic 6:a52ded837184 411 transactionSetGoal[LEG_S]->completed());
symbiotic 6:a52ded837184 412 }
symbiotic 6:a52ded837184 413
symbiotic 6:a52ded837184 414
symbiotic 6:a52ded837184 415 /**
symbiotic 6:a52ded837184 416 Check the completion status of the Set Leg Goal transaction.
symbiotic 6:a52ded837184 417
symbiotic 6:a52ded837184 418 @return true if the last leg goal set was completed successfully.
symbiotic 6:a52ded837184 419 */
symbiotic 6:a52ded837184 420
symbiotic 6:a52ded837184 421 bool LegInterface::getLegGoalSuccess()
symbiotic 6:a52ded837184 422 {
symbiotic 6:a52ded837184 423 return(transactionSetGoal[LEG_W]->success() &&
symbiotic 6:a52ded837184 424 transactionSetGoal[LEG_E]->success() &&
symbiotic 6:a52ded837184 425 transactionSetGoal[LEG_S]->success());
symbiotic 6:a52ded837184 426 }
symbiotic 6:a52ded837184 427
symbiotic 0:346370254254 428 void LegInterface::displayStatus()
symbiotic 0:346370254254 429 {
symbiotic 0:346370254254 430 pc->printf("Transaction status:\n\r");
symbiotic 0:346370254254 431 transactionEstop[LEG_W]->displayStatus(pc, "Estop W:\t");
symbiotic 0:346370254254 432 transactionEstop[LEG_E]->displayStatus(pc, "Estop E:\t");
symbiotic 0:346370254254 433 transactionEstop[LEG_S]->displayStatus(pc, "Estop S:\t");
symbiotic 0:346370254254 434 pc->printf("\n\r");
symbiotic 0:346370254254 435 transactionQueryState[LEG_W]->displayStatus(pc, "Query State W:\t");
symbiotic 0:346370254254 436 transactionQueryState[LEG_E]->displayStatus(pc, "Query State E:\t");
symbiotic 0:346370254254 437 transactionQueryState[LEG_S]->displayStatus(pc, "Query State S:\t");
symbiotic 0:346370254254 438 pc->printf("\n\r");
symbiotic 0:346370254254 439 transactionQueryLiftParams[LEG_W]->displayStatus(pc, "Query Lift Params W");
symbiotic 0:346370254254 440 transactionQueryLiftParams[LEG_E]->displayStatus(pc, "Query Lift Params E");
symbiotic 0:346370254254 441 transactionQueryLiftParams[LEG_S]->displayStatus(pc, "Query Lift Params S");
symbiotic 0:346370254254 442 pc->printf("\n\r");
symbiotic 0:346370254254 443 transactionQueryWheelParams[LEG_W]->displayStatus(pc, "Query Wheel Params W");
symbiotic 0:346370254254 444 transactionQueryWheelParams[LEG_E]->displayStatus(pc, "Query Wheel Params E");
symbiotic 0:346370254254 445 transactionQueryWheelParams[LEG_S]->displayStatus(pc, "Query Wheel Params S");
symbiotic 0:346370254254 446 pc->printf("\n\r");
symbiotic 0:346370254254 447 transactionSetLiftParams[LEG_W]->displayStatus(pc, "Set Lift Params W");
symbiotic 0:346370254254 448 transactionSetLiftParams[LEG_E]->displayStatus(pc, "Set Lift Params E");
symbiotic 0:346370254254 449 transactionSetLiftParams[LEG_S]->displayStatus(pc, "Set Lift Params S");
symbiotic 0:346370254254 450 pc->printf("\n\r");
symbiotic 0:346370254254 451 transactionSetWheelParams[LEG_W]->displayStatus(pc, "Set Wheel Params W");
symbiotic 0:346370254254 452 transactionSetWheelParams[LEG_E]->displayStatus(pc, "Set Wheel Params E");
symbiotic 0:346370254254 453 transactionSetWheelParams[LEG_S]->displayStatus(pc, "Set Wheel Params S");
symbiotic 0:346370254254 454 pc->printf("\n\r");
symbiotic 0:346370254254 455 transactionSetGoal[LEG_W]->displayStatus(pc, "Set Goal W:\t");
symbiotic 0:346370254254 456 transactionSetGoal[LEG_E]->displayStatus(pc, "Set Goal E:\t");
symbiotic 0:346370254254 457 transactionSetGoal[LEG_S]->displayStatus(pc, "Set Goal S:\t");
symbiotic 0:346370254254 458 pc->printf("\n\r");
symbiotic 0:346370254254 459
symbiotic 4:5f7d38d0e22d 460 }
symbiotic 4:5f7d38d0e22d 461
symbiotic 4:5f7d38d0e22d 462 void LegInterface::reset()
symbiotic 4:5f7d38d0e22d 463 {
symbiotic 4:5f7d38d0e22d 464 I2CTransaction::reset();
symbiotic 4:5f7d38d0e22d 465 }
symbiotic 4:5f7d38d0e22d 466
symbiotic 4:5f7d38d0e22d 467 void LegInterface::resetBus()
symbiotic 4:5f7d38d0e22d 468 {
symbiotic 4:5f7d38d0e22d 469 I2CTransaction::resetBus();
symbiotic 4:5f7d38d0e22d 470 }
symbiotic 4:5f7d38d0e22d 471
symbiotic 4:5f7d38d0e22d 472 void LegInterface::cycleBus()
symbiotic 4:5f7d38d0e22d 473 {
symbiotic 4:5f7d38d0e22d 474 I2CTransaction::cycleBus();
symbiotic 6:a52ded837184 475 }
symbiotic 6:a52ded837184 476
symbiotic 6:a52ded837184 477 void LegInterface::clearQueryTransactions()
symbiotic 6:a52ded837184 478 {
symbiotic 6:a52ded837184 479 transactionQueryState[LEG_W]->clearStatus();
symbiotic 6:a52ded837184 480 transactionQueryState[LEG_E]->clearStatus();
symbiotic 6:a52ded837184 481 transactionQueryState[LEG_S]->clearStatus();
symbiotic 6:a52ded837184 482 }
symbiotic 6:a52ded837184 483
symbiotic 6:a52ded837184 484 void LegInterface::clearSetGoalTransactions()
symbiotic 6:a52ded837184 485 {
symbiotic 6:a52ded837184 486 transactionSetGoal[LEG_W]->clearStatus();
symbiotic 6:a52ded837184 487 transactionSetGoal[LEG_E]->clearStatus();
symbiotic 6:a52ded837184 488 transactionSetGoal[LEG_S]->clearStatus();
symbiotic 6:a52ded837184 489 }
symbiotic 6:a52ded837184 490
symbiotic 6:a52ded837184 491
symbiotic 6:a52ded837184 492 /**
symbiotic 6:a52ded837184 493 Initialize the I2C pins and other hardware. If it has already been initialized, then
symbiotic 6:a52ded837184 494 we will re-initialize.
symbiotic 6:a52ded837184 495
symbiotic 6:a52ded837184 496 @param sda Name of the pin that is used for data
symbiotic 6:a52ded837184 497 @param scl Name of the pin that is used for the clock
symbiotic 6:a52ded837184 498 */
symbiotic 6:a52ded837184 499
symbiotic 6:a52ded837184 500 void LegInterface::initI2C(PinName sda, PinName scl)
symbiotic 6:a52ded837184 501 {
symbiotic 6:a52ded837184 502 I2CTransaction::initI2C(sda, scl);
symbiotic 6:a52ded837184 503 }