Andrew Fagg / SIPPCLegInterface

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

Diff: LegInterface.cpp

Revision:
5:86f1cd6657de
Parent:
4:5f7d38d0e22d
Child:
6:a52ded837184
diff -r 5f7d38d0e22d -r 86f1cd6657de LegInterface.cpp
--- a/LegInterface.cpp	Tue Jun 10 19:21:21 2014 +0000
+++ b/LegInterface.cpp	Fri Jun 20 06:10:34 2014 +0000
@@ -190,7 +190,7 @@
 
     // Transaction complete: copy the state of the legs
     for(int i = 0; i < NUM_LEGS; ++i) {
-        liftPosition[i] = this->legState[i].lift.pos / LIFT_TICKS_PER_METER;
+        liftPosition[i] = this->legState[i].lift.pos / LIFT_TICKS_PER_METER - LIFT_OFFSET;
         liftVelocity[i] = this->legState[i].lift.vel / LIFT_TICKS_PER_METER;
         wheelPosition[i] = this->legState[i].wheel.pos / TICKS_PER_METER;
         wheelVelocity[i] = this->legState[i].wheel.vel / TICKS_PER_METER;
@@ -305,7 +305,7 @@
 
     for(int i = 0; i < NUM_LEGS; ++i) {
         wheel[i] = (int32_t) (wheelVel[i] * TICKS_PER_METER);
-        lift[i] = (int32_t) (liftPos[i] * LIFT_TICKS_PER_METER);
+        lift[i] = (int32_t) ((liftPos[i] + LIFT_OFFSET) * LIFT_TICKS_PER_METER);
     }
 
     return setLegGoal(lift, wheel);