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@2:17c7a02f8401, 2014-05-23 (annotated)

Committer:
symbiotic
Date:
Fri May 23 19:27:12 2014 +0000
Revision:
2:17c7a02f8401
Parent:
1:d3abc0577ebc
Child:
3:8e7471af3453
Added support for continuous units

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