Chief Delphi - CMUCam and autonomous code

Here's what I have so far

Code:

        

int I_Want_To_Search = 1;

int pwm01_On_Off = 1;

int pwm02_On_Off = 1;

int pwm03_On_Off = 1; //these are all at the top of user_routines.c Everything else is in Processfrom master UP (except TARGETRANGE[] and FIND_DISTANCE())







                          if(I_Want_To_Search == 1)

        {

                Servo_Track();

        }





                          if((Get_Tracking_State() == CAMERA_ON_TARGET) || (Get_Tracking_State() == TARGET_IN_VIEW))

                {

                        pwm01_On_Off = 0;

                        pwm02_On_Off = 0;

                        pwm01 = 230; //Desired Forward Velocity

                        pwm02 = 230;

                        Switch3_LED = 1;

                        

                        //printf("2"); //these statements are just to see if it worked

                }

                else

                {

                        Switch3_LED = 0;

                        pwm01_On_Off = 1;

                        pwm02_On_Off = 1;

                        //printf("4"); //these statements are just to see if it worked

                }

                /*

                if(Find_Distance() <= 50) //THIS DOESN"T WORK!! WHY NOT?

                {

                        pwm01 = 127;

                        pwm02 = 127;

                        pwm01_On_Off = 1;

                        pwm02_On_Off = 1;

                        I_Want_To_Search = 0; //Stops calling Servo_Track(). THEN CALL ANOTHER THING

                        Switch3_LED = 0;

                        //printf("3"); //these statements are just to see if it worked

                }

                */



                if(p1_y > 129 || p2_y > 129)

                {

                        pwm01_On_Off = 1;

                        pwm02_On_Off = 1;

                }

                else if(p1_y < 125 || p2_y < 125)

                {

                        pwm01_On_Off = 1;

                        pwm02_On_Off = 1;

                }

The Find_Distance() function is this:

Code:

unsigned int Find_Distance(void)

{        

        return(targetRange[pwm07-144]);

}

and target range is this: (I just changed this, but the new values are on my other computer. So that might be the problem, but ya never know. Look at the bottom at my explanation to see if it makes sense)

Code:

const rom unsigned int targetRange[] =

        {

3355, 

1676,

1116,

835,

666,

553,

472,

411,

363,

325, //place 10

293,

267,

244,

225, 

208,

193,

179,

167,

156,

146, //place 20

137,

128,

121, 

113, 

107,

100,

94,

89,

83,

78, //place 30

73,

68, 

64,

60,

56,

51,

48,

44,

40,

36, //place 40

33, 

29, 

26,

23,

19,

16,

13,

10,

6,

3, //place 50

0, //place 51

};

SoThere are a few problems with this so far. My Find_Distance doesn't seem to be working. (Default Max tilt is 194 and min is 94, center at 144. Should I change these? My degree per pwm step is 1.9608 (100/51? is that right?) Then I multiplied that by the pwm indexes up to 51 and found the degree value and then the radian value and plugged it into TAN(x) and then did 116-12.625(my height)/tan(the radian value from before)) Assuming that's correct, what's wrong with the rest of the code? It's not working properly. Camera looks away from the light when I comment in the Find_Distance code (less than 50 or whatever). This could be because I just changed the degree per pwmstep to 1.9608 from 1.7something. I have yet to change the values in the code. I will do that tomorrow. Also, to move the robot forward could I do something like this?:

Code:

if((Get_Tracking_State() == CAMERA_ON_TARGET) || (Get_Tracking_State() == TARGET_IN_VIEW))

                {

                        pwm01_On_Off = 0;

                        pwm02_On_Off = 0;

                        Switch3_LED = 1;

                                       if(PAN_SERVO > PAN_CENTER_PWM_DEFAULT)

                                      {

                                           pwm01 = 180;

                                           pwm02 = 74;



                                      }

                                      else if(PAN_SERVO < PAN_CENTER_PWM_DEFAULT)

                                      {

                                           pwm01 = 74

                                           pwm02 = 180;

                                      }

                                      else

                                      {

                                           pwm01 = 180;

                                           pwm02 = 180;

                                      }                     

                          }

That make any sense? Thanks for your help