Tim Heuer has created some very tight stuff for Live Writer such as Flickr4Writer and after sending in a nice little email to the LW team on why I couldn't get a feature that was there in Community Server, within I swear, 15 minutes, Tim had a fix on the totally different end than I expected.

He made SubText ... work like WordPress ... in a few ways.  I gave me one feature I really wanted, another I might use, and is working on one I REALLY want.

Tim, you, are my hero.  I can tag harder now.

SparkFun rules, they are drop shipping me a new IMU unit!

I’m totally going to be buying these guys a drink at Maker Faire.

Time to see if I can’t do some phatty real-time coding while talking to thousands of people and not having it kill someone.  I may get some plywood and do testing inside my pelican case for safety.

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

I'm about 99% sure the gyro on my IMU is dead.  Gotta call up SparkFun on Monday to see what I can do to.  The accelerometers work but this could explain why I never was able to get the skateboard to work.

How did I realize this?  With the increased logging and now visuals on the debugging app I wrote, I came to this conclusion.  To verify it wasn't my app too, I hooked it into SparkFun's application and verified this.  No matter how hard I shook, it didn't radically change.

This could explain why the Kalman filters didn't work since they use data from the gyros.  Also this would explain why the skateboard work before.  I always thought it was sluggish.  It heavily favored the gyro and without a gyro working, it couldn't detect a fall.  I also wonder if this is why the values for my accelerometer fluctuate and need to be reset every so often.

I could "mock" in a gyro I think my saying "old angle" and "new angle" with the change in time.

<clintSwearTime>
Frack, 'eff'in frack frack double mofo frack 'eff'in frack and doo doo and poop.
</clintSwearTime>

I have to ship the skateboard out no matter what on Wednesday if it is to get there.  I could always bring it on the airplane with me.  Wonder if I can't do a gate check.  But god knows this beast looks under an x-ray.

So to recap, I have to fake a gyro in my system in 2-3 days.  Sweet man, real sweet.

I've been reading up on PID controllers and they actually make sense now.  It is a nice Saturday in St. Louis and I'm inside researching PID controllers.  Yes, I'm a nerd.  A very large nerd.  A nerd who's toy isn't playing nicely with him.

So here is the break down of a PID controller as I see it along with how I'll be implementing it.

P = Short term corrections

I = Adds long-term precision

D = This gives you a rough estimate of the velocity (delta position/sample time), which predicts where the position will be in a while.  (quote from PID without a PhD)

The D directly relates to my Gyro.  It measures change in degrees per second.

* images from PID without a PhD too

With only P

With PI

With a full PID system

pseudo code from the PID without a PhD site:

typedef struct { 
  double dState; // Last position input 
  double iState; // Integrator state 
  double iMax, iMin; // Maximum and minimum allowable integrator state 
  double iGain, // integral gain 
        pGain, // proportional gain 
        dGain; // derivative gain 

} SPid; 

double UpdatePID(SPid * pid, double error, double position) {
   double pTerm, dTerm, iTerm; 
   pTerm = pid->pGain * error; // calculate the proportional term 

  // calculate the integral state with appropriate limiting 
  pid->iState += error; 
  if (pid->iState > pid->iMax) 
    pid->iState = pid->iMax; 
  else if (pid->iState < pid->iMin)
    pid->iState = pid->iMin; 

  iTerm = pid->iGain * iState; // calculate the integral term 
  dTerm = pid->dGain * (position - pid->dState); 
  pid->dState = position; 
  return pTerm + iTerm - dTerm; 
}

Here is the current UI iteration.  I added in some sweet visual angle controls I found on code project.

I'll be doing a post about PID controllers shortly.  I think the logic may shift a bit.

There is a water line.  Are you sinking or swimming.  With the skateboard, no matter what I try, I think I'm sinking.  I feel stupid doing this.  Nothing I do seems to fix the issues I'm having.  I also think it has officially gotten to the point where I can say it isn't fun anymore.  Tweak this, tweak that, with very little instant gratification. 

Right now the skateboard acts like some that is bipolar and has a seizure every 2 seconds.

Kalman filter v2 in my opinion is too slow to react.  It may be due to my var's I use to initialize it.

I did some research and found the PID wikipedia page.  I may adapt my system to fit more of this.

Some Pseudo code from the wikipedia page:

start: previous_error = error or 0 if undefined
   error = setpoint - actual_position
   P = Kp * error
   I = I + Ki * error * dt
   D = (Kd / dt) * (error - previous_error)
   output = P + I + D wait(dt)
   goto start

It also has some sweet links, I will read "PID without a PhD" first.  Maybe I'll feel slightly less dumberer and will be upgraded to just dumb.

Airplanes for me are a mixed blessing when it comes to work.  Due to the fact I have no where else to go, no real distractions (aka zero access to my media server), and virtually an unlimited supply of stuff to get done, I get a massive amount done.  But for the skateboard project and another, it semi sucks.  I need access to the skateboard to test out new code segments and I need access to Twitter for the other.  While I have my IMU with me in my book bag, I can't use it on the plane for fear of ending up in Guantanamo Bay since I think I'm the only one on the airplane that deals with electronics.

Anywho, here is what I've gotten done on the skateboard.

• New Kalman filter hopefully implemented with the gyro bias correction.
• Refactored out IMU correction code from the old Kalman filter code to correct the accelerator and gyro for temperature drift.

What I think I may do during some downtime while at the US Imagine Cup finals in LA.

• Try to reduce cross threading.
• Remove the idea of delegates from the Skateboard class and replace it with a boolean
• Add in phidget code
• See if I can't produce a lighter version of the data logging.
• Work on motor control code
• Add in active polling so I can use the WatchDog features (turns it off if computer doesn't respond)
• Correct the code for poling voltage and other features.
• Work on my Twitter app.
• Can't talk more about it other than I'm seriously behind on it.

Using a PD model, with 21 and 22 values (source is on this post), check this out.  This is an oscillation in process.

Nothing too crazy

Looks normal.  Data looks pretty much legit

At about 275 and 550, notice some weirdness?  I DO!!!  At no point should this go past -25 to 25.

HELLO messed up angle ratings.  500 and 1000 ratings?  I'd be doing loops.

An angle rating shouldn't be this crazy and I still feel like a whale is on my chest with a massive hangover (no i haven't done on a bender, as much fun as those are).

damn you.  Tried the reduced GC collection mode and it didn't stop it.

I wonder if the act of doing logging actually is causing this spike.  Heisenberg uncertainty principle could explain.  Storing extra data would cause it to eventually need to claim more.

Yeah, I think I'm pushing it too.

Seriously, in STL, this has happened to me too many times to count.

On this topic, one very interesting thing as I prep for a presentation tonight is could it be the garbage collector firing off causing my timing issues?

If it is, I can do this and it should magically fix it:

GCLatencyMode mode = GCSettings.LatencyMode;
// entering time-critical phase
GCSettings.LatencyMode = GCLatencyMode.LowLatency;

// work work work work and some more work
work();

// restore and play
GCSettings.LatencyMode = mode;

After spending too much quality time in the fetal position, here is the timing from my new logging.  I've tried a rather large sum of things to fix the spikes including switching to a console application.  The problem I have is I have no clue what is causing this spike in CPU processing time.  This makes me believe that it isn't my win32 app at all.  Release mode doesn't make a difference also.

me wonders if it is how I've implemented my threading.  It may be cross threading actually.  I could move everything out of my skateboard class and make it into a quick Win32 app OR add in a lot more logic I wanted abstracted out of the skateboard class into it.  Namely how to kill the loop.  I could add in the phidget code breaking the cross threading.  I can't think of why else every 24 to 26 cycles I'd have this weird spike other than cross threading UI update.  I have a multicore laptop so this shouldn't be happening.

me wonder twice:  Will .1 seconds really make the difference?  Every .25 seconds I'll have a 100 ms lag period.  That brings me to about 75 cycles a second.  My face says I want 8 million cycles a second but my reduced time frame and money already spent says 75 may just work. 

Man, this sucks.  Why can't Vista be a real time operating system.  I don't have time to play with the eBox yet.

Hopefully the improved logging with help me figure out a clever way to beat the oscillation.

I fell to the common flu or something on Sunday night and wanted to die.  So my work as of late has been light since working on a cold concrete floor only made me want to cool off my head on it and fall asleep again.

What I did do was realize a serious problem.  Updating the UI caused a massive spike in processing time.  Normally I'd get about 100 to 111 cycles but on updating the UI, I'd see a massive 100ms lag.  This is very bad.  SO, I created a way to do UI updating when I need to verify data and then data logging when I don't.

I'll have to add in a 3rd condition of "don't track anything" since this is a tad memory greedy over time.

I'm suffering a pretty wicked oscillation problem and I need to solve it.  What is happening is I need a rather large multiplier to get the skateboard to kick back up.  I realized I really didn't have any proper data logging built in so I just added in yet another text box on my diagnostic application.

Now with this data, I can do pretty graphs like this with Excel (yes this is the first time I've ever done a graph with excel:

Why care?  This allows me to possibly see how during the oscillation the major players are acting and see which is possibly causing the root of the cause and how to deal with it.

If anyone can see my flaw, please ping me.  My ProportionalMultiplier and DerivativeMultiplier are about 20 to get stuff to start working.  Stuff that is irrelevant was removed to keep the post short.

public void StartSelfBalancing()
{
    // checks and getting stuff setup, code removed for posting
    
    int leftMotorPower = 0;
    int rightMotorPower = 0;
    int speedOffset = 0;
    int turn = 0;

    double angle = 0;
    double motor = 0;

    while (IsUserOnSkateboard())
    {
        data = imu.GetImuData();

        double gyroDamped = dampenGyro(data.X_GyroCorrectedRadians);
        double Acceleration = data.X_AccelerationCorrectedRadians;

        angle = 0.9 * (angle + gyroDamped * (data.ChangeInTime.TotalSeconds))
            + 0.1 * (Acceleration + speedOffset);

        /* 'motor' is the base value of the motors, independent from turning speeds. */
        motor += dampenMotor(((ProportionalMultiplier * angle) -
                (DerivativeMultiplier * gyroDamped)));

        motor = verifyOverflow((int)motor, 512);

        //speedOffset = (int)(angle * .025); //0.025

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

        ///* Steering commented out for inital balance testing
        /* Factor in steering with motors*/
        leftMotorPower = -leftMotorPower + turn;
        rightMotorPower = rightMotorPower + turn;

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

        motorController.SetMotorSpeed(Types.Enums.MotorChannel.A, leftMotorPower);
        motorController.SetMotorSpeed(Types.Enums.MotorChannel.B, rightMotorPower);

        UpdateUserInterface(data, Angles.RadiansToDegrees(angle), motor, turn, leftMotorPower, rightMotorPower);
    }
    // shut down motors and IMU removed for posting
}

And my big ass Pelican case, god how could I forget how to ship this mofo.  I need to rubber cement in the foam but everything fits.  I should add in a box to carry tools too I think.  I also need to add in a spot for the batteries that are actually in the body of the skateboard right now, I don't want any power on it during transportation.  14 days before I have to ship it out!  And I'll be in LA for 4 of those days for the Microsoft Imagine Cup US final supporting my region's SDI team, from University of Iowa!

I got my deadman switch tester app working.  Basically I got some phidgets with voltage dividers and force sensors and will be putting those directly on the skateboard.  This is my testing app, I learned that my TextLCD display can do 20 char's per line (2 lines, total 40 chars).  When I switch to Windows CE, this will be my eyes instead of giant diagnostic application.

This program really isn't too hard.  It will be coupled with my solution with the skateboard however.

using System;
using System.Text.RegularExpressions;
using System.Windows.Forms;

using Phidgets;
using Phidgets.Events;

namespace PhidgetTester
{
    public partial class Form1 : Form
    {
        TextLCD lcd;
        InterfaceKit ifKit;
        public Form1()
        {
            InitializeComponent();
            lcd = new TextLCD();

            ifKit = new InterfaceKit();
            ifKit.SensorChange += ifKit_SensorChange;
            
            ifKit.open();
            lcd.open();
        }

        void ifKit_SensorChange(object sender, SensorChangeEventArgs e)
        {
            if (e.Index == 0)
            {
                lblValue1.Text = e.Value.ToString();
                pbValue1.Value = e.Value;
            }
            else if(e.Index == 1)
            {
                lblValue2.Text = e.Value.ToString();
                pbValue2.Value = e.Value;
            }
        }

        private void textBox1_TextChanged(object sender, EventArgs e)
        {
            string[] mc = Regex.Split(textBox1.Text, "\r\n");
            if( mc.Length > 0)
            {
                lcd.rows[0].DisplayString = mc[0];
                if (mc.Length > 1)
                    lcd.rows[1].DisplayString = mc[1];
                else
                    lcd.rows[1].DisplayString = "";
            }
        }

        private void btnSoftTimeCheck_Click(object sender, EventArgs e)
        {
            lblNonRealTimeValue1.Text = ifKit.sensors[0].Value.ToString();
            lblNonRealTimeValue2.Text = ifKit.sensors[1].Value.ToString();
        }
    }
}

getting this sucker tuned in is far harder than I could have ever thought.  I've smashed my poor fingers three times already.  I use the term smashed since the skateboard with batteries isn't exactly feather weight.

Code still needs tweaking and tons of improvements but finally I can go to sleep before 4am!

The wonders of diagnostic applications and being ugly and not having everything fully hooked up.  I was iffy on the Kalman filter pulling out correct values but as soon as I talked with Filpe Varela, the original coder of the Kalman filter implementation I'm using, talked me through some of the values and wtf they actually meant, I think I got it!  Once I tuned in my accelerators, the angle estimates started working properly.  In doing this, my headache magically disappeared too.

I actually only need the angle estimate for the X axis too.

I will say doing code with magic numbers, not so much fun but when people help out you, makes life so much easier.  Thanks Filpe for the email response!  Once I track down what the last few numbers I'm not sure on actually mean, I can tune in this mofo perfectly.

I've wasted literally an hour messy around with a section of code I've been attempting to make elegant (aka pretty).

Why is this so important to me besides the possibility of being obsessive compulsive?  I think it makes the code easier to read along with more maintainable.  While you can't reduce the amount of code you repeat 100%, I think you should be able to do it for most instances.  My gut told me this was one of those times and I found a clever solution.  While Dan may be banging his head against the wall since I have another deadline for him ... I think it is well worth it.

So I have 3 Acceleration variables within my ImuData class.  In my Kalman filter class, I had to loop through these vars and do some simple calculations and updating.  This should be done with a function (wow, I know).  The problem is updating vars from a function.  The ref keyword for referencing won't work here, I could use an out but then I'd have far too many repeats for initializing the variables going in the function.  So what did I do?

I fixed the ImuData class.  Instead of all member variables, I created a base arrays. 

An Example, I may still rename the variables to type_Axis instead of Axis_type:

public double X_Acceleration
{
    get { return Acceleration[0]; }
    set { Acceleration[0] = value; }
}
public double Y_Acceleration
{
    get { return Acceleration[1]; }
    set { Acceleration[1] = value; }
}
public double Z_Acceleration
{
    get { return Acceleration[2]; }
    set { Acceleration[2] = value; }
}

public double[] Acceleration
{
    get { return _acceleration; }
    set { _acceleration = value; }
}
private double[] _acceleration = { 0, 0, 0 };

which lets me do this in the kalman filter class:

private void applyCorrectionFactorsForGyro(int index)
{
    data.GyroVoltCorrection[index] = ((2.5 / data.GyroVolt[index]) * 1024) / 5;

    data.Gyro[index] *= data.GyroVoltCorrection[index];
    data.GyroTemp[index] *= data.GyroVoltCorrection[index];
}

mmmm, pretty and no code repeat.  (and yes, that picture does freak me out but it is a decent ad)

I tracked down where the original Kalman filter code came from and I'm pretty sure they did it incorrect after looking at the original source.  Their implementation didn't appear to work properly.  After seeing the original, I think they left out some key parts.

Plus can someone port code then give that ported code a GPL license and claim copyright on it without giving credit back to the author?

Anyways, I'll be knee deep in C code for a bit.

My angle filter didn't work as expected, it had a high and low pass filter built in but it had some drifting problems where the angle got bigger and bigger.

Why am I not freaking out about this?  I found some sweet c# Kalman filter code by Adriaan Swanepoel.  It has a few bugs in it that I'm going to report back on.  Plus after asking, he switched the license to the BSD license for me instead of the GPL.

Clint's view on a Kalman filter:

It is Magic, but magic that works and has been proven.  So going to see if I can't use it.  I'm adding in a boolean to say useKalmanFilter.  If enabled, I'll use it instead of the high / low pass filter.

So I've decided I need to ship this out by the 28th of April for Maker Faire.  I need to swap out the filter and tweak the adjustment variables.  I also need to add in code for the deadman switches that arrived while I was in South Dakota.  If I do have time, I'll get this working on Windows CE too else a laptop will be strapped to the skateboard somehow.

I'm shocked at how long the batteries last too.

I finished up the source for the most part last night.  I'm in South Dakota currently so I have to actually alter the program slightly to work without the motor controller present which actually would be a good idea.

I did test it a few times on the skateboard last night too but it did not work properly.  I think the solution of testing this out without the hardware present is the way to go.

One interesting bug I did find was with the Speed Controller was it returning data.  I'd do a query and it wouldn't always give me the result right away.  Now I've come up with a few possible ways to deal with this.  One is to passively get data and use an event here.  Based on the return result I update proper property.  It will be slightly delayed but most of the data I'd be querying doesn't need an instant response.  I just need to verify does the battery have enough juice right off the bat and that isn't too hard of a problem to solve.