Does this help? The problem is in the DriveWheel::calibrate() method.
DriveWheel.h
Code:
#ifndef DRIVEWHEEL_H
#define DRIVEWHEEL_H
#include "WPILib.h"
class DriveWheel
{
CANJaguar driveMotor;
CANJaguar steerMotor;
DigitalInput limitSw;
bool calibrated;
public:
DriveWheel(UINT8 driveID, UINT8 steerID, UINT8 limitID)
: driveMotor(driveID, CANJaguar::kPercentVbus),
steerMotor(steerID, CANJaguar::kPosition),
limitSw(limitID),
calibrated(false)
{
}
void init();
void calibrate(int clock);
void setValues(double drive, double rotation);
};
#endif
DriveWheel.cpp
Code:
#include "DriveWheel.h"
#define LIMIT_ACTIVE 0
void DriveWheel::calibrate(int clock)
{
int limit = limitSw.Get();
printf("limit %d\n", limit);
if (clock == 0)
{
printf("start calibrate\n");
if (limit == LIMIT_ACTIVE)
{
calibrated = true;
}
else
{
steerMotor.ChangeControlMode(CANJaguar::kPercentVbus);
steerMotor.EnableControl(0);
steerMotor.Set(0.1);
}
}
else if (!calibrated)
{
if (limit == LIMIT_ACTIVE)
{
steerMotor.Set(0);
calibrated = true;
init();
}
}
}
void DriveWheel::init()
{
steerMotor.ChangeControlMode(CANJaguar::kPosition);
steerMotor.SetPID(2000, 0.1, 0);
steerMotor.SetPositionReference(CANJaguar::kPosRef_QuadEncoder);
steerMotor.ConfigEncoderCodesPerRev(250);
steerMotor.EnableControl();
}
void DriveWheel::setValues(double drive, double rotation)
{
driveMotor.Set(drive);
steerMotor.Set(rotation);
}