void CCenterPage::DoDataExchange(CDataExchange* pDX)
{
CPropertyPage::DoDataExchange(pDX);
//{{AFX_DATA_MAP(CCenterPage)
DDX_Text(pDX, IDC_STATICX, m_StaticX);
DDX_Text(pDX, IDC_STATICY, m_StaticY);
DDX_Text(pDX, IDC_STATICZ, m_StaticZ);
DDX_Text(pDX, IDC_XPAN, m_fXpos);
DDX_Text(pDX, IDC_YPAN, m_fYpos);
DDX_Text(pDX, IDC_ZPAN, m_fZpos);
//}}AFX_DATA_MAP
if (pDX->m_bSaveAndValidate)
{
if (!theApp.GetSettings()->IsMetric())
{
m_fMetricX=FeetToMeters(m_fXpos);
m_fMetricY=FeetToMeters(m_fYpos);
m_fMetricZ=FeetToMeters(m_fZpos);
}
else
{
m_fMetricX=m_fXpos;
m_fMetricY=m_fYpos;
m_fMetricZ=m_fZpos;
}
}
}
bool FGWaypoint::Run(void )
{
double target_latitude = target_latitude_pNode->getDoubleValue() * target_latitude_unit;
double target_longitude = target_longitude_pNode->getDoubleValue() * target_longitude_unit;
double source_latitude = source_latitude_pNode->getDoubleValue() * source_latitude_unit;
double source_longitude = source_longitude_pNode->getDoubleValue() * source_longitude_unit;
FGLocation source(source_longitude, source_latitude, radius);
if (WaypointType == eHeading) { // Calculate Heading
double heading_to_waypoint_rad = source.GetHeadingTo(target_longitude,
target_latitude);
double heading_to_waypoint = 0;
if (eUnit == eDeg) heading_to_waypoint = heading_to_waypoint_rad * radtodeg;
else heading_to_waypoint = heading_to_waypoint_rad;
Output = heading_to_waypoint;
} else { // Calculate Distance
double wp_distance = source.GetDistanceTo(target_longitude, target_latitude);
if (eUnit == eMeters) {
Output = FeetToMeters(wp_distance);
} else {
Output = wp_distance;
}
}
Clip();
if (IsOutput) SetOutput();
return true;
}
//Figure out what interval of tick to use. TempScale is in m/cm
//We try to calculate
//a scale for the ruler to the nearest multiple of 5. The idea is that
//if you have 15.0 meters/cm then your scale would be 1 cm exactly.
//if you have 15.7 meters/cm your scale would be slightly larger then
//one cm and your gradations would still be 15 meters.
void CRuler::GetReasonableIncrements(float fTempScale,float *pfUnits)
{
if (!theApp.GetSettings()->IsMetric())
{
fTempScale=MetersToFeet(fTempScale)*CMPERINCH;
}
//First see if we have some sort of reasonable scale (ie, divisible by 5)
//in which case we return that unit.
int iRounded;
iRounded=(int)((fTempScale+0.499f)/5.0f); //round a little
*pfUnits=(float)(iRounded*5);
if (!theApp.GetSettings()->IsMetric())
{
*pfUnits=FeetToMeters(*pfUnits)/2;
}
if (*pfUnits<5.0f)
{
*pfUnits=5.0f;
}
}
void ElevatorControl::Update()
{
m_robot->Lock();
//printf("elevator: %f\n",m_robot->GetElevatorHeightValue());
// control loop toggle
if (!m_robot->controlLoopsOn)
{
m_robot->SetElevatorMotor(m_robot->elevatorPower);
m_robot->Unlock();
return;
}
// checking if at hard stops
double max_up = FeetToMeters(m_csvReader->GetValue("ELEVATOR_UP_HEIGHT_FEET"));
double max_down = 0;
if (m_robot->elevatorGoal <= max_down)
m_robot->elevatorGoal = max_down;
if (m_robot->elevatorGoal >= max_up)
m_robot->elevatorGoal = max_up;
double elevator_height = m_robot->GetElevatorHeightValue();
double height_error = m_robot->elevatorGoal - elevator_height;
double height_deriv = elevator_height - m_last_elevator_height;
m_robot->elevatorVelocity = height_deriv / m_robot->dt;
m_elevator_integral += height_error;
//printf("Elevator Error %f ", height_error);
double kP;
double kI;
double kD;
if (height_error >= 0 && m_robot->GetSecondStageLimitSwitch()) {
kP = m_csvReader->GetValue("KP_ELEVATOR_UP_SECOND_DOWN");
kI = m_csvReader->GetValue("KI_ELEVATOR_UP_SECOND_DOWN");
kD = m_csvReader->GetValue("KD_ELEVATOR_UP_SECOND_DOWN");
} else if (height_error > 0) {
kP = m_csvReader->GetValue("KP_ELEVATOR_UP_SECOND_UP");
kI = m_csvReader->GetValue("KI_ELEVATOR_UP_SECOND_UP");
kD = m_csvReader->GetValue("KD_ELEVATOR_UP_SECOND_UP");
} else if (height_error <= 0 && m_robot->GetSecondStageLimitSwitch()) {
kP = m_csvReader->GetValue("KP_ELEVATOR_DOWN_SECOND_DOWN");
kI = m_csvReader->GetValue("KI_ELEVATOR_DOWN_SECOND_DOWN");
kD = m_csvReader->GetValue("KD_ELEVATOR_DOWN_SECOND_DOWN");
} else {
kP = m_csvReader->GetValue("KP_ELEVATOR_DOWN_SECOND_UP");
kI = m_csvReader->GetValue("KI_ELEVATOR_DOWN_SECOND_UP");
kD = m_csvReader->GetValue("KD_ELEVATOR_DOWN_SECOND_UP");
}
double power = kP * height_error + kD * height_deriv;
if (kI * m_elevator_integral > 1)
m_elevator_integral = 1/kI;
else if (kI * m_elevator_integral < -1)
m_elevator_integral = -1/kI;
power += kI * m_elevator_integral;
if (m_robot->isZeroingElevator) {
// If the current height is close to the point where it would
// stop manually apply current to make it go down until it has
// zeroed. Resets encoder if it thinks it is at negative height
//printf("Trying to zero\n");
if (elevator_height <= InchesToMeters(1) || power == 0) {
//printf("Force driving down \n");
power = -.2;
if (elevator_height < 0)
m_robot->ResetElevatorEncoder();
}
// Makes sure both stages are down, then resets the elevator encoderr
// Added a check to make sure it's above 6 inches, otherwise the force from
// the downward acceleration can trip the carriage limit switch
if (m_robot->GetCarriageLimitSwitch() && m_robot->GetSecondStageLimitSwitch() && elevator_height<=InchesToMeters(6)) {
// printf("It thinks it's done \n");
m_robot->ResetElevatorEncoder();
m_robot->isZeroingElevator = false;
}
} else if (m_robot->GetCarriageLimitSwitch() && !m_robot->isZeroingElevator
&& m_robot->GetSecondStageLimitSwitch() && elevator_height<=InchesToMeters(6) && power < 0) {
// printf("Resetting the elevator encoder \n");
power = 0;
elevator_height = 0;
m_robot->ResetElevatorEncoder();
} else if (elevator_height >= max_up && !m_robot->isZeroingElevator && power > 0) {
power = 0;
elevator_height = max_up;
}
//printf("elevator speed fraction: %f\n",m_robot->elevatorSpeedFraction);
m_robot->SetElevatorMotor(power);
m_robot->Unlock();
m_last_elevator_height = elevator_height;
}
void CRuler::OnZoom5000I()
{
if (m_View!=NULL)
m_View->SetExactView(NAN,NAN,FeetToMeters(5000.0f)/CMPERINCH,NAN,NAN,NAN);
}
