private static void RampUpServo(Servo servo, double desiredSpeed)
{
var currentSpeed = servo.Degree;
if (desiredSpeed < currentSpeed)
{
if (currentSpeed - desiredSpeed < ServoStep)
{
servo.Degree = desiredSpeed;
}
else
{
servo.Degree = currentSpeed - ServoStep;
}
}
else if (desiredSpeed > currentSpeed)
{
if (desiredSpeed - currentSpeed < ServoStep)
{
servo.Degree = desiredSpeed;
}
else
{
servo.Degree = currentSpeed + ServoStep;
}
}
}
public Serb(Cpu.Pin leftServoPin, Cpu.Pin righServoPin)
{
_leftServo = new Servo(leftServoPin, false);
_rightServo = new Servo(righServoPin, true);
_leftServo.Degree = LEFTSERVSTOP;
_rightServo.Degree = RIGHTSERVSTOP;
_timer = new Timer(UpdateMotors, null, 0, 15);
}