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