Gravity compensation
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