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@3:8e7471af3453, 2014-05-26 (annotated)

Committer:
symbiotic
Date:
Mon May 26 20:07:53 2014 +0000
Revision:
3:8e7471af3453
Parent:
2:17c7a02f8401
Child:
4:5f7d38d0e22d
updated to support transaction resetting (not clear that this is a good idea).;

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 3:8e7471af3453 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 2:17c7a02f8401 148
symbiotic 2:17c7a02f8401 149 bool LegInterface::queryStateCompleted()
symbiotic 2:17c7a02f8401 150 {
symbiotic 2:17c7a02f8401 151 for(int i = 0; i < NUM_LEGS; ++i) {
symbiotic 2:17c7a02f8401 152 if(!transactionQueryState[i]->completed()) {
symbiotic 2:17c7a02f8401 153 return false;
symbiotic 2:17c7a02f8401 154 }
symbiotic 2:17c7a02f8401 155 }
symbiotic 2:17c7a02f8401 156 return true;
symbiotic 2:17c7a02f8401 157 }
symbiotic 2:17c7a02f8401 158
symbiotic 3:8e7471af3453 159 void LegInterface::queryStateTest(Serial *pc)
symbiotic 3:8e7471af3453 160 {
symbiotic 3:8e7471af3453 161 for(int i = 0; i < NUM_LEGS; ++i) {
symbiotic 3:8e7471af3453 162 pc->printf("Status %d: %d %d\n\r", i, transactionQueryState[i]->getStatus(0), transactionQueryState[i]->getStatus(1));
symbiotic 3:8e7471af3453 163 }
symbiotic 3:8e7471af3453 164 }
symbiotic 3:8e7471af3453 165
symbiotic 3:8e7471af3453 166
symbiotic 0:346370254254 167 bool LegInterface::queryStateCopy(LegState_t legState[NUM_LEGS])
symbiotic 0:346370254254 168 {
symbiotic 0:346370254254 169 // Has the prior attempt at sending the estop completed?
symbiotic 3:8e7471af3453 170 if(!queryStateCompleted()) {
symbiotic 0:346370254254 171 // No: do not attempt
symbiotic 0:346370254254 172 return false;
symbiotic 0:346370254254 173 }
symbiotic 0:346370254254 174
symbiotic 0:346370254254 175 // Transaction complete: copy the state of the legs
symbiotic 0:346370254254 176 legState[LEG_W] = this->legState[LEG_W];
symbiotic 0:346370254254 177 legState[LEG_E] = this->legState[LEG_E];
symbiotic 0:346370254254 178 legState[LEG_S] = this->legState[LEG_S];
symbiotic 0:346370254254 179
symbiotic 0:346370254254 180 return true;
symbiotic 0:346370254254 181 }
symbiotic 0:346370254254 182
symbiotic 2:17c7a02f8401 183 bool LegInterface::queryStateCopy(float liftPosition[NUM_LEGS], float liftVelocity[NUM_LEGS], float wheelPosition[NUM_LEGS], float wheelVelocity[NUM_LEGS])
symbiotic 2:17c7a02f8401 184 {
symbiotic 2:17c7a02f8401 185 // Has the prior attempt at sending the estop completed?
symbiotic 3:8e7471af3453 186 if(!queryStateCompleted()) {
symbiotic 2:17c7a02f8401 187 // No: do not attempt
symbiotic 2:17c7a02f8401 188 return false;
symbiotic 2:17c7a02f8401 189 }
symbiotic 2:17c7a02f8401 190
symbiotic 2:17c7a02f8401 191 // Transaction complete: copy the state of the legs
symbiotic 2:17c7a02f8401 192 for(int i = 0; i < NUM_LEGS; ++i) {
symbiotic 2:17c7a02f8401 193 liftPosition[i] = this->legState[i].lift.pos / LIFT_TICKS_PER_METER;
symbiotic 2:17c7a02f8401 194 liftVelocity[i] = this->legState[i].lift.vel / LIFT_TICKS_PER_METER;
symbiotic 2:17c7a02f8401 195 wheelPosition[i] = this->legState[i].wheel.pos / TICKS_PER_METER;
symbiotic 2:17c7a02f8401 196 wheelVelocity[i] = this->legState[i].wheel.vel / TICKS_PER_METER;
symbiotic 2:17c7a02f8401 197 }
symbiotic 2:17c7a02f8401 198
symbiotic 2:17c7a02f8401 199 return true;
symbiotic 2:17c7a02f8401 200 }
symbiotic 2:17c7a02f8401 201
symbiotic 0:346370254254 202 void LegInterface::displayLegState(LegState_t *legState)
symbiotic 0:346370254254 203 {
symbiotic 0:346370254254 204 pc->printf("Cliff = %d, Limit = %d\n\r", legState->cliff, legState->limit);
symbiotic 0:346370254254 205 pc->printf("Wheel: pos=%d, vel=%d\n\r", legState->wheel.pos, legState->wheel.vel);
symbiotic 0:346370254254 206 pc->printf("Lift: pos=%d, vel=%d\n\r", legState->lift.pos, legState->lift.vel);
symbiotic 0:346370254254 207 }
symbiotic 0:346370254254 208
symbiotic 0:346370254254 209
symbiotic 0:346370254254 210 bool LegInterface::queryLegParams(Leg leg, LegControlParams_t *liftParams, LegControlParams_t *wheelParams)
symbiotic 0:346370254254 211 {
symbiotic 0:346370254254 212 /*
symbiotic 0:346370254254 213 if(pc != NULL) {
symbiotic 0:346370254254 214 pc->printf("completed: %d %d %d %d\n\r", transactionQueryLiftParams[leg]->completed(), transactionQueryWheelParams[leg]->completed(),
symbiotic 0:346370254254 215 transactionSetLiftParams[leg]->completed(), transactionSetWheelParams[leg]->completed());
symbiotic 0:346370254254 216 pc->printf("codes: %d %d\n\r", transactionSetLiftParams[leg]->getStatus(0), transactionSetLiftParams[leg]->getStatus(1));
symbiotic 0:346370254254 217 }
symbiotic 0:346370254254 218 */
symbiotic 0:346370254254 219
symbiotic 0:346370254254 220 // Has the prior attempt at querying/setting leg parameters completed?
symbiotic 0:346370254254 221 if(!transactionQueryLiftParams[leg]->completed() || !transactionQueryWheelParams[leg]->completed()
symbiotic 0:346370254254 222 || !transactionSetLiftParams[leg]->completed() || !transactionSetWheelParams[leg]->completed()) {
symbiotic 0:346370254254 223 // No: do not attempt
symbiotic 0:346370254254 224 return false;
symbiotic 0:346370254254 225 }
symbiotic 0:346370254254 226
symbiotic 0:346370254254 227 // Yes: initiate the transactions
symbiotic 0:346370254254 228 transactionQueryLiftParams[leg]->initiateTransaction();
symbiotic 0:346370254254 229 transactionQueryWheelParams[leg]->initiateTransaction();
symbiotic 0:346370254254 230 //pc->printf("codes: %d %d\n\r", transactionQueryWheelParams[leg]->getStatus(0), transactionQueryWheelParams[leg]->getStatus(1));
symbiotic 0:346370254254 231
symbiotic 0:346370254254 232 if(transactionQueryLiftParams[leg]->waitForCompletion() && transactionQueryWheelParams[leg]->waitForCompletion()) {
symbiotic 0:346370254254 233 // Completed
symbiotic 0:346370254254 234 *liftParams = this->legLiftParams;
symbiotic 0:346370254254 235 *wheelParams = this->legWheelParams;
symbiotic 0:346370254254 236 return true;
symbiotic 0:346370254254 237 }
symbiotic 0:346370254254 238 // A timeout happened (no copy)
symbiotic 0:346370254254 239 return false;
symbiotic 0:346370254254 240 }
symbiotic 0:346370254254 241
symbiotic 0:346370254254 242 void LegInterface::displayParams(LegControlParams_t *params)
symbiotic 0:346370254254 243 {
symbiotic 0:346370254254 244 pc->printf("Kp = %d\t Kv = %d\t Ki=%d\n\r", params->Kp, params->Kv, params->Ki);
symbiotic 0:346370254254 245 pc->printf("deadband = %d\n\r", params->deadband);
symbiotic 0:346370254254 246 pc->printf("Control signal range = [%d, %d]\n\r", params->min_val, params->max_val);
symbiotic 0:346370254254 247 pc->printf("Max error = %d, max error accum = %d\n\r", params->max_error, params->max_error_accum);
symbiotic 0:346370254254 248 pc->printf("Max acceleration = %d\n\r", params->max_accel);
symbiotic 0:346370254254 249 }
symbiotic 0:346370254254 250
symbiotic 0:346370254254 251 bool LegInterface::setLegParams(Leg leg, LegControlParams_t *liftParams, LegControlParams_t *wheelParams)
symbiotic 0:346370254254 252 {
symbiotic 0:346370254254 253
symbiotic 0:346370254254 254
symbiotic 0:346370254254 255 // Has the prior attempt at querying/setting leg parameters completed?
symbiotic 0:346370254254 256 if(!transactionQueryLiftParams[leg]->completed() || !transactionQueryWheelParams[leg]->completed()
symbiotic 0:346370254254 257 || !transactionSetLiftParams[leg]->completed() || !transactionSetWheelParams[leg]->completed()) {
symbiotic 0:346370254254 258 // No: do not attempt
symbiotic 0:346370254254 259 return false;
symbiotic 0:346370254254 260 }
symbiotic 0:346370254254 261
symbiotic 0:346370254254 262 // Copy provided parameters into structure to send
symbiotic 0:346370254254 263 this->legLiftParams = *liftParams;
symbiotic 0:346370254254 264 this->legWheelParams = *wheelParams;
symbiotic 0:346370254254 265
symbiotic 0:346370254254 266 // Yes: initiate the transactions
symbiotic 0:346370254254 267 transactionSetLiftParams[leg]->initiateTransaction();
symbiotic 0:346370254254 268 transactionSetWheelParams[leg]->initiateTransaction();
symbiotic 0:346370254254 269
symbiotic 0:346370254254 270 if(transactionQueryLiftParams[leg]->waitForCompletion() && transactionQueryWheelParams[leg]->waitForCompletion()) {
symbiotic 0:346370254254 271 // Completed
symbiotic 0:346370254254 272 return true;
symbiotic 0:346370254254 273 }
symbiotic 0:346370254254 274 // A timeout happened (no copy)
symbiotic 0:346370254254 275 return false;
symbiotic 0:346370254254 276 }
symbiotic 0:346370254254 277
symbiotic 0:346370254254 278 bool LegInterface::setLegGoal(int32_t liftPos[NUM_LEGS], int32_t wheelVel[NUM_LEGS])
symbiotic 0:346370254254 279 {
symbiotic 0:346370254254 280 // Has the previous attempt completed?
symbiotic 0:346370254254 281 if(!transactionSetGoal[LEG_W]->completed() ||
symbiotic 0:346370254254 282 !transactionSetGoal[LEG_E]->completed() ||
symbiotic 0:346370254254 283 !transactionSetGoal[LEG_S]->completed()) {
symbiotic 0:346370254254 284 // No: refuse to send
symbiotic 0:346370254254 285 return false;
symbiotic 0:346370254254 286 }
symbiotic 0:346370254254 287
symbiotic 0:346370254254 288 // Copy goals into structures to send and initiate
symbiotic 0:346370254254 289 for(int i = 0; i < LegInterface::NUM_LEGS; ++i) {
symbiotic 0:346370254254 290 legPacketSetGoal[i].contents.as_goal.liftPos = liftPos[i];
symbiotic 0:346370254254 291 legPacketSetGoal[i].contents.as_goal.wheelVel = wheelVel[i];
symbiotic 0:346370254254 292 transactionSetGoal[i]->initiateTransaction();
symbiotic 0:346370254254 293 }
symbiotic 0:346370254254 294
symbiotic 0:346370254254 295 // Indicate success
symbiotic 0:346370254254 296 return true;
symbiotic 0:346370254254 297 }
symbiotic 0:346370254254 298
symbiotic 0:346370254254 299
symbiotic 1:d3abc0577ebc 300
symbiotic 1:d3abc0577ebc 301 bool LegInterface::setLegGoal(float liftPos[NUM_LEGS], float wheelVel[NUM_LEGS])
symbiotic 1:d3abc0577ebc 302 {
symbiotic 2:17c7a02f8401 303 int32_t lift[NUM_LEGS];
symbiotic 2:17c7a02f8401 304 int32_t wheel[NUM_LEGS];
symbiotic 1:d3abc0577ebc 305
symbiotic 2:17c7a02f8401 306 for(int i = 0; i < NUM_LEGS; ++i) {
symbiotic 2:17c7a02f8401 307 wheel[i] = (int32_t) (wheelVel[i] * TICKS_PER_METER);
symbiotic 2:17c7a02f8401 308 lift[i] = (int32_t) (liftPos[i] * LIFT_TICKS_PER_METER);
symbiotic 2:17c7a02f8401 309 }
symbiotic 2:17c7a02f8401 310
symbiotic 2:17c7a02f8401 311 return setLegGoal(lift, wheel);
symbiotic 1:d3abc0577ebc 312 }
symbiotic 1:d3abc0577ebc 313
symbiotic 0:346370254254 314 void LegInterface::displayStatus()
symbiotic 0:346370254254 315 {
symbiotic 0:346370254254 316 pc->printf("Transaction status:\n\r");
symbiotic 0:346370254254 317 transactionEstop[LEG_W]->displayStatus(pc, "Estop W:\t");
symbiotic 0:346370254254 318 transactionEstop[LEG_E]->displayStatus(pc, "Estop E:\t");
symbiotic 0:346370254254 319 transactionEstop[LEG_S]->displayStatus(pc, "Estop S:\t");
symbiotic 0:346370254254 320 pc->printf("\n\r");
symbiotic 0:346370254254 321 transactionQueryState[LEG_W]->displayStatus(pc, "Query State W:\t");
symbiotic 0:346370254254 322 transactionQueryState[LEG_E]->displayStatus(pc, "Query State E:\t");
symbiotic 0:346370254254 323 transactionQueryState[LEG_S]->displayStatus(pc, "Query State S:\t");
symbiotic 0:346370254254 324 pc->printf("\n\r");
symbiotic 0:346370254254 325 transactionQueryLiftParams[LEG_W]->displayStatus(pc, "Query Lift Params W");
symbiotic 0:346370254254 326 transactionQueryLiftParams[LEG_E]->displayStatus(pc, "Query Lift Params E");
symbiotic 0:346370254254 327 transactionQueryLiftParams[LEG_S]->displayStatus(pc, "Query Lift Params S");
symbiotic 0:346370254254 328 pc->printf("\n\r");
symbiotic 0:346370254254 329 transactionQueryWheelParams[LEG_W]->displayStatus(pc, "Query Wheel Params W");
symbiotic 0:346370254254 330 transactionQueryWheelParams[LEG_E]->displayStatus(pc, "Query Wheel Params E");
symbiotic 0:346370254254 331 transactionQueryWheelParams[LEG_S]->displayStatus(pc, "Query Wheel Params S");
symbiotic 0:346370254254 332 pc->printf("\n\r");
symbiotic 0:346370254254 333 transactionSetLiftParams[LEG_W]->displayStatus(pc, "Set Lift Params W");
symbiotic 0:346370254254 334 transactionSetLiftParams[LEG_E]->displayStatus(pc, "Set Lift Params E");
symbiotic 0:346370254254 335 transactionSetLiftParams[LEG_S]->displayStatus(pc, "Set Lift Params S");
symbiotic 0:346370254254 336 pc->printf("\n\r");
symbiotic 0:346370254254 337 transactionSetWheelParams[LEG_W]->displayStatus(pc, "Set Wheel Params W");
symbiotic 0:346370254254 338 transactionSetWheelParams[LEG_E]->displayStatus(pc, "Set Wheel Params E");
symbiotic 0:346370254254 339 transactionSetWheelParams[LEG_S]->displayStatus(pc, "Set Wheel Params S");
symbiotic 0:346370254254 340 pc->printf("\n\r");
symbiotic 0:346370254254 341 transactionSetGoal[LEG_W]->displayStatus(pc, "Set Goal W:\t");
symbiotic 0:346370254254 342 transactionSetGoal[LEG_E]->displayStatus(pc, "Set Goal E:\t");
symbiotic 0:346370254254 343 transactionSetGoal[LEG_S]->displayStatus(pc, "Set Goal S:\t");
symbiotic 0:346370254254 344 pc->printf("\n\r");
symbiotic 0:346370254254 345
symbiotic 0:346370254254 346 }