Code Assistance

[Sachiel7]

Hey all!
This is my first attempt at writing some code for our autonomous (wow! Were getting off to a late start (::looks at calendar: 4 days til ship! )
Anyway, I'd like everyone to browse through and offer some comments.
Now, this isn't the *best* code in the world, I mainly tried to writeup everything into one packet, so some stuff might be messy, and I know I can remove a few variables, but I wrote them in for neatness's sake.
Here it is!:

//Init all our Variables
int time=0; //define a counter in the beginning
int seconds=0; //set up a variable for the time in seconds too! 
int step=0; //Define our steps too!
int tracking = 0; //Define our tracking watcher
int tea_time = 0; //A temporary time holder :) Its Tea time!
int start_pos = 0; //Field startup Position
int left_sw_hist = 0; //Left Switch History
int rght_sw_hist = 0; //Right Switch History
int fwd_spd = 175; //Tracking forward speed


//Maintain our start position data from the RC
if (rc_dig_in11 == 1)
{
start_pos = 1;
}
if (rc_dig_in12 == 1)
{
start_pos = 2;
}


time++; //Increment the time!
seconds = time / 1000; //Get the time in seconds!

//Reset all drive/arm variables
pwm11 = pwm12 = 127;
relay2_fwd = relay3_fwd = relay4_fwd = 0;

//Set up the Pneumatics to keep the charge
  relay1_fwd = !rc_dig_in01; //Run the pump if the system is low on pressure.//
  relay1_rev =0;


if (step == 0) //Drive out onto the field a bit
  {
  pwm11 = pwm12 == 150;
   if (time == 500)
   {
   step++;
   }
  }

if(step==1) //Begin the track sequence, track the tetra until contact
  {
   camera_init(45,19,121); //Init the Camera with the calibration values
   camera_auto_servo(1); //Turn on the Auto Servo Feature
   camera_find_color(GREEN); //Seek Green
	if (rc_dig_in02 == 1 || rc_dig_in03 == 1) //If one of the skirt switches trip, go to the next step.
	{
	 if (rc_dig_in02 == 1) //Record Left History for the next step
         {
	 left_sw_hist = 1;
	 }
	 if (rc_dig_in03 == 1) //Record right history for the next step
	 {
	 rght_sw_hist = 1;
	 }
	tea_time = time;
	step++;
	}
   }

if (step==2) //Align the tetra with the "grabber"
  {

  if (left_sw_hist == 1 && rght_sw_hist == 1) 
  {
  left_sw_hist = 0;
  rght_sw_hist = 0;  
  step++;
  }
  if (left_sw_hist == 0 && rght_sw_hist == 1)
  {
  pwm11 = 175;
  pwm12 = 130;
   if (time == tea_time + 150)
   {
   left_sw_hist = 0;
   rght_sw_hist = 0;  
   step++;
   }
  }
  if (left_sw_hist ==1 && rght_sw_hist == 0)
  {
  pwm11 = 130;
  pwm12 = 175;
   if (time == tea_time + 150)
   {
   left_sw_hist = 0;
   rght_sw_hist = 0;   
   step++;
   }
  }
}

if (step==3) //Raise the arm to loading stand height and check to see if the tetra is held. If so, proceed. Otherwise, retrack the tetra.
{
relay2_fwd = relay3_fwd = 1; //extend the arm
if (rc_dig_in06 == 1)
 {
  if (rc_dig_in04 == 1)
  {
  step++;
  }
  if (rc_dig_in04 == 0)
  {
  relay2_fwd = relay3_fwd = 0;
  relay4_fwd = 1;
   if (rc_dig_in05 == 1)
   { 
   step = 1;
   }
  } 
 }
}


if (step==4) //Track up to the goal until contact is made
{
camera_find_color(YELLOW); //Seek Yellow
relay2_fwd = relay3_fwd = 1; //extend the arm
 if (rc_dig_in10 == 1)
 {
  relay2_fwd = relay3_fwd = 0; //Stop extending the piston if its fully extended
 }
  if (rc_dig_in02 == 1 || rc_dig_in03 == 1) //If one of the skirt switches trip, go to the next step.
	{
	 if (rc_dig_in02 ==1) //Record Left History for the next step
         {
	 left_sw_hist = 1;
	 }
	 if (rc_dig_in03 ==1) //Record right history for the next step
	 {
	 rght_sw_hist = 1;
	 }
	 tea_time = time;
	 step++;
         }
}

if (step==5) //Almost done! Wait until the last few seconds to trigger the capping sequence!
{
 if (left_sw_hist == 1 && rght_sw_hist == 1)
 {
 pwm11 = pwm12 = 130;
  if (seconds == 13) //Lets try giving it 2 seconds to play it safe for now...
  {
  step++;
  }
 }
 if (left_sw_hist == 1 && rght_sw_hist == 0)
 {
 pwm11 = 130;
 pwm12 = 140;
  if (rc_dig_in03 == 1)
  {
  rght_sw_hist = 1;
  }
 }
 if (left_sw_hist == 0 && rght_sw_hist == 1)
 {
 pwm11 = 140;
 pwm12 = 130;
  if (rc_dig_in02 == 1)
  {
  left_sw_hist = 1;
  }
 }
}

if (step==6) //The Final step! Plant that tetra on the goal in the last few seconds!
{
relay4_fwd = 1;
}

if(camera_track_update()==1)
  {
   if (cam.size>0)
     {
     tracking=1;
     }
   else
      tracking=0;
  }

if (step==1 || step==4) //If we're in a tracking step, drive! 
{
 if(tracking==1) //Follow the color if we see any of it!
 {
  pwm11 = Limit_Mix(2000 + fwd_spd + cam.pan_servo - 127);// Ok now put some single joystick drive code here using the variable cam.pan_servo as the x axis of the joystick, and a constant value as the y axis
  pwm12 = Limit_Mix(2000 + fwd_spd - cam.pan_servo + 127);
 }
 else
  pwm11 = 63; //Seek to the left if all else fails!
  pwm12 = 175;
  if (start_pos == 1) //If we started on the left half of the field, turn to the right
  {
  pwm11 = 145;
  pwm12 = 100;
  }
  if (start_pos == 2) //If we started on the right half of the field, turn to the left
  {
  pwm11 = 100;
  pwm12 = 145;
  }
}

I used some tidbits of code posted here on CD to start with. Now, I just wrote this (at 1am) so I won't get to test it til tomorrow, but I'm crossing my fingers that it'll work ok! It did compile!!!
Any comments would be great. Just understand I'm not making it neat or extremely well laid out just yet, I'm trying to get the main process down
Thanks!

[AIBob]

Hmm.... Interesting method.
But note that variables 'time' and 'step' will reset to zero each time that you go through this loop.
Make them 'static signed int time = 0, step = 0;'.
You probably could cut down the length of that a lot, and make it a lot more modular so that you could have quick changes during the compeptitions (whenever allowed), and have a better layout of the code.