Gravity compensation

1. Write an option for our mode
2. Calibrate joint angles (zeros + joint limit) (might have to be done in bldc)
3. Servo loop
   1. 1. 1. readposition
      2. 2. calcposition
      3. 3. gravityCompensation
      4. 4. scale down the gravity comp to loadtorque command
      5. 5. apply base force

some constants in control.cpp

// BASE MOTOR CONSTANTS

const float KDAC_BASE[3] = {10.0/4096.0, 10.0/4096.0, 5.0/4096.0}; // DAC gain, V/count

const float KAMP_BASE[3] = {2.0, 2.0, 2.0}; // amplifier gain, A/V

#define KTORQUE_BASE 0.0727 // motor torque constant, N.m./A

#define limitDAC2 1500

#define ADCFACTOR 10.0/4096.0

#define EL1BIAS? 0

float gearRatioBase[3] = {120.0,100.0,50.0};

#define GRAVITY 9.8

#define KDAC 0.00244

#define KAMP 1.00 // 01/19/05, PST changed all amp. gain to 1.00