Autonomous Runs Inconsistent w/ Sensors

VEX V5 Technical Support UNOFFICIAL Tech Support
#1

FIXED

Hi, our team is having a problem with our autonomous code being inconsistent, but it doesn’t seem to be from sensor values being inaccurate.

When we run our autonomous code, it either works fine, or one of the first turns in the program will be completely off(turns about half of what is is supposed to). When the first turn is not accurate, it throws off our lift heights and turns throughout the rest of the program.

We are using a potentiometer to control our lift height, and the lift will only lift to about half of what it is supposed to if the first turn is not correct.

We are using ROBOTC 4.27, and all the firmware is up to date.

Here is our code:

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, in1,    potentiometerLt, sensorPotentiometer)
#pragma config(Sensor, dgtl2,  claw,           sensorDigitalOut)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Motor,  port1,           Intake1,       tmotorVex393_HBridge, openLoop, reversed)
#pragma config(Motor,  port2,           RtBackWheel,   tmotorVex393HighSpeed_MC29, openLoop, reversed, encoderPort, I2C_1)
#pragma config(Motor,  port3,           RtDrive,       tmotorVex393HighSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port4,           RtBottomLift,  tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port5,           RtTopLift,     tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port6,           LtBackWheel,   tmotorVex393HighSpeed_MC29, openLoop, encoderPort, I2C_2)
#pragma config(Motor,  port7,           LtDrive,       tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port8,           LtBottomLift,  tmotorVex393HighSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port9,           LtTopLift,     tmotorVex393HighSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port10,          Intake2,       tmotorVex393_HBridge, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)
void fwds(int distance, int speed)
{
	nMotorEncoder[RtBackWheel] = 0;
	nMotorEncoder[LtBackWheel] = 0;
	while(nMotorEncoder[RtBackWheel] < distance && nMotorEncoder[LtBackWheel] > (distance * -1))
	{
		motor[LtDrive] = speed;
		motor[LtBackWheel] = speed;
		motor[RtDrive] = speed;
		motor[RtBackWheel] = speed;
	}
	motor[LtDrive] = 0;
	motor[LtBackWheel] = 0;
	motor[RtDrive] = 0;
	motor[RtBackWheel] = 0;
}
void bwds(int distance, int speed)
{
	nMotorEncoder[RtBackWheel] = 0;
	nMotorEncoder[LtBackWheel] = 0;
	while(nMotorEncoder[RtBackWheel] > (distance * -1) && nMotorEncoder[LtBackWheel] < distance)
	{
		motor[LtDrive] = -speed;
		motor[LtBackWheel] = -speed;
		motor[RtDrive] = -speed;
		motor[RtBackWheel] = -speed;
	}
	motor[LtDrive] = 0;
	motor[LtBackWheel] = 0;
	motor[RtDrive] = 0;
	motor[RtBackWheel] = 0;
}
void turnL(int distance, int speed)
{
	nMotorEncoder[LtBackWheel] = 0;
	nMotorEncoder[RtBackWheel] = 0;
	while(nMotorEncoder[LtBackWheel] < distance && nMotorEncoder[RtBackWheel] < distance)
	{
		motor[LtBackWheel] = -speed;
		motor[LtDrive] = -speed;
		motor[RtBackWheel] = speed;
		motor[RtDrive] = speed;
	}
	motor[LtBackWheel] = 0;
	motor[LtDrive] = 0;
	motor[RtBackWheel] = 0;
	motor[RtDrive] = 0;
}
void turnR(int distance, int speed)
{
	nMotorEncoder[LtBackWheel] = 0;
	nMotorEncoder[RtBackWheel] = 0;
	while(nMotorEncoder[LtBackWheel] > -distance && nMotorEncoder[RtBackWheel] > -distance)
	{
		motor[LtBackWheel] = speed;
		motor[LtDrive] = speed;
		motor[RtBackWheel] = -speed;
		motor[RtDrive] = -speed;
	}
	motor[LtBackWheel] = 0;
	motor[LtDrive] = 0;
	motor[RtBackWheel] = 0;
	motor[RtDrive] = 0;
}
void bwdslft(int distance, int speed)
{
	nMotorEncoder[RtBackWheel] = 0;
	nMotorEncoder[LtBackWheel] = 0;
	while(nMotorEncoder[RtBackWheel] > (distance * -1) && nMotorEncoder[LtBackWheel] < distance)
	{
		motor[LtDrive] = -speed;
		motor[LtBackWheel] = -speed;
		motor[RtDrive] = -speed;
		motor[RtBackWheel] = -speed;
		motor[RtBottomLift] = 127;
		motor[RtTopLift] = 127;
		motor[LtBottomLift] = 127;
		motor[LtTopLift] = 127;
	}
	motor[LtDrive] = 0;
	motor[LtBackWheel] = 0;
	motor[RtDrive] = 0;
	motor[RtBackWheel] = 0;
	motor[RtBottomLift] = 0;
	motor[RtTopLift] = 0;
	motor[LtBottomLift] = 0;
	motor[LtTopLift] = 0;
}
void liftHold()
{
	motor[RtTopLift] = 17;
	motor[LtTopLift] = 17;
	motor[RtBottomLift] = 17;
	motor[LtBottomLift] = 17;
}
void liftup(int speed, int distance)
{
	while(SensorValue[potentiometerLt] > distance)
	{
		motor[LtTopLift] = speed;
		motor[LtBottomLift] = speed;
		motor[RtTopLift] = speed;
		motor[RtBottomLift] = speed;
	}
	motor[LtTopLift] = 0;
	motor[LtBottomLift] = 0;
	motor[RtTopLift] = 0;
	motor[RtBottomLift] = 0;
}
void liftdown(int speed, int distance)
{
	while(SensorValue[potentiometerLt] < distance)
	{
		motor[LtTopLift] = -speed;
		motor[LtBottomLift] = -speed;
		motor[RtTopLift] = -speed;
		motor[RtBottomLift] = -speed;
	}
	motor[LtTopLift] = 0;
	motor[LtBottomLift] = 0;
	motor[RtTopLift] = 0;
	motor[RtTopLift] = 0;
}
void intake(int speed, int time)
{
	motor[Intake1] = -speed;
	motor[Intake2] = -speed;

	wait1Msec(time);

	motor[Intake1] = 0;
	motor[Intake2] = 0;

}

void outtake(int speed, int time)
{
	motor[Intake1] = speed;
	motor[Intake2] = speed;

	wait1Msec(time);

	motor[Intake1] = 0;
	motor[Intake2] = 0;
}
void skyriseintake()
{
	SensorValue[claw] = 1;
}

void skyriseouttake()
{
	SensorValue[claw] = 0;
}
void liftTick (int speed, int distance, int maxTicks)
{
	int ticks = 0;
	while(ticks <= maxTicks)
	{
		if(SensorValue[potentiometerLt] > distance)
		{
			motor[LtTopLift] = speed;
			motor[LtBottomLift] = speed;
			motor[RtTopLift] = speed;
			motor[RtBottomLift] = speed;
		}
		else if(SensorValue[potentiometerLt] < (distance - 150))
		{
			motor[RtTopLift] = -speed;
			motor[LtTopLift] = -speed;
			motor[RtBottomLift] = -speed;
			motor[LtBottomLift] = -speed;
		}
		else
		{
			motor[LtTopLift] = 17;
			motor[LtBottomLift] = 17;
			motor[RtTopLift] = 17;
			motor[RtBottomLift] = 17;
			ticks++;
		}
		wait1Msec(25);
	}
	motor[LtTopLift] = 0;
	motor[LtBottomLift] = 0;
	motor[RtTopLift] = 0;
	motor[RtBottomLift] = 0;
}

task main()
{

	//RED SKYRISE 8 PT

	outtake(127,250);
	intake(127,250);
	outtake(127,250);
	intake(127,400);
	bwds(80,127);
	turnL(50,100);
	turnR(1,17);
	wait1Msec(1000);
	liftTick(127,1900,3);
	liftHold();
	bwds(50,50);
	wait1Msec(500);
	fwds(70,50);
	wait1Msec(1000);
	skyriseintake();
	wait1Msec(500);
	bwdslft(300,75);
	turnL(195,70);
	turnR(1,17);
	fwds(10,50);
	wait1Msec(1000);
	liftdown(35,1800);
	liftHold();
	wait1Msec(1000);
	skyriseouttake();
	bwds(125,75);
	liftup(127,1500);
	liftHold();
	fwds(90,75);
	outtake(127,3000);



}

One thing we tried to fix this was the “liftTick” function, where the potentiometer value would be checked multiple times before the function was stopped. We noticed that this function only had to correct the lift height when the first turn was off(just like before). This fixed the problem with the lift height, but the turns throughout the program seemed to still be off.

We have also tried using another cortex, but had the same problem.

Both IME’s are also reading values.

Any help is appreciated. Thanks!

#2

First off, nice code!

We don’t see functions being used properly and things generally following at least SOME kind of format that often here.

I’m going to do what I normally do, and sweep through the code line by line, making suggestions along the way.

So first off, your bwds() function could be rewritten in terms of fwds() like so:

void bwds(int distance, int speed)
{
	fwds(distance, -speed)
}

This is better because it’s A, less code and B, using DRY coding methodology, which is one of those ideas I tend to advocate.

And if we’re going to do that, we should fix the duplicate code in the turn() methods too…

#define LEFT 0
#define RIGHT 1

void turn(int distance, int speed, bool direction)
{

	nMotorEncoder[LtBackWheel] = 0;
	nMotorEncoder[RtBackWheel] = 0;

	if(direction)
	{
		// Turn RIGHT
		while(nMotorEncoder[LtBackWheel] > -distance && nMotorEncoder[RtBackWheel] > -distance)
		{
			motor[LtBackWheel] = speed;
			motor[LtDrive] = speed;
			motor[RtBackWheel] = -speed;
			motor[RtDrive] = -speed;
		}
	}
	else
	{
		// Turn LEFT
		while(nMotorEncoder[LtBackWheel] < distance && nMotorEncoder[RtBackWheel] < distance)
		{
			motor[LtBackWheel] = -speed;
			motor[LtDrive] = -speed;
			motor[RtBackWheel] = speed;
			motor[RtDrive] = speed;
		}
	}

	motor[LtBackWheel] = 0;
	motor[LtDrive] = 0;
	motor[RtBackWheel] = 0;
	motor[RtDrive] = 0;

}

You’d call that like so:


turn(4, 127, LEFT);

BUT we can do better, WAY better…

#define LEFT 1
#define RIGHT -1

void turn(int distance, int speed, int direction)
{
	nMotorEncoder[LtBackWheel] = 0;
	nMotorEncoder[RtBackWheel] = 0;

	while(direction * nMotorEncoder[LtBackWheel] < distance && direction * nMotorEncoder[RtBackWheel] < distance)
	{
		motor[LtBackWheel] = -direction * speed;
		motor[LtDrive] = -direction * speed;
		motor[RtBackWheel] = direction * speed;
		motor[RtDrive] = direction * speed;
	}

	motor[LtBackWheel] = 0;
	motor[LtDrive] = 0;
	motor[RtBackWheel] = 0;
	motor[RtDrive] = 0;
}

The trick here is the #define marcos, making use of -1, and 1 to change signs. Make sure the #define statements are at the top of your file. Preferably in a header.

Give that a spin and see what happens.
-Cody

#3

Another thing that can affect an autonomous greatly, just pointing out the obvious, is that a software problem can very often be a hardware problem. Make sure that the potentiometer and the axles are fully screwed on. Double check all of the motors to make sure that they are not burned out. Also, make sure your motors are screwed on completely (that gave our team a fair amount of trouble last year). Last year, while I was writing my autonomous, I spent a solid two hours writing the first part of my autonomous before I realized one of the motors were burnt out. It’s a lesson I won’t soon forget…

#4

Any chance that your practice field is building up electrostatic charge? I heard some teams complain last winter that they thought that their IMEs (Integrated Motor Encoders) used on their wheels were giving them bad readings intermittently because of static charge. Just a thought. I suppose you have been watching your sensor values on a debugger window or something like that.

#5

What’s the gearing on the drive wheels? 50 encoder counts is not very far (1 inch with 4" wheels, motors on torque and 1:1 gearing), short distances are notoriously hard to achieve without controlled acceleration/deceleration. Turn right of 1 count? What’s that all about?

	turnL(50,100);
	turnR(1,17);
#6

I assumed the turn right 1 tick was to make it stop. Just enough power the other way to try go from full speed to dead stop.

#7

It’s completely random. It will work for a few runs, not work, then start working again without making any changes or redownloading the program.

Thanks for going through and cleaning up our code! We’ll give that a try and see if it makes a difference.

All our motors seem to be fine and sensors are screwed in and reading good values, but I’ll double check. Thanks!

This is possible, but I don’t seem to be getting any bad values or shocks from the robot, and it doesn’t seem to happen more as we run the robot more. I’ll do some more testing though.

We are 1:1.2 on 4" omnis(turbo gears in motors with 12 tooth and 24 tooth sprockets). I know that small turns will naturally be a little inconsistent, but I find it kind of weird that it is able to get within about 5-10 degrees sometimes, and other times it will be off by about 30+ degrees. The 1 count turn right acted as a quick brake which seemed to help a little with the robot drifting, but it isn’t really necessary.

#8

You are using integrated motor encoders which are prone to failure. This has been discussed a lot on the forums with no real fix. 5% of the time your routine will fail if you choose to use them. JP has posted code to help, but I have not tested enough to say if it helps or does not. I did not look at your code only your description and sensor setup.

#9

With driving distance or turns being innacurate with the encoders, I usually put on some sort of closed loop control and then break the control loop when the setpoint has been reached.

#10

Thanks for the help guys. Cody’s changes in the code actually seemed to have made a difference and our autonomous code isn’t having the same problem as before. Not exactly sure why, but it works so we’re going to leave it at that.

#11

Sounds good. PM me if you need any more help.:slight_smile:

#12

I actually really thought that might work.

It’s happened to me before, many times. Errors hide in the dark corners of the shadows created by complexity. When you de-clutter the code there are less places errors can hide.

In this case because the error was intermittent, and ONLY affecting the turning I knew the problem was in the turn function, so that’s where I put time into.

I didn’t really go hunting for the issue, instead I refactored the code and the problem simply went away.

Guys, please take a little time to understand what I wrote and how it works if you’re going to use it. I didn’t test that code at all, so there’s no guarantee that it’s proper, but it’s not that complex, it should work.

There are even better ways to do this, but that was the easiest snap in solution.

Don’t be afraid to try something completely different, or iterate on that.

Otherwise, good luck!

-Cody

EDIT: Also, the parameter direction could be (and probably should be) a char instead of an int. It’s 1 byte v/s 4, so a small savings.

EDIT 2: It’s also very possible to rewrite fwds() and bkwds() as a simple move() function that takes a direction as a parameter too. Same idea, good for the same reasons.

#13

One of the weird things with original code is that during turns, the wheel encoders were counting opposite directions, but being compared the same; Also the left and right turn original versions changed both the sign of the direction, AND the <> comparison; surely one or the other would do.

Here is another untested version based on Cody2, that keeps L and R going individually until they EACH reach their distance limit, which might make your turns more accurate.

Its formatted to show commonalities between Lt and Rt
I’m assuming RobotC allows stacking assignments and trinaries.
Some people also just use DRIVE() code that does all possibilities of forward,backward,left,right, pivot turns, spin in place turns, with just a few parameters.

// let Lt and Rt wheels rotate independently
#define LEFT 1
#define RIGHT -1

void turn(int distance, int speed, int direction)
{
	nMotorEncoder[LtBackWheel] = 0;
	nMotorEncoder[RtBackWheel] = 0;
            // +/- makes encoders count up,   while not there yet
	while(  direction * nMotorEncoder[LtBackWheel] < distance
             && direction * nMotorEncoder[RtBackWheel] < distance)
	{
      ( nMotorEncoder[LtBackWheel] * direction  < distance ? // trinary
		motor[LtBackWheel] =
		motor[LtDrive]     = -direction * speed    :  // if true
		motor[LtBackWheel] =
		motor[LtDrive]     = 0                    );  // if false

      ( nMotorEncoder[RtBackWheel] * direction  < distance ? // trinary
		motor[RtBackWheel] =
		motor[RtDrive]     =  direction * speed    :  // if true
		motor[RtBackWheel] =
		motor[RtDrive]     = 0                    );  // if false

	}
    // Safety stop, should not be needed 
	motor[LtBackWheel] = 0;
	motor[LtDrive]     = 0;
	motor[RtBackWheel] = 0;
	motor[RtDrive]     = 0;
}
#14