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@8:402f4a307d08, 2014-12-20 (annotated)

Committer:
symbiotic
Date:
Sat Dec 20 15:41:40 2014 +0000
Revision:
8:402f4a307d08
Parent:
7:c164b4869f74 
Added transaction reset support

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 7:c164b4869f74 83
symbiotic 0:346370254254 84 }
symbiotic 0:346370254254 85
symbiotic 0:346370254254 86 /**
symbiotic 0:346370254254 87 Attempt to send an estop to all legs.
symbiotic 0:346370254254 88
symbiotic 0:346370254254 89 @param estop true = stop all motion; false = enable motion
symbiotic 0:346370254254 90
symbiotic 0:346370254254 91 @return true = transaction successfully scheduled, false = prior attempt at sending had not completed.
symbiotic 0:346370254254 92 */
symbiotic 0:346370254254 93
symbiotic 0:346370254254 94 bool LegInterface::sendEstop(bool estop)
symbiotic 0:346370254254 95 {
symbiotic 0:346370254254 96 // Has the prior attempt at sending the estop completed?
symbiotic 0:346370254254 97 if(!transactionEstop[LEG_W]->completed() ||
symbiotic 0:346370254254 98 !transactionEstop[LEG_E]->completed() ||
symbiotic 0:346370254254 99 !transactionEstop[LEG_S]->completed()) {
symbiotic 0:346370254254 100 // No: do not attempt
symbiotic 0:346370254254 101 return false;
symbiotic 0:346370254254 102 }
symbiotic 0:346370254254 103
symbiotic 0:346370254254 104 // Yes - configure the packet (all packets are the same in this case)
symbiotic 0:346370254254 105 legPacketEstop.type = estop?nsPacketsLeg::ESTOP_ON:nsPacketsLeg::ESTOP_OFF;
symbiotic 0:346370254254 106
symbiotic 0:346370254254 107 // Schedule the transactions
symbiotic 0:346370254254 108 transactionEstop[LEG_W]->initiateTransaction();
symbiotic 0:346370254254 109 transactionEstop[LEG_E]->initiateTransaction();
symbiotic 0:346370254254 110 transactionEstop[LEG_S]->initiateTransaction();
symbiotic 0:346370254254 111
symbiotic 0:346370254254 112 // Complete
symbiotic 0:346370254254 113 return true;
symbiotic 0:346370254254 114 }
symbiotic 0:346370254254 115
symbiotic 0:346370254254 116 bool LegInterface::queryStateInitiate()
symbiotic 0:346370254254 117 {
symbiotic 3:8e7471af3453 118 /*
symbiotic 3:8e7471af3453 119 transactionQueryState[LEG_W]->checkTransaction();
symbiotic 3:8e7471af3453 120 transactionQueryState[LEG_E]->checkTransaction();
symbiotic 3:8e7471af3453 121 transactionQueryState[LEG_S]->checkTransaction();
symbiotic 3:8e7471af3453 122 */
symbiotic 4:5f7d38d0e22d 123
symbiotic 0:346370254254 124 // Has the prior attempt at sending the estop completed?
symbiotic 3:8e7471af3453 125 if(!queryStateCompleted()) {
symbiotic 0:346370254254 126 // No: do not attempt
symbiotic 0:346370254254 127 return false;
symbiotic 0:346370254254 128 }
symbiotic 0:346370254254 129
symbiotic 7:c164b4869f74 130 //this->pc->printf("########################## FOO @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@\n\r");
symbiotic 7:c164b4869f74 131 // Magic numbers
symbiotic 7:c164b4869f74 132 for(int i= 0; i < NUM_LEGS; ++i) {
symbiotic 7:c164b4869f74 133 this->legState[i].magic = 0;
symbiotic 7:c164b4869f74 134 }
symbiotic 7:c164b4869f74 135
symbiotic 0:346370254254 136 // Query last had completed
symbiotic 0:346370254254 137 // Schedule the transactions
symbiotic 0:346370254254 138 transactionQueryState[LEG_W]->initiateTransaction();
symbiotic 0:346370254254 139 transactionQueryState[LEG_E]->initiateTransaction();
symbiotic 0:346370254254 140 transactionQueryState[LEG_S]->initiateTransaction();
symbiotic 0:346370254254 141
symbiotic 0:346370254254 142 return true;
symbiotic 0:346370254254 143 }
symbiotic 0:346370254254 144
symbiotic 2:17c7a02f8401 145 bool LegInterface::queryStateWaitForCompletion(int timeout)
symbiotic 0:346370254254 146 {
symbiotic 0:346370254254 147 for(int i = 0; i < NUM_LEGS; ++i) {
symbiotic 0:346370254254 148 if(!transactionQueryState[i]->waitForCompletion(timeout)) {
symbiotic 0:346370254254 149 return false;
symbiotic 0:346370254254 150 }
symbiotic 0:346370254254 151 }
symbiotic 0:346370254254 152 return true;
symbiotic 0:346370254254 153 }
symbiotic 0:346370254254 154
symbiotic 6:a52ded837184 155 /**
symbiotic 6:a52ded837184 156 Check if the query of leg state has been completed (successful or not)
symbiotic 6:a52ded837184 157
symbiotic 6:a52ded837184 158 @return true if completed
symbiotic 6:a52ded837184 159 */
symbiotic 2:17c7a02f8401 160
symbiotic 2:17c7a02f8401 161 bool LegInterface::queryStateCompleted()
symbiotic 2:17c7a02f8401 162 {
symbiotic 2:17c7a02f8401 163 for(int i = 0; i < NUM_LEGS; ++i) {
symbiotic 2:17c7a02f8401 164 if(!transactionQueryState[i]->completed()) {
symbiotic 2:17c7a02f8401 165 return false;
symbiotic 2:17c7a02f8401 166 }
symbiotic 2:17c7a02f8401 167 }
symbiotic 2:17c7a02f8401 168 return true;
symbiotic 2:17c7a02f8401 169 }
symbiotic 2:17c7a02f8401 170
symbiotic 6:a52ded837184 171 /**
symbiotic 6:a52ded837184 172 Check if the query of leg state has been successfully completed
symbiotic 6:a52ded837184 173
symbiotic 6:a52ded837184 174 @return true if successfully completed
symbiotic 6:a52ded837184 175 */
symbiotic 6:a52ded837184 176
symbiotic 6:a52ded837184 177 bool LegInterface::queryStateSuccess()
symbiotic 6:a52ded837184 178 {
symbiotic 6:a52ded837184 179 for(int i = 0; i < NUM_LEGS; ++i) {
symbiotic 6:a52ded837184 180 if(!transactionQueryState[i]->success()) {
symbiotic 6:a52ded837184 181 return false;
symbiotic 6:a52ded837184 182 }
symbiotic 6:a52ded837184 183 }
symbiotic 6:a52ded837184 184 return true;
symbiotic 6:a52ded837184 185 }
symbiotic 6:a52ded837184 186
symbiotic 6:a52ded837184 187 /**
symbiotic 7:c164b4869f74 188 Print out the internal magic numbers
symbiotic 7:c164b4869f74 189 */
symbiotic 7:c164b4869f74 190
symbiotic 7:c164b4869f74 191 void LegInterface::queryStateReportMagic()
symbiotic 7:c164b4869f74 192 {
symbiotic 7:c164b4869f74 193 for(int i = 0; i < NUM_LEGS; ++i) {
symbiotic 7:c164b4869f74 194 pc->printf("%x ", legState[i].magic);
symbiotic 7:c164b4869f74 195 }
symbiotic 7:c164b4869f74 196 pc->printf("\n\r");
symbiotic 7:c164b4869f74 197 }
symbiotic 7:c164b4869f74 198 /**
symbiotic 6:a52ded837184 199 Check if the query of estop has been completed (successful or not)
symbiotic 6:a52ded837184 200
symbiotic 6:a52ded837184 201 @return true if completed
symbiotic 6:a52ded837184 202 */
symbiotic 6:a52ded837184 203
symbiotic 6:a52ded837184 204 bool LegInterface::queryEstopCompleted()
symbiotic 6:a52ded837184 205 {
symbiotic 6:a52ded837184 206 for(int i = 0; i < NUM_LEGS; ++i) {
symbiotic 6:a52ded837184 207 if(!transactionEstop[i]->completed()) {
symbiotic 6:a52ded837184 208 return false;
symbiotic 6:a52ded837184 209 }
symbiotic 6:a52ded837184 210 }
symbiotic 6:a52ded837184 211 return true;
symbiotic 6:a52ded837184 212 }
symbiotic 6:a52ded837184 213
symbiotic 6:a52ded837184 214 /**
symbiotic 6:a52ded837184 215 Check if the query of estop has been successfully completed
symbiotic 6:a52ded837184 216
symbiotic 6:a52ded837184 217 @return true if successfully completed
symbiotic 6:a52ded837184 218 */
symbiotic 6:a52ded837184 219
symbiotic 6:a52ded837184 220 bool LegInterface::queryEstopSuccess()
symbiotic 6:a52ded837184 221 {
symbiotic 6:a52ded837184 222 for(int i = 0; i < NUM_LEGS; ++i) {
symbiotic 6:a52ded837184 223 if(!transactionEstop[i]->success()) {
symbiotic 6:a52ded837184 224 return false;
symbiotic 6:a52ded837184 225 }
symbiotic 6:a52ded837184 226 }
symbiotic 6:a52ded837184 227 return true;
symbiotic 6:a52ded837184 228 }
symbiotic 6:a52ded837184 229
symbiotic 3:8e7471af3453 230 void LegInterface::queryStateTest(Serial *pc)
symbiotic 3:8e7471af3453 231 {
symbiotic 3:8e7471af3453 232 for(int i = 0; i < NUM_LEGS; ++i) {
symbiotic 3:8e7471af3453 233 pc->printf("Status %d: %d %d\n\r", i, transactionQueryState[i]->getStatus(0), transactionQueryState[i]->getStatus(1));
symbiotic 3:8e7471af3453 234 }
symbiotic 3:8e7471af3453 235 }
symbiotic 3:8e7471af3453 236
symbiotic 3:8e7471af3453 237
symbiotic 0:346370254254 238 bool LegInterface::queryStateCopy(LegState_t legState[NUM_LEGS])
symbiotic 0:346370254254 239 {
symbiotic 6:a52ded837184 240 // Has the prior attempt at querying the state completed?
symbiotic 3:8e7471af3453 241 if(!queryStateCompleted()) {
symbiotic 0:346370254254 242 // No: do not attempt
symbiotic 0:346370254254 243 return false;
symbiotic 0:346370254254 244 }
symbiotic 0:346370254254 245
symbiotic 6:a52ded837184 246 // Transaction complete: copy the state of the legs only if the magic number is correct
symbiotic 6:a52ded837184 247 if(this->legState[LEG_W].magic == I2C_MAGIC_NUMBER)
symbiotic 6:a52ded837184 248 legState[LEG_W] = this->legState[LEG_W];
symbiotic 6:a52ded837184 249 if(this->legState[LEG_E].magic == I2C_MAGIC_NUMBER)
symbiotic 6:a52ded837184 250 legState[LEG_E] = this->legState[LEG_E];
symbiotic 6:a52ded837184 251 if(this->legState[LEG_S].magic == I2C_MAGIC_NUMBER)
symbiotic 6:a52ded837184 252 legState[LEG_S] = this->legState[LEG_S];
symbiotic 0:346370254254 253
symbiotic 0:346370254254 254 return true;
symbiotic 0:346370254254 255 }
symbiotic 0:346370254254 256
symbiotic 6:a52ded837184 257 /**
symbiotic 6:a52ded837184 258 Copy the state of the legs from the LegInterface structure to usable structures. In order for the data to be copied, the
symbiotic 7:c164b4869f74 259 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 260 basis).
symbiotic 7:c164b4869f74 261
symbiotic 6:a52ded837184 262 The input parameters are destructively modified only if the data are valid. The exception is the "magic" parameter. This is
symbiotic 6:a52ded837184 263 only changed if the set of transactions was successful.
symbiotic 7:c164b4869f74 264
symbiotic 6:a52ded837184 265 @param liftPosition Lift position of each of the legs (m above the ground)
symbiotic 6:a52ded837184 266 @param liftVelocity Velocity of the lifts (m/s)
symbiotic 6:a52ded837184 267 @param wheelPosition Position of each of the wheels (m)
symbiotic 6:a52ded837184 268 @param wheelVelocity Velocity of the wheels (m/s)
symbiotic 6:a52ded837184 269 @param cliff Cliff sensors
symbiotic 6:a52ded837184 270 @param limit Limit sensors (lifts)
symbiotic 6:a52ded837184 271 @param estop Estop state of each of the legs
symbiotic 6:a52ded837184 272 @param magic Magic number from each leg
symbiotic 7:c164b4869f74 273
symbiotic 6:a52ded837184 274 @return true if Some data were updated; false if no change
symbiotic 7:c164b4869f74 275
symbiotic 6:a52ded837184 276 */
symbiotic 6:a52ded837184 277
symbiotic 7:c164b4869f74 278 bool LegInterface::queryStateCopy(float liftPosition[NUM_LEGS], float liftVelocity[NUM_LEGS], float wheelPosition[NUM_LEGS], float wheelVelocity[NUM_LEGS],
symbiotic 7:c164b4869f74 279 bool cliff[NUM_LEGS], bool limit[NUM_LEGS], bool estop[NUM_LEGS], uint8_t magic[NUM_LEGS])
symbiotic 2:17c7a02f8401 280 {
symbiotic 6:a52ded837184 281 // Has the prior attempt at getting state completed?
symbiotic 6:a52ded837184 282 if(!queryStateSuccess()) {
symbiotic 6:a52ded837184 283 // No: do not attempt the copy
symbiotic 2:17c7a02f8401 284 return false;
symbiotic 2:17c7a02f8401 285 }
symbiotic 2:17c7a02f8401 286
symbiotic 2:17c7a02f8401 287 // Transaction complete: copy the state of the legs
symbiotic 2:17c7a02f8401 288 for(int i = 0; i < NUM_LEGS; ++i) {
symbiotic 6:a52ded837184 289 if(this->legState[i].magic == I2C_MAGIC_NUMBER) {
symbiotic 6:a52ded837184 290 // Only copy if the magic number matches
symbiotic 6:a52ded837184 291 liftPosition[i] = this->legState[i].lift.pos / LIFT_TICKS_PER_METER - LIFT_OFFSET;
symbiotic 6:a52ded837184 292 liftVelocity[i] = this->legState[i].lift.vel / LIFT_TICKS_PER_METER;
symbiotic 6:a52ded837184 293 wheelPosition[i] = this->legState[i].wheel.pos / TICKS_PER_METER;
symbiotic 6:a52ded837184 294 wheelVelocity[i] = this->legState[i].wheel.vel / TICKS_PER_METER;
symbiotic 6:a52ded837184 295 cliff[i] = this->legState[i].cliff;
symbiotic 6:a52ded837184 296 limit[i] = this->legState[i].limit;
symbiotic 6:a52ded837184 297 estop[i] = this->legState[i].estop;
symbiotic 6:a52ded837184 298
symbiotic 6:a52ded837184 299 }
symbiotic 6:a52ded837184 300 // Also copy the magic number
symbiotic 6:a52ded837184 301 magic[i] = this->legState[i].magic;
symbiotic 7:c164b4869f74 302 //pc->printf("%x ", magic[i]);
symbiotic 2:17c7a02f8401 303 }
symbiotic 7:c164b4869f74 304 //pc->printf("\n\r");
symbiotic 2:17c7a02f8401 305
symbiotic 2:17c7a02f8401 306 return true;
symbiotic 2:17c7a02f8401 307 }
symbiotic 2:17c7a02f8401 308
symbiotic 0:346370254254 309 void LegInterface::displayLegState(LegState_t *legState)
symbiotic 0:346370254254 310 {
symbiotic 0:346370254254 311 pc->printf("Cliff = %d, Limit = %d\n\r", legState->cliff, legState->limit);
symbiotic 0:346370254254 312 pc->printf("Wheel: pos=%d, vel=%d\n\r", legState->wheel.pos, legState->wheel.vel);
symbiotic 0:346370254254 313 pc->printf("Lift: pos=%d, vel=%d\n\r", legState->lift.pos, legState->lift.vel);
symbiotic 0:346370254254 314 }
symbiotic 0:346370254254 315
symbiotic 0:346370254254 316
symbiotic 0:346370254254 317 bool LegInterface::queryLegParams(Leg leg, LegControlParams_t *liftParams, LegControlParams_t *wheelParams)
symbiotic 0:346370254254 318 {
symbiotic 0:346370254254 319 /*
symbiotic 0:346370254254 320 if(pc != NULL) {
symbiotic 0:346370254254 321 pc->printf("completed: %d %d %d %d\n\r", transactionQueryLiftParams[leg]->completed(), transactionQueryWheelParams[leg]->completed(),
symbiotic 0:346370254254 322 transactionSetLiftParams[leg]->completed(), transactionSetWheelParams[leg]->completed());
symbiotic 0:346370254254 323 pc->printf("codes: %d %d\n\r", transactionSetLiftParams[leg]->getStatus(0), transactionSetLiftParams[leg]->getStatus(1));
symbiotic 0:346370254254 324 }
symbiotic 0:346370254254 325 */
symbiotic 0:346370254254 326
symbiotic 0:346370254254 327 // Has the prior attempt at querying/setting leg parameters completed?
symbiotic 0:346370254254 328 if(!transactionQueryLiftParams[leg]->completed() || !transactionQueryWheelParams[leg]->completed()
symbiotic 0:346370254254 329 || !transactionSetLiftParams[leg]->completed() || !transactionSetWheelParams[leg]->completed()) {
symbiotic 0:346370254254 330 // No: do not attempt
symbiotic 0:346370254254 331 return false;
symbiotic 0:346370254254 332 }
symbiotic 0:346370254254 333
symbiotic 0:346370254254 334 // Yes: initiate the transactions
symbiotic 0:346370254254 335 transactionQueryLiftParams[leg]->initiateTransaction();
symbiotic 0:346370254254 336 transactionQueryWheelParams[leg]->initiateTransaction();
symbiotic 0:346370254254 337 //pc->printf("codes: %d %d\n\r", transactionQueryWheelParams[leg]->getStatus(0), transactionQueryWheelParams[leg]->getStatus(1));
symbiotic 0:346370254254 338
symbiotic 0:346370254254 339 if(transactionQueryLiftParams[leg]->waitForCompletion() && transactionQueryWheelParams[leg]->waitForCompletion()) {
symbiotic 0:346370254254 340 // Completed
symbiotic 0:346370254254 341 *liftParams = this->legLiftParams;
symbiotic 0:346370254254 342 *wheelParams = this->legWheelParams;
symbiotic 0:346370254254 343 return true;
symbiotic 0:346370254254 344 }
symbiotic 0:346370254254 345 // A timeout happened (no copy)
symbiotic 0:346370254254 346 return false;
symbiotic 0:346370254254 347 }
symbiotic 0:346370254254 348
symbiotic 0:346370254254 349 void LegInterface::displayParams(LegControlParams_t *params)
symbiotic 0:346370254254 350 {
symbiotic 0:346370254254 351 pc->printf("Kp = %d\t Kv = %d\t Ki=%d\n\r", params->Kp, params->Kv, params->Ki);
symbiotic 0:346370254254 352 pc->printf("deadband = %d\n\r", params->deadband);
symbiotic 0:346370254254 353 pc->printf("Control signal range = [%d, %d]\n\r", params->min_val, params->max_val);
symbiotic 0:346370254254 354 pc->printf("Max error = %d, max error accum = %d\n\r", params->max_error, params->max_error_accum);
symbiotic 0:346370254254 355 pc->printf("Max acceleration = %d\n\r", params->max_accel);
symbiotic 0:346370254254 356 }
symbiotic 0:346370254254 357
symbiotic 0:346370254254 358 bool LegInterface::setLegParams(Leg leg, LegControlParams_t *liftParams, LegControlParams_t *wheelParams)
symbiotic 0:346370254254 359 {
symbiotic 0:346370254254 360
symbiotic 0:346370254254 361
symbiotic 0:346370254254 362 // Has the prior attempt at querying/setting leg parameters completed?
symbiotic 0:346370254254 363 if(!transactionQueryLiftParams[leg]->completed() || !transactionQueryWheelParams[leg]->completed()
symbiotic 0:346370254254 364 || !transactionSetLiftParams[leg]->completed() || !transactionSetWheelParams[leg]->completed()) {
symbiotic 0:346370254254 365 // No: do not attempt
symbiotic 0:346370254254 366 return false;
symbiotic 0:346370254254 367 }
symbiotic 0:346370254254 368
symbiotic 0:346370254254 369 // Copy provided parameters into structure to send
symbiotic 0:346370254254 370 this->legLiftParams = *liftParams;
symbiotic 0:346370254254 371 this->legWheelParams = *wheelParams;
symbiotic 0:346370254254 372
symbiotic 0:346370254254 373 // Yes: initiate the transactions
symbiotic 0:346370254254 374 transactionSetLiftParams[leg]->initiateTransaction();
symbiotic 0:346370254254 375 transactionSetWheelParams[leg]->initiateTransaction();
symbiotic 0:346370254254 376
symbiotic 0:346370254254 377 if(transactionQueryLiftParams[leg]->waitForCompletion() && transactionQueryWheelParams[leg]->waitForCompletion()) {
symbiotic 0:346370254254 378 // Completed
symbiotic 0:346370254254 379 return true;
symbiotic 0:346370254254 380 }
symbiotic 0:346370254254 381 // A timeout happened (no copy)
symbiotic 0:346370254254 382 return false;
symbiotic 0:346370254254 383 }
symbiotic 0:346370254254 384
symbiotic 8:402f4a307d08 385 /**
symbiotic 8:402f4a307d08 386 Set the goals for all the legs
symbiotic 8:402f4a307d08 387 */
symbiotic 8:402f4a307d08 388
symbiotic 0:346370254254 389 bool LegInterface::setLegGoal(int32_t liftPos[NUM_LEGS], int32_t wheelVel[NUM_LEGS])
symbiotic 0:346370254254 390 {
symbiotic 0:346370254254 391 // Has the previous attempt completed?
symbiotic 0:346370254254 392 if(!transactionSetGoal[LEG_W]->completed() ||
symbiotic 0:346370254254 393 !transactionSetGoal[LEG_E]->completed() ||
symbiotic 0:346370254254 394 !transactionSetGoal[LEG_S]->completed()) {
symbiotic 0:346370254254 395 // No: refuse to send
symbiotic 8:402f4a307d08 396
symbiotic 8:402f4a307d08 397 // Debugging
symbiotic 8:402f4a307d08 398 pc->putc('{');
symbiotic 8:402f4a307d08 399 pc->putc('0' + transactionSetGoal[LEG_W]->completed());
symbiotic 8:402f4a307d08 400 pc->putc('0' + transactionSetGoal[LEG_E]->completed());
symbiotic 8:402f4a307d08 401 pc->putc('0' + transactionSetGoal[LEG_S]->completed());
symbiotic 8:402f4a307d08 402 pc->putc('-');
symbiotic 8:402f4a307d08 403
symbiotic 8:402f4a307d08 404 /*
symbiotic 8:402f4a307d08 405 // For offending legs: clear the I2C status (next attempt will be seen as completed)
symbiotic 8:402f4a307d08 406 for(int i = 0; i < NUM_LEGS; ++i) {
symbiotic 8:402f4a307d08 407 if(!transactionSetGoal[i]->completed()) {
symbiotic 8:402f4a307d08 408 transactionSetGoal[i]->clearStatus();
symbiotic 8:402f4a307d08 409 }
symbiotic 8:402f4a307d08 410 }
symbiotic 8:402f4a307d08 411 */
symbiotic 8:402f4a307d08 412
symbiotic 8:402f4a307d08 413 pc->putc('0' + transactionSetGoal[LEG_W]->completed());
symbiotic 8:402f4a307d08 414 pc->putc('0' + transactionSetGoal[LEG_E]->completed());
symbiotic 8:402f4a307d08 415 pc->putc('0' + transactionSetGoal[LEG_S]->completed());
symbiotic 8:402f4a307d08 416 pc->putc('}');
symbiotic 0:346370254254 417 return false;
symbiotic 0:346370254254 418 }
symbiotic 0:346370254254 419
symbiotic 0:346370254254 420 // Copy goals into structures to send and initiate
symbiotic 0:346370254254 421 for(int i = 0; i < LegInterface::NUM_LEGS; ++i) {
symbiotic 0:346370254254 422 legPacketSetGoal[i].contents.as_goal.liftPos = liftPos[i];
symbiotic 0:346370254254 423 legPacketSetGoal[i].contents.as_goal.wheelVel = wheelVel[i];
symbiotic 0:346370254254 424 transactionSetGoal[i]->initiateTransaction();
symbiotic 0:346370254254 425 }
symbiotic 0:346370254254 426
symbiotic 0:346370254254 427 // Indicate success
symbiotic 0:346370254254 428 return true;
symbiotic 0:346370254254 429 }
symbiotic 0:346370254254 430
symbiotic 0:346370254254 431
symbiotic 1:d3abc0577ebc 432
symbiotic 1:d3abc0577ebc 433 bool LegInterface::setLegGoal(float liftPos[NUM_LEGS], float wheelVel[NUM_LEGS])
symbiotic 1:d3abc0577ebc 434 {
symbiotic 2:17c7a02f8401 435 int32_t lift[NUM_LEGS];
symbiotic 2:17c7a02f8401 436 int32_t wheel[NUM_LEGS];
symbiotic 1:d3abc0577ebc 437
symbiotic 2:17c7a02f8401 438 for(int i = 0; i < NUM_LEGS; ++i) {
symbiotic 2:17c7a02f8401 439 wheel[i] = (int32_t) (wheelVel[i] * TICKS_PER_METER);
symbiotic 5:86f1cd6657de 440 lift[i] = (int32_t) ((liftPos[i] + LIFT_OFFSET) * LIFT_TICKS_PER_METER);
symbiotic 2:17c7a02f8401 441 }
symbiotic 2:17c7a02f8401 442
symbiotic 2:17c7a02f8401 443 return setLegGoal(lift, wheel);
symbiotic 1:d3abc0577ebc 444 }
symbiotic 1:d3abc0577ebc 445
symbiotic 6:a52ded837184 446 /**
symbiotic 6:a52ded837184 447 Check the completion status of the Set Leg Goal transaction.
symbiotic 6:a52ded837184 448
symbiotic 6:a52ded837184 449 @return true if the last leg goal set was completed.
symbiotic 6:a52ded837184 450 */
symbiotic 6:a52ded837184 451
symbiotic 6:a52ded837184 452 bool LegInterface::getLegGoalCompleted()
symbiotic 6:a52ded837184 453 {
symbiotic 6:a52ded837184 454 return(transactionSetGoal[LEG_W]->completed() &&
symbiotic 6:a52ded837184 455 transactionSetGoal[LEG_E]->completed() &&
symbiotic 6:a52ded837184 456 transactionSetGoal[LEG_S]->completed());
symbiotic 6:a52ded837184 457 }
symbiotic 6:a52ded837184 458
symbiotic 6:a52ded837184 459
symbiotic 6:a52ded837184 460 /**
symbiotic 6:a52ded837184 461 Check the completion status of the Set Leg Goal transaction.
symbiotic 6:a52ded837184 462
symbiotic 6:a52ded837184 463 @return true if the last leg goal set was completed successfully.
symbiotic 6:a52ded837184 464 */
symbiotic 6:a52ded837184 465
symbiotic 6:a52ded837184 466 bool LegInterface::getLegGoalSuccess()
symbiotic 6:a52ded837184 467 {
symbiotic 6:a52ded837184 468 return(transactionSetGoal[LEG_W]->success() &&
symbiotic 6:a52ded837184 469 transactionSetGoal[LEG_E]->success() &&
symbiotic 6:a52ded837184 470 transactionSetGoal[LEG_S]->success());
symbiotic 6:a52ded837184 471 }
symbiotic 6:a52ded837184 472
symbiotic 0:346370254254 473 void LegInterface::displayStatus()
symbiotic 0:346370254254 474 {
symbiotic 0:346370254254 475 pc->printf("Transaction status:\n\r");
symbiotic 0:346370254254 476 transactionEstop[LEG_W]->displayStatus(pc, "Estop W:\t");
symbiotic 0:346370254254 477 transactionEstop[LEG_E]->displayStatus(pc, "Estop E:\t");
symbiotic 0:346370254254 478 transactionEstop[LEG_S]->displayStatus(pc, "Estop S:\t");
symbiotic 0:346370254254 479 pc->printf("\n\r");
symbiotic 0:346370254254 480 transactionQueryState[LEG_W]->displayStatus(pc, "Query State W:\t");
symbiotic 0:346370254254 481 transactionQueryState[LEG_E]->displayStatus(pc, "Query State E:\t");
symbiotic 0:346370254254 482 transactionQueryState[LEG_S]->displayStatus(pc, "Query State S:\t");
symbiotic 0:346370254254 483 pc->printf("\n\r");
symbiotic 0:346370254254 484 transactionQueryLiftParams[LEG_W]->displayStatus(pc, "Query Lift Params W");
symbiotic 0:346370254254 485 transactionQueryLiftParams[LEG_E]->displayStatus(pc, "Query Lift Params E");
symbiotic 0:346370254254 486 transactionQueryLiftParams[LEG_S]->displayStatus(pc, "Query Lift Params S");
symbiotic 0:346370254254 487 pc->printf("\n\r");
symbiotic 0:346370254254 488 transactionQueryWheelParams[LEG_W]->displayStatus(pc, "Query Wheel Params W");
symbiotic 0:346370254254 489 transactionQueryWheelParams[LEG_E]->displayStatus(pc, "Query Wheel Params E");
symbiotic 0:346370254254 490 transactionQueryWheelParams[LEG_S]->displayStatus(pc, "Query Wheel Params S");
symbiotic 0:346370254254 491 pc->printf("\n\r");
symbiotic 0:346370254254 492 transactionSetLiftParams[LEG_W]->displayStatus(pc, "Set Lift Params W");
symbiotic 0:346370254254 493 transactionSetLiftParams[LEG_E]->displayStatus(pc, "Set Lift Params E");
symbiotic 0:346370254254 494 transactionSetLiftParams[LEG_S]->displayStatus(pc, "Set Lift Params S");
symbiotic 0:346370254254 495 pc->printf("\n\r");
symbiotic 0:346370254254 496 transactionSetWheelParams[LEG_W]->displayStatus(pc, "Set Wheel Params W");
symbiotic 0:346370254254 497 transactionSetWheelParams[LEG_E]->displayStatus(pc, "Set Wheel Params E");
symbiotic 0:346370254254 498 transactionSetWheelParams[LEG_S]->displayStatus(pc, "Set Wheel Params S");
symbiotic 0:346370254254 499 pc->printf("\n\r");
symbiotic 0:346370254254 500 transactionSetGoal[LEG_W]->displayStatus(pc, "Set Goal W:\t");
symbiotic 0:346370254254 501 transactionSetGoal[LEG_E]->displayStatus(pc, "Set Goal E:\t");
symbiotic 0:346370254254 502 transactionSetGoal[LEG_S]->displayStatus(pc, "Set Goal S:\t");
symbiotic 0:346370254254 503 pc->printf("\n\r");
symbiotic 0:346370254254 504
symbiotic 4:5f7d38d0e22d 505 }
symbiotic 4:5f7d38d0e22d 506
symbiotic 8:402f4a307d08 507 /**
symbiotic 8:402f4a307d08 508 Reset the transaction status for each of the legs (if necessary).
symbiotic 8:402f4a307d08 509
symbiotic 8:402f4a307d08 510 This affects the internal status of the MODI2C transaction objects. Hence,
symbiotic 8:402f4a307d08 511 this method should only be called if it is clear that the ISR has "forgotten"
symbiotic 8:402f4a307d08 512 about one of the leg transactions.
symbiotic 8:402f4a307d08 513
symbiotic 8:402f4a307d08 514 @TODO
symbiotic 8:402f4a307d08 515 It is unclear why we need this. There is probably a bug in the MODI2C code that
symbiotic 8:402f4a307d08 516 causes this to happen. However, it is interesting that it is only this transaction type
symbiotic 8:402f4a307d08 517 that seems to need this...
symbiotic 8:402f4a307d08 518
symbiotic 8:402f4a307d08 519 This is somewhat redundant with clearSetGoalTransactions(). Should resolve this at some point
symbiotic 8:402f4a307d08 520 */
symbiotic 8:402f4a307d08 521
symbiotic 8:402f4a307d08 522 void LegInterface::clearLegSetGoalStatus()
symbiotic 8:402f4a307d08 523 {
symbiotic 8:402f4a307d08 524 for(int i = 0; i < NUM_LEGS; ++i) {
symbiotic 8:402f4a307d08 525 // Reset leg only if it has not completed
symbiotic 8:402f4a307d08 526 if(!transactionSetGoal[i]->completed()) {
symbiotic 8:402f4a307d08 527 // This causes the transaction to be seen as being completed
symbiotic 8:402f4a307d08 528 transactionSetGoal[i]->clearStatus();
symbiotic 8:402f4a307d08 529 }
symbiotic 8:402f4a307d08 530 }
symbiotic 8:402f4a307d08 531 }
symbiotic 8:402f4a307d08 532
symbiotic 4:5f7d38d0e22d 533 void LegInterface::reset()
symbiotic 4:5f7d38d0e22d 534 {
symbiotic 4:5f7d38d0e22d 535 I2CTransaction::reset();
symbiotic 4:5f7d38d0e22d 536 }
symbiotic 4:5f7d38d0e22d 537
symbiotic 4:5f7d38d0e22d 538 void LegInterface::resetBus()
symbiotic 4:5f7d38d0e22d 539 {
symbiotic 4:5f7d38d0e22d 540 I2CTransaction::resetBus();
symbiotic 4:5f7d38d0e22d 541 }
symbiotic 4:5f7d38d0e22d 542
symbiotic 4:5f7d38d0e22d 543 void LegInterface::cycleBus()
symbiotic 4:5f7d38d0e22d 544 {
symbiotic 4:5f7d38d0e22d 545 I2CTransaction::cycleBus();
symbiotic 6:a52ded837184 546 }
symbiotic 6:a52ded837184 547
symbiotic 6:a52ded837184 548 void LegInterface::clearQueryTransactions()
symbiotic 6:a52ded837184 549 {
symbiotic 6:a52ded837184 550 transactionQueryState[LEG_W]->clearStatus();
symbiotic 6:a52ded837184 551 transactionQueryState[LEG_E]->clearStatus();
symbiotic 6:a52ded837184 552 transactionQueryState[LEG_S]->clearStatus();
symbiotic 6:a52ded837184 553 }
symbiotic 6:a52ded837184 554
symbiotic 6:a52ded837184 555 void LegInterface::clearSetGoalTransactions()
symbiotic 6:a52ded837184 556 {
symbiotic 6:a52ded837184 557 transactionSetGoal[LEG_W]->clearStatus();
symbiotic 6:a52ded837184 558 transactionSetGoal[LEG_E]->clearStatus();
symbiotic 6:a52ded837184 559 transactionSetGoal[LEG_S]->clearStatus();
symbiotic 6:a52ded837184 560 }
symbiotic 6:a52ded837184 561
symbiotic 6:a52ded837184 562
symbiotic 6:a52ded837184 563 /**
symbiotic 6:a52ded837184 564 Initialize the I2C pins and other hardware. If it has already been initialized, then
symbiotic 6:a52ded837184 565 we will re-initialize.
symbiotic 7:c164b4869f74 566
symbiotic 6:a52ded837184 567 @param sda Name of the pin that is used for data
symbiotic 6:a52ded837184 568 @param scl Name of the pin that is used for the clock
symbiotic 6:a52ded837184 569 */
symbiotic 6:a52ded837184 570
symbiotic 6:a52ded837184 571 void LegInterface::initI2C(PinName sda, PinName scl)
symbiotic 6:a52ded837184 572 {
symbiotic 6:a52ded837184 573 I2CTransaction::initI2C(sda, scl);
symbiotic 6:a52ded837184 574 }