I did see one problem:
The "if" statement in your step code looks like:
if y = 304; {
vspeed = 0;
//set an alarm
}
That semicolon should not be there. It may very well be screwing up how the program interprets the code, so that the vspeed is getting set to 0 regardless of the y value.