Camera for turning

Technical Programming
1

I am using a combination of the camera and a gyro to turn until the camera is lined up with the green light. With the code I’m using the robot just rotates back and forth and will never stop. Is there a better way to accomplish this. Here is the code:

Camera_Handler();
Servo_Track();

temp_gyro = Get_Gyro_Angle();
tracking_state = Get_Tracking_State();

printf("gyro_angle = %d
", (int)temp_gyro);

if(capture == 0 && tracking_state == CAMERA_ON_TARGET )
{
goal_angle = ((((int)PAN_SERVO - 124) * 65)/124);
capture = 1;
}

milliradian_goal = ((goal_angle * 3.1415 * 1000)/180);

if(capture == 1)
{
if((milliradian_goal - 60) < temp_gyro)
{
pwm03 = pwm06 = 150; //Turns robot left
pwm04 = pwm05 = 150;
}
if(temp_gyro < milliradian_goal + 60)
{
pwm03 = pwm06 = 104; //Turns robot right
pwm04 = pwm05 = 104;
}
if(((milliradian_goal + 60) >= temp_gyro) && (temp_gyro >= (milliradian_goal - 60)))
{
pwm03 = pwm06 = 127;
pwm04 = pwm05 = 127;
capture = 2;
}
}

2

Why are you setting all four PWM outs to the same value? If you want to turn left, set your right motors forward and your left motors either stopped or reversed and opposite for turning right.

3

The motors on the left side have to go in reverse in order to move the robot forward. So by using the same number the left side wheels and the right side wheels go opposite directions.

4

OK, we usually wire the motors up with the left motor polarities being opposite the right motor polarites so that 254==full forawrd on both the left and the right sides. Sorry bout that. In this case I would suggest using a proportional control loop where the output applied to the motors scales itself to the amount of error, lestening the oscillations.

5

I have messed around with control loops before build season but never could get one to work correctly. Could you post example?

6

Making the motor speed proportional to the error will stop the oscillations if the proportional constant is low enough. But then you’ll probably find the robot won’t quite get to the desired angle, and you’ll want to add some error integration to force it to move after it’s been off-angle long enough. At that point, you might have some overshoot, and to fix that requires some negative feedback based on the rate of change of the error.

In short, it’s time to implement a PID control system for your robot direction. Search the forums to find plenty of information, and even some code, that will do what you need.

7

I have actually searched all over chiefdelphi for information. I found a lot except that I couldn’t find anything about an output to control more than one pwm.

8

i actually have a bit of code that may help you.

unsigned char PIkDControl(char Kp, char Ki, char Kd, int error) {
	static int errorTotal = 0;  //there is a better way to do this... meh
	static int prevError = 0;
	int P = 0;
	int I = 0;
	int D = 0;
	
	P = (error*Kp)/100;
	I = (errorTotal*Ki)/100;
	D = ((error - prevError)*Kd)/100;

	errorTotal += error;
	prevError = error;

	return  (Limit_Mix(2000+127+P+I-D));
}

There are a couple of other tweaks, but i edited them out. you should come across them whilst testing (=

9

Thanks, I’ll try that. One question: What’s the limit mix for. I have never seen it on a control loop.

10

limiting output, of course. you don’t want your unsigned char to overflow, that would be bad

11

A brief look at your existing code reveals that to be a non-issue. If you supply the same PID-computed value to all your motor pwms, your robot will turn in place. That’s exactly what you want it to do, isn’t it?

12

While using your code how do i implement a target value and how do I out put it to four motors. Sorry for all the questions but PID loops are still very new to me.

13

the function is returning a motor value from 0-255. it is up to you to map it to your motors, like you would a joystick.

as for inputting the error… you need to figure out your error much like you did in your original loop. with the dual targets this year, though, you will have to do a little (lot) of extra math and AI to get your desired target.

14

PID Without a Ph.D

All you had to do was ask…:cool:

15

Thanks a lot for all of your help. I finally got a PID loop running and it works great.

16

Please help me, I can’t figure out how to use the camera to actually MOVE the robot. I have the camera code working along with the camera itself, but how do I move pwm01 (left drive wheel) and pwm02 (right drive wheel) individually according to the pan of the camera? I have no idea where to begin with this code. (I just got the code to work for the camera and I’ve been trying for weeks. Now I only have 2 days, Please help if you can)


		
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:

unsigned int Find_Distance(void)
{	
	return(targetRange[pwm07-144]);
}and target range is this:


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

So I would like to know why my distance thing isn’t working (Max tilt is 194 and min is 94, center at 144. My degree per pwm step was 1.7647 (50/90? is that right?) Then I multiplied that by the pwm index up to 51 and found 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). ALSO, how do I program the robot to move forward???

17

it may be a little off, but to fix the motors going the wrong way, you can either switch up some wires (not sure which though, im only programming) or put

pwmXX=255-pwmXX;

putting the number of the reversed motors in the “XX” at the end of the code. Then you can put all of your code as 255 forward and 0 back. Instead of the odd backwards motor.

18

Ok, I got that. But about making the bot go in the direction of the light, would this work?

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;

                                      }
                                      else if(PAN_SERVO < PAN_CENTER_PWM_DEFAULT)
                                      {
                                           pwm02 = 180;
                                      }
                                      else
                                      {
                                           pwm01 = 180;
                                           pwm02 = 180;
                                      }                     
                          }

(pwm01_On_Off and pwm02_On_Off are just to map the joystick to the pwm. if 1, then joystick works, if its not 1 (usually 0), then joysticsk dont work and the motors can be set to specific speeds.)

19

Yes, this looks right to me. Note that you will want to set pwm01 and pwm02 to 127 when they aren’t supposed to be moving or else they will stay at their old value (and thus always be 180). Also, the


else
{
    pwm01 = 180;
    pwm02 = 180;
}

would probably never happen without at least proportional control (search CD for posts about it, but basically error * a constant = motor speed). Give the code a deadband where it will go straight even if off by a little. Also consider doing a differential turn (where you turn while moving forward by driving one side faster than the other).

Good luck,
Robinson Levin