TEST(CANTalonTest, QuickTest) {
double throttle = 0.1;
CANTalon talon(deviceId);
talon.SetControlMode(CANSpeedController::kPercentVbus);
talon.EnableControl();
talon.Set(throttle);
Wait(0.25);
EXPECT_NEAR(talon.Get(), throttle, 5e-3);
talon.Set(-throttle);
Wait(0.25);
EXPECT_NEAR(talon.Get(), -throttle, 5e-3);
talon.Disable();
Wait(0.1);
EXPECT_FLOAT_EQ(talon.Get(), 0.0);
}
TEST(CANTalonTest, DISABLED_PositionModeWorks) {
CANTalon talon(deviceId);
talon.SetFeedbackDevice(CANTalon::AnalogPot);
talon.SetControlMode(CANSpeedController::kPosition);
Wait(0.1);
double p = 2;
double i = 0.00;
double d = 0.00;
Wait(0.2);
talon.SetControlMode(CANSpeedController::kPosition);
talon.SetFeedbackDevice(CANTalon::AnalogPot);
talon.SetPID(p, i, d);
Wait(0.2);
talon.Set(100);
Wait(100);
talon.Disable();
EXPECT_NEAR(talon.Get(), 500, 1000);
}
TEST(CANTalonTest, SetGetPID) {
// Tests that we can actually set and get PID values as intended.
CANTalon talon(deviceId);
double p = 0.05, i = 0.098, d = 1.23;
talon.SetPID(p, i, d);
// Wait(0.03);
EXPECT_NEAR(p, talon.GetP(), 1e-5);
EXPECT_NEAR(i, talon.GetI(), 1e-5);
EXPECT_NEAR(d, talon.GetD(), 1e-5);
// Test with new values in case the talon was already set to the previous
// ones.
p = 0.15;
i = 0.198;
d = 1.03;
talon.SetPID(p, i, d);
// Wait(0.03);
EXPECT_NEAR(p, talon.GetP(), 1e-5);
EXPECT_NEAR(i, talon.GetI(), 1e-5);
EXPECT_NEAR(d, talon.GetD(), 1e-5);
}
void test_trackJacobians() {
const int MAXNS = NUMSTATE(WmrModel::MAXNF);
//make WmrModel object
WmrModel mdl;
Real state[MAXNS];
talon(mdl,state,0);
//get from WmrModel
const int nt = mdl.get_nt();
//terrain
SurfaceVector surfs;
flat(surfs);
//ramp(surfs);
//grid(surfs, ResourceDir() + std::string("gridsurfdata.txt") );
//convert state to homogeneous transforms
HomogeneousTransform HT_parent[WmrModel::MAXNF + WmrModel::MAXNW];
HomogeneousTransform HT_world[WmrModel::MAXNF + WmrModel::MAXNW];
stateToHT(mdl,state,HT_parent,HT_world);
//contact geometry
TrackContactGeom tcontacts[WmrModel::MAXNW];
sub_initTrackContactGeom(mdl, tcontacts);
ContactGeom* contacts = static_cast<ContactGeom*>(tcontacts);
updateModelContactGeom(mdl, surfs, HT_world, mdl.min_npic, contacts);
//number of points in contact
int npic = 0;
for (int tno = 0; tno < nt; tno++) {
npic += logicalCount(tcontacts[tno].get_np(), tcontacts[tno].incontact);
}
//allocate matrix
const int MAXNV = NUMQVEL(WmrModel::MAXNF);
const int MAXNP = WmrModel::MAXNT * ContactGeom::MAXNP; //max number of contact points
Real A[3*MAXNP * MAXNV];
trackJacobians(mdl, HT_world, tcontacts, A);
//DEBUGGING, print
int ncc = 3*npic; //number of contact constraints
int nv = NUMQVEL(mdl.get_nf());
std::cout << "A=\n";
printMatReal(ncc,nv,A,-1,-1);
std::cout << std::endl;
if (1) {
//time it
int n= (int) 1e6;
timeval t0, t1;
gettimeofday(&t0, NULL);
for (int i=0; i<n; i++) {
trackJacobians(mdl, HT_world, tcontacts, A);
}
gettimeofday(&t1, NULL);
std::cout << "trackJacobians()\n";
std::cout << "iterations: " << (Real) n << std::endl;
std::cout << "clock (sec): " << tosec(t1)-tosec(t0) << std::endl;
}
}
