Andrew Fagg / SIPPCLegInterface

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

LegInterface.h@7:c164b4869f74, 2014-11-11 (annotated)

Committer:
symbiotic
Date:
Tue Nov 11 23:01:02 2014 +0000
Revision:
7:c164b4869f74
Parent:
6:a52ded837184
Child:
8:402f4a307d08
Added error recovery

Who changed what in which revision?

UserRevisionLine numberNew contents of line
symbiotic 0:346370254254 1 #ifndef LEGINTERFACE_H
symbiotic 0:346370254254 2 #define LEGINTERFACE_H
symbiotic 0:346370254254 3 #include "I2CTransaction.h"
symbiotic 0:346370254254 4 #include "leg_packets.h"
symbiotic 7:c164b4869f74 5 #include "robot_config.h"
symbiotic 4:5f7d38d0e22d 6
symbiotic 4:5f7d38d0e22d 7
symbiotic 4:5f7d38d0e22d 8
symbiotic 4:5f7d38d0e22d 9
symbiotic 4:5f7d38d0e22d 10
symbiotic 0:346370254254 11 class LegInterface
symbiotic 0:346370254254 12 {
symbiotic 0:346370254254 13 public:
symbiotic 0:346370254254 14 // Indices for the different legs
symbiotic 0:346370254254 15 enum Leg {LEG_W=0, LEG_E, LEG_S, NUM_LEGS};
symbiotic 0:346370254254 16
symbiotic 0:346370254254 17 // An array of I2C addresses indexable by enum Leg
symbiotic 0:346370254254 18 static const int LegAddress[];
symbiotic 6:a52ded837184 19
symbiotic 2:17c7a02f8401 20 // Encoder to wheel
symbiotic 2:17c7a02f8401 21 static const float TICKS_PER_METER = 52633.97;
symbiotic 6:a52ded837184 22
symbiotic 2:17c7a02f8401 23 // Angle between front legs
symbiotic 2:17c7a02f8401 24 static const float FRONT_LEG_ANGLE = 130; // Degrees
symbiotic 6:a52ded837184 25
symbiotic 2:17c7a02f8401 26 // Distance from center of robot to wheel
symbiotic 7:c164b4869f74 27 //static const float ROBOT_RADIUS = 0.4702; // m
symbiotic 6:a52ded837184 28
symbiotic 2:17c7a02f8401 29 // Number of A2D ticks per meter. TODO: is 4096 right?
symbiotic 2:17c7a02f8401 30 static const float LIFT_TICKS_PER_METER = 4096 / 0.1;
symbiotic 0:346370254254 31
symbiotic 5:86f1cd6657de 32 // Offset between measured height and true height (m)
symbiotic 7:c164b4869f74 33 //static const float LIFT_OFFSET = 0.01;
symbiotic 7:c164b4869f74 34 #if (ROBOT_CONFIG_LEG_SYMMETRY == 0)
symbiotic 7:c164b4869f74 35 static const float LIFT_OFFSET = 0.015;
symbiotic 7:c164b4869f74 36 #else
symbiotic 7:c164b4869f74 37 static const float LIFT_OFFSET = -0.005;
symbiotic 7:c164b4869f74 38 #endif
symbiotic 0:346370254254 39
symbiotic 0:346370254254 40 LegInterface(Serial *pc = NULL);
symbiotic 0:346370254254 41
symbiotic 0:346370254254 42 bool sendEstop(bool estop);
symbiotic 0:346370254254 43 bool queryStateInitiate();
symbiotic 2:17c7a02f8401 44 bool queryStateCompleted();
symbiotic 6:a52ded837184 45 bool queryStateSuccess();
symbiotic 7:c164b4869f74 46 void queryStateReportMagic();
symbiotic 6:a52ded837184 47 bool queryEstopCompleted();
symbiotic 6:a52ded837184 48 bool queryEstopSuccess();
symbiotic 2:17c7a02f8401 49 bool queryStateWaitForCompletion(int timeout = 1000);
symbiotic 0:346370254254 50 bool queryStateCopy(LegState_t legState[NUM_LEGS]);
symbiotic 6:a52ded837184 51 bool queryStateCopy(float liftPosition[NUM_LEGS], float liftVelocity[NUM_LEGS], float wheelPosition[NUM_LEGS], float wheelVelocity[NUM_LEGS], bool cliff[NUM_LEGS], bool limit[NUM_LEGS], bool estop[NUM_LEGS], uint8_t magic[NUM_LEGS]);
symbiotic 0:346370254254 52 void displayLegState(LegState_t *legState);
symbiotic 3:8e7471af3453 53 void queryStateTest(Serial *pc);
symbiotic 0:346370254254 54 bool queryLegParams(Leg leg, LegControlParams_t *liftParams, LegControlParams_t *wheelParams);
symbiotic 0:346370254254 55 void displayParams(LegControlParams_t *params);
symbiotic 0:346370254254 56 bool setLegParams(Leg leg, LegControlParams_t *liftParams, LegControlParams_t *wheelParams);
symbiotic 0:346370254254 57 bool setLegGoal(int32_t liftPos[NUM_LEGS], int32_t wheelVel[NUM_LEGS]);
symbiotic 1:d3abc0577ebc 58 bool setLegGoal(float liftPos[NUM_LEGS], float wheelVel[NUM_LEGS]);
symbiotic 6:a52ded837184 59 bool getLegGoalCompleted();
symbiotic 6:a52ded837184 60 bool getLegGoalSuccess();
symbiotic 0:346370254254 61 void displayStatus();
symbiotic 6:a52ded837184 62 //bool getLegGoalStatus();
symbiotic 6:a52ded837184 63 void clearQueryTransactions();
symbiotic 6:a52ded837184 64 void clearSetGoalTransactions();
symbiotic 4:5f7d38d0e22d 65 static void reset();
symbiotic 4:5f7d38d0e22d 66 static void resetBus();
symbiotic 4:5f7d38d0e22d 67 static void cycleBus();
symbiotic 6:a52ded837184 68 static void initI2C(PinName sda, PinName scl);
symbiotic 0:346370254254 69
symbiotic 0:346370254254 70 private:
symbiotic 0:346370254254 71 I2CTransaction *transactionEstop[NUM_LEGS];
symbiotic 0:346370254254 72 LegPacket_t legPacketEstop;
symbiotic 0:346370254254 73
symbiotic 0:346370254254 74 I2CTransaction *transactionQueryState[NUM_LEGS];
symbiotic 0:346370254254 75 LegPacket_t legPacketQuery;
symbiotic 0:346370254254 76 LegState_t legState[NUM_LEGS];
symbiotic 0:346370254254 77
symbiotic 0:346370254254 78 I2CTransaction *transactionQueryLiftParams[NUM_LEGS];
symbiotic 0:346370254254 79 I2CTransaction *transactionQueryWheelParams[NUM_LEGS];
symbiotic 0:346370254254 80 LegPacket_t legPacketQueryLiftParams;
symbiotic 0:346370254254 81 LegPacket_t legPacketQueryWheelParams;
symbiotic 0:346370254254 82 LegControlParams_t legLiftParams;
symbiotic 0:346370254254 83 LegControlParams_t legWheelParams;
symbiotic 0:346370254254 84
symbiotic 0:346370254254 85 I2CTransaction *transactionSetLiftParams[NUM_LEGS];
symbiotic 0:346370254254 86 I2CTransaction *transactionSetWheelParams[NUM_LEGS];
symbiotic 0:346370254254 87 LegPacket_t legPacketSetLiftParams;
symbiotic 0:346370254254 88 LegPacket_t legPacketSetWheelParams;
symbiotic 6:a52ded837184 89
symbiotic 0:346370254254 90 I2CTransaction *transactionSetGoal[NUM_LEGS];
symbiotic 0:346370254254 91 LegPacket_t legPacketSetGoal[NUM_LEGS];
symbiotic 0:346370254254 92 Serial *pc;
symbiotic 0:346370254254 93
symbiotic 0:346370254254 94 };
symbiotic 0:346370254254 95
symbiotic 0:346370254254 96 #endif