void ContainerStacker::DriveToPoint() {
// SmartDashboard::PutNumber("TargetPosition", m_targetHeight);
int position = CalculatePos();
double error = position - GetCurrentPos();
// SmartDashboard::PutNumber("error, can", error);
// SmartDashboard::PutNumber("current, can", GetCurrentPos());
// SmartDashboard::PutNumber("point, can", position);
CheckSensors();
if(error > 50){
if(error > 1000) {
containerLift->Set(-1);
}
else {
containerLift->Set(-error/1000);
}
}
else if(error < -50) {
if(error < -1000) {
containerLift->Set(1);
}
else {
containerLift->Set(-error/1000);
}
}
else {
containerLift->Set(0);
}
}
void ContainerStacker::DriveToPoint(int point) {
double error = point - GetCurrentPos();
// SmartDashboard::PutNumber("error1, can", error);
// SmartDashboard::PutNumber("current1, can", GetCurrentPos());
// SmartDashboard::PutNumber("point1, can", point);
CheckSensors();
if(error > 50){
if(error > 1000) {
containerLift->Set(-1);
}
else {
containerLift->Set(-error/1000);
}
}
else if(error < -50) {
if(error < -1000) {
containerLift->Set(1);
}
else {
containerLift->Set(-error/1000);
}
}
else {
containerLift->Set(0);
}
}
String Tracker::GetCheckSensorString()const
{
String strRet;
int iCheckSensor = CheckSensors();
if (iCheckSensor == 0)
strRet = "OK";
else
{
if (iCheckSensor & SENSOR_NoMag)
strRet += "SENSOR_NoMag;";
if (iCheckSensor & SENSOR_NoGryo)
strRet += "SENSOR_NoGryo;";
if (iCheckSensor & SENSOR_GryoTooSlow)
strRet += "SENSOR_GryoTooSlow;";
if (iCheckSensor & SENSOR_NoMag)
strRet += "SENSOR_NoAccel;";
if (iCheckSensor & SENSOR_NoAccel)
strRet += "SENSOR_AccelTooSlow;";
if (iCheckSensor & SENSOR_AccelTooSlow)
strRet += "SENSOR_NoMag;";
}
return strRet;
}
