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; } }