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).
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);
}