Taking the LifeCam-3000 out of its body. FOV Change?

Technical Programming
#1

Due to space constraints, this year we had to take the lifecam out of its body. I’ve found that the angles I’m turning to with the ADXRS450_Gyro are inaccurate, and I’d like to assume it’s because of the change in degrees per pixel. Here is the equation I’m using.

centerX = the center of the contour.
imageCenter = the center of the camera image.


				if (!isTurning) {
					turn = (centerX[0] - imageCenter) * degreesPerPixel;
				}
				isTurning = true;
				if (fwd == 0) {
					turnConstant = 0.08;
				}

				if (gyro.getAngle() <= (turn - 0.1)) {
					speed = (turn - gyro.getAngle()) * turnConstant;
				} else if (gyro.getAngle() >= turn + 0.1) {
					speed = (turn + -(gyro.getAngle())) * turnConstant;
				} else {
					speed = -angle;
				}
				SmartDashboard.putNumber("speed", -speed);
				drive.mecanumDrive_Cartesian(fwd, 0, -speed, 0);

With the given horizontal FOV of the LifeCam, it should be 0.18 degrees per pixel.

60/320

But I’ve actually found that 0.158 works better. Sometimes it doesn’t turn to the right angle, really confused.

#2

Based on the default LabVIEW code, the camera Field of Vision(FoV) at 320x240 is around 52 degrees in the horizontal. By comparison, at 640x480 the FoV is 60 degrees in the horizontal. Different setting will change the FoV, especially if you use a 16:9 resolution rather than a 4:3, or are looking vertically. Removing the case should have no effect on the FoV.

I would think that your code would always stop short because you need a certain amount of power just to start turning. What I am saying is handing your drive train a turning speed of .15 might not make your robot move. You might need something as high as .45. This is based on your mechanical design. Because you need at least some minimum output to turn, you might be stopping short when the angle is small, which creates a small output. This is especially true if your battery is low on power. For the former problem, I would try giving a constant speed, or at least a lower limit, for the latter think about normalizing your power output somehow.

#3

That’s very good to know, thank you!

Also, I’m glad that you mentioned that. The way that the turn is working, is that it’s high enough to turn when the robot is moving towards the target, but too slow to move when the distance is correct. Couldn’t this be countered by saying


if (fwd == 0) {
    turnConstant = 0.1;
}

forward being the drive speed, and turn constant being the constant the turn speed is multiplied by (by default its 0.05).

It works quite well, but sometimes it is off by 10-12 pixels, sometimes 25. What could cause this? I’ve heard that PID loops are useful for this, but wouldn’t setting the drive speed based on the gyro angle be the same thing?

#4

As Rob was mentioning, there is a minimum power needed to continue turning. Now something to differentiate between which isn’t yet clear to me, is if the controller is not getting it to the correct angle commanded and just stops short or if the controller is getting it to where it is commanded and the input is wrong. To tell which one is the problem, I would recommend putting your error (commanded angle - current angle) in the smart dashboard.

If your error when the robot stops is within your tolerance range, then the controller is not the problem and you would want to further look into how you determine the commanded angle.

Otherwise if your error when the robot stops is outside of the tolerance range, you could do various modifications to ensure it has a high enough speed to ensure it continues turning until it is within tolerance. You mentioned PID already and with what you currently have you are essentially doing just a P controller, which is that it is proportional to the error.

Now a way of dealing with this problem of the robot going too slow as you approach the target is by introducing the I or integral term from PID. What this does is it sums your error over time and then adds more speed to the controller as your error over time increases. This means that as you approach the target and your proportional term is getting smaller, your integral term continues to grow allowing you to ensure you always have the necessary speed to continue turning. Of course, just like you have the turning constant which is essentially what is referred to as the proportional constant, you would also have an integral constant to limit the effects of the integral term.

#5

Not to hijack this thread, but this is a great explanation of the integral part of PID, thanks. PID has been a bit confusing to me for a while but it’s starting to make a lot more sense and this helps quite a bit.

#6

Is there some sort of example for this? I guess this is just a bit confusing to me.

So, what you’re saying is. If my proportional constant P is lowering, and it comes to a point where it does not lower anymore, but the robot is not within the tolerance, the integral constant will add onto it to ensure that the robot has enough speed to fix itself?

How would this work? I would assume I would need to be running this in a separate thread, or is there a simple solution to this with a few lines of code?

P.S sorry for the digression of the thread. I’m the only programmer on my team with no programming mentors, and I’m looking for as much help as I can find haha. Thank you for your help!

#7

PID can be a complicated topic. There are many posts around detailing how to tune PID. The WPIlib comes with a PIDController, and there are two examples built into the FRC examples. But neither of them are for a drivetrain. If you are using CommandBased architecture, then they do have a PIDSubsystem.

But without a mentor, I would forget the PID stuff. The theory often gets tossed in with the practical, and then you have a minimum power output problem too. It could work, but there is a lot to learn. I usually start by teaching a simple system that acts like the Proportional stuff you have in code, but is a little more grainy:


   	if (currAngle < (angle-2)){
    		if(currAngle < (angle - 60)){
    			drive.arcadeDrive(0,-.7);//very far off target, turn fast
    		}else if(currAngle < (angle - 30)){
    			drive.arcadeDrive(0,-.6 );//slow down as you approach
    		}else if(currAngle < (angle - 8)){
    			drive.arcadeDrive(0,-.55);//keep slowing down
		}else{
    			drive.arcadeDrive(0,-.4);//this is the slowest you can go and the robot still turns
    		}
    	}else if(currAngle > (angle+2)){
    		if(currAngle > (angle + 60)){
    			drive.arcadeDrive(0, .7);//very far off target, turn fast
    		}else if(currAngle > (angle + 30)){
    			drive.arcadeDrive(0, .6);//slow down as you approach
    		}else if( currAngle > (angle + 8)){
    			drive.arcadeDrive(0,.55);//keep slowing down
    		}else{
    			drive.arcadeDrive(0,.4);//this is the slowest you can go and the robot still turns
    		}
    	}
    	else{
    		drive.arcadeDrive(0, 0);//close to target, stop turning
    	}

Here you won’t hit a stalling motor just because you are too close to the target as long as you get that .4 to be the right number. These numbers are very dependent on your mechanical system. And as a point that needs to be focused on, if you are moving forward the power require to turn is less than if you are not moving forward or backward. You may need to add more gradations, with in-between output values, between more angles. I.e. add another else if for angles less than 15. Most of all, try this in an absolute voltage control mode, rather than percent voltage control mode. Though this is only an option on a talonSRX, you can fake it with any motor controller by doing something like this:


drive.arcadeDrive(0,(-.47 * 12) / ControllerPower.getInputVoltage());

Getting the voltage at the roborio and using it to divide the voltage you want to output, will rescale the voltage getting to the motors, making it easier to compensate for variation in the battery voltage.

#8

There is a built in PID object in the WPIlib, and if you are using CommandBase, there is a PIDSubsystem. As to use there are two examples for PID built into the example code. Though neither of them is a drive train example. Also there are many posts around on tuning PID.

But honestly, without a mentor, there is a lot of theory in all these posts which you might not get. PID might be overkill as you are attempting to compensate for many factors. I usually teach something like what you started with, that uses just the concept of proportional, but a little more granular.


double currAngle = gyro.getAngle();
angleDiff = angle - currAngle;//angle being the target angle
if (angleDiff > 0){
	if(angleDiff > 40){
    		drive.arcadeDrive(0, -1);//far away from the right angle, go fast
    	}else if(angleDiff > 20){
    		drive.arcadeDrive(0,-.7);//getting closer, slow down
    	}else{
    		drive.arcadeDrive(0,-.4);//this the lowest power where the robot still turns.
    	}
}else if(angleDiff < 0){
    	if(angleDiff > 40){
    		drive.arcadeDrive(0, 1);//far away from the right angle, go fast
    	}else if(angleDiff > 20){
    		drive.arcadeDrive(0,.7);//getting closer, slow down
	}else{
		drive.arcadeDrive(0,.4);//this the lowest power where the robot still turns.
	}
}else{
	drive.arcadeDrive(0, 0);
}

These numbers are very dependent on your mechanical system. So you will need to test them. You might need to add more else if’s with different power outputs to create a more precise control. And I should mention that driving forward while turning will lower the minimum turning power.

You should also think about also compensating for the changing power level of your battery over the course of the match. I would suggest either using the Absolute Voltage Mode on the TalonSRX or use something like:

drive.arcadeDrive(0,.4*12/ControllerPower.getInputVoltage());

This would read the voltage to the roborio, and scale the motor output to compensate.

#9

I was originally using what you stated in the above code. I switched over to the current method I’m using because it seemed to be a smoother way of doing things. Wouldn’t it make more sense to use a curved (smooth) drive pattern than a linear (theoretically bumpy) drive pattern?

Also I’m using something similar to your suggestion with my shooter motor, although I’m using the voltage output from the driver station.


	public double getVoltage() {
		voltage = DriverStation.getInstance().getBatteryVoltage();
		return voltage;
	}

	public double getShooterSpeed() {
		if ((getVoltage() <= 13.4) && getVoltage() >= 12.9) {
			shooterSpeed = 0.525;
		} else if ((getVoltage() <= 12.89) && getVoltage() >= 12.8) {
			shooterSpeed = 0.535;
		} else if ((getVoltage() <= 12.79) && (getVoltage() >= 12.65)) {
			shooterSpeed = 0.545;
		} else {
			shooterSpeed = 0.55;
		}
		return -shooterSpeed;
	}

As we only have 6 hours of unbag time, would testing and changing to your preferred method lead to a significant change with the accuracy of our vision tracking?

#10

The problem is with the calculated angle, I had done what you suggested prior to the post. The gyro angle matches up with the calculated angle, but the angle is not right.