No doubt, I need the gyro

I need the gyro to properly detect the rate of change.  I thought I could get away with something like this:

data.GyroCorrectedDegrees_X = (currentAngle - lastCycleAngle) * (1 / data.ChangeInTime.TotalSeconds);

But alas, it doesn't work properly.  Maybe someone sees a math error I don't.  It jitters like me after a twelve pack of Dr. Pepper (Dr. Pepper, if you're out there, I'm looking for sponsorship).

IMG_4628-P0ST

IMG_4633-P0ST

Hopefully Sparkfun will take pity on me tomorrow.

Just incase since I know everyone is smarter than me, here is the source, please someone see a massive error on my part.

while (!ProgrammaticalStop)// && IsUserOnSkateboard())
{
    data = imu.GetImuData(PiDividedBySix, PiDividedByTwo, PiDividedByTwo);// 25 degress
    data.ReturnDegrees = true;

    angle = data.AccelerationCorrected_X;
    data.GyroCorrectedDegrees_X = (angle - oldAngle) * (1 / data.ChangeInTime.TotalSeconds);
    gyro = data.GyroCorrected_X;

    // reduce micromovements by me for testing
    if (angle < 2.5 && angle > -2.5)
        angle = 0;

    error = angle;

    //motor = (ProportionalMultiplier * error) + oldIntegral - ((angle - oldAngle)  * DerivativeMultiplier);
    //motor -= motor * .05;

    //oldIntegral += (motor - oldIntegral) * IntegralMultiplier;
    motor += (ProportionalMultiplier * (error + motor * .025)) + (gyro * DerivativeMultiplier);
    oldAngle = angle;
    
    motor = verifyOverflow((int)motor, 512);

    #region motor setting and moving
    // gotta do this else turning won't work at max speed
    leftMotorPower = rightMotorPower = (int)motor;

    ///* Steering */
    // this should be perportional to relative speed
    //turn = (int)(data.AccelerationCorrectedDegrees_Y * SteeringMultiplier);

    /* Factor in steering with motors*/
    leftMotorPower = -leftMotorPower + turn;
    rightMotorPower = rightMotorPower + turn;

    // how I have the motors wired

    leftMotorPower = verifyOverflow(leftMotorPower / 4,  127);
    rightMotorPower = verifyOverflow(rightMotorPower / 4, 127);

    motorController.SetMotorSpeed(Types.Enums.MotorChannel.A, leftMotorPower);
    motorController.SetMotorSpeed(Types.Enums.MotorChannel.B, rightMotorPower);
    #endregion
    UpdateUserInterface(data, motor, turn, leftMotorPower, rightMotorPower);
}

No comments posted yet.

Post a Comment

Please add 1 and 7 and type the answer here:
  • About
  • Clint Rutkas: I build stuff that will eventually hurt me. I'm your average nerd.
  • Follow Clint on Twitter