Reply To: Preparations for GyroBot

[Jacek S]

Hi Anders,
I mean labview block. I’m trying to translate block sequence from original program to c# code.
I spend many hours during weekend but still with no success. Robot is far from balancing.
Still don’t know where is diffrence. Two points where I’m not sure is gyro sensor rate read and “unregulated motor”.
“Unregulated motor” in my program is sequence: motor.On() (if power < 0) motor.Reverse and motor.SetPower(power).
The oryginal program is very tricky and depends strongly on enviroment and even on battery level.

Fragment of my program(balance method):

        private void Balance()
        {

            var lastSumMotor = sumMotor;
            sumMotor = leftMotor.GetTachoCount() + rightMotor.GetTachoCount();

            var diffMotor = (sumMotor - lastSumMotor);
            rateMotor = 0.75 * rateMotor + 0.25 * (diffMotor / avgLoopTimer);
            angleMotor = moveMotor * avgLoopTimer + angleMotor + diffMotor;

            var gyroRead = (double)gyro.Read();
            rateGyro = gyroRead - offsetGyro;
            offsetGyro = 0.001 * gyroRead + 0.999 * offsetGyro;
            angleGyro = angleGyro + avgLoopTimer * rateGyro;

            motorPower = rateGyro * GYRO_RATE + angleGyro * GYRO_ANGLE + rateMotor * MOTOR_RATE
                + angleMotor * MOTOR_POS;

            if (motorPower < 0)
            {
                motorPower *= -1d;
                if (!leftMotor.Reverse) rightMotor.Reverse = leftMotor.Reverse = true;
            }
            else
            {
                if (leftMotor.Reverse) rightMotor.Reverse = leftMotor.Reverse = false;
            }

            if (motorPower > 100d) motorPower = 100d;

            leftMotor.SetPower((byte)motorPower);
            rightMotor.SetPower((byte)motorPower);
        }

Jacek