Manualshelf

Datasheet

ManualsBrandsADAFRUIT ManualsDev KitsFitting aid
31323334353637383940
Following Objects
The object following behavior is implemented in the FollowBlock function. FollowBlock uses just
proportional control. But we have two measurements (size and pan position) and two outputs (left
and right drive motors).
The size (block height times width) gives us a rough idea of how far away the object is and we use
that to calculate the 'forwardSpeed'. This makes the robot slow down as it approaches the object. If
the object appears larger than the setpoint value, forwardSpeed will become negative and the robot
};
// ServoLoop Constructor
ServoLoop::ServoLoop(int32_t proportionalGain, int32_t derivativeGain)
{
m_pos = RCS_CENTER_POS;
m_proportionalGain = proportionalGain;
m_derivativeGain = derivativeGain;
m_prevError = 0x80000000L;
}
// ServoLoop Update
// Calculates new output based on the measured
// error and the current state.
void ServoLoop::update(int32_t error)
{
long int velocity;
char buf[32];
if (m_prevError!=0x80000000)
{
velocity = (error*m_proportionalGain + (error - m_prevError)*m_derivativeGain)>>10;
m_pos += velocity;
if (m_pos>RCS_MAX_POS)
{
m_pos = RCS_MAX_POS;
}
else if (m_pos<RCS_MIN_POS)
{
m_pos = RCS_MIN_POS;
}
}
m_prevError = error;
}
// End Servo Loop Class
//---------------------------------------