Team 323Z Code Help

Alright, I have this function

void DriveStraight(int target, int speed)
{
	nMotorEncoder[RightBack] = 0;
	nMotorEncoder[LeftBack] = 0;

	while((nMotorEncoder[RightBack] < target) || (nMotorEncoder[LeftBack] < target))
	{
		if(nMotorEncoder[RightBack] < target)
			{
				motor[RightFront] = speed;
				motor[RightBack] = speed;
			}
			else
			{
				motor[RightFront] = 0;
				motor[RightBack] = 0;
			}

		if(nMotorEncoder[LeftBack] < target)
			{
				motor[LeftFront] = speed;
				motor[LeftBack] = speed;
			}
			else
			{
				motor[LeftFront] = 0;
				motor[LeftBack] = 0;
			}

	}

}

I try to use it in Autonomous by doing this.


DriveStraight(3, 127);

And only the LEFT side moves. But it does not stop moving.

I have the IME’s set-up correctly, and when I put it in Driver Control, they work.

What is wrong?

try this :


if (abs( nMotorEncoder[RightBack] ) < target)

What I think is happening is the right encoder is reading negative while the left one is reading positive. Using the abs function makes sure the encoder your testing is a positive value.

#lol

I was about to say that. One returns +, and one returns -.

So I add this wherever the old line is?

I would suggest putting the absolute value function in both places just so both sides look the same.

Yeah, anywhere you test an encoder function, just wrap an absolute function around it.

*On second thought, I would caution you that once you put everything in absolute functions, you won’t be able to drive backwards. So another solution would be to do this:

if ( -nMotorEncoder[RightBack] < target )

Anywhere you have nMotorEncoder[RightBack], put a negative sign in front of it.*

Sorry I was wrong about that xD

why can’t you drive backwards? the abs function would still give the distance. and I would assume you would run the motors in the correct direction.

Actually I was thinking about that to how I have my robot’s code setup, you’re completely correct.

I was thinking “DriveStraight(-1000, 127);”, where you set the distance negative, and keep the speed positive.

However, you could do “DriveStraight(1000, -127);”.

My mistake, thanks.

We just use simple forward(distance) and reverse(distance) functions with PID control. the forward and reverse help with less experienced programming/ trouble shooting.

void Move(int target, int speed)
{
	nMotorEncoder[RightBack] = 0;
	nMotorEncoder[LeftBack] = 0;

	while((-nMotorEncoder[RightBack] < target) || (nMotorEncoder[LeftBack] < target))
	{
		if(-nMotorEncoder[RightBack] < target)
			{
				motor[RightFront] = speed;
				motor[RightBack] = speed;
			}
			else
			{
				motor[RightFront] = 0;
				motor[RightBack] = 0;
			}

		if(nMotorEncoder[LeftBack] < target)
			{
				motor[LeftFront] = speed;
				motor[LeftBack] = speed;
			}
			else
			{
				motor[LeftFront] = 0;
				motor[LeftBack] = 0;
			}

	}

}

I am using the above function, but it acts weird.

I tell it to go: Move(100, 127);
The LEFT side goes 100, but the right side keeps on going and going. Why doesn’t the right side stop?

Is one of the encoders reversed?

Seems like the exact same question you posted at the beginning of the thread.

Having nMotorEncoder working backwards is a bad idea. You must have a motor reversed somewhere. You really want to to setup the motors so that there are no red/black wire swaps and that the motors in ports 1 & 10 have the red to the inside of the cortex and act the same as ports 2 through 9. When everything is wired correctly and the motors and sensors setup adjusted (the reversal flag) so that positive command values give forward motor motion (whatever you decide that is) then the nMotorEncoder values should increment for positive commands.

Having said that, it’s also hard to tell what may be wrong with the code when it’s out of context, I don’t see any logical errors other than not knowing why you inverted one of the encoder values. Post the #pragma statements from the top of the code so we can see how everything is configured.

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, in1,    LeftPot,        sensorPotentiometer)
#pragma config(Sensor, in2,    RightPot,       sensorPotentiometer)
#pragma config(Sensor, dgtl1,  Programming_Skills, sensorTouch)
#pragma config(Sensor, dgtl7,  RightButton,    sensorTouch)
#pragma config(Sensor, dgtl8,  LeftButton,     sensorTouch)
#pragma config(Sensor, dgtl10, Offense_Right,  sensorTouch)
#pragma config(Sensor, dgtl11, Defense_Left,   sensorTouch)
#pragma config(Sensor, dgtl12, Defense_Right,  sensorTouch)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Motor,  port1,           RightBack,     tmotorVex393HighSpeed, PIDControl, reversed, encoder, encoderPort, I2C_2, 1000)
#pragma config(Motor,  port2,           RightFront,    tmotorVex393HighSpeed, openLoop, reversed)
#pragma config(Motor,  port3,           IntakeLeft,    tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port4,           IntakeRight,   tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port5,           SWLeft,        tmotorVex393, openLoop)
#pragma config(Motor,  port6,           SWRight,       tmotorVex393, openLoop)
#pragma config(Motor,  port7,           LiftRight,     tmotorVex393, openLoop)
#pragma config(Motor,  port8,           LiftLeft,      tmotorVex393, openLoop)
#pragma config(Motor,  port9,           LeftFront,     tmotorVex393HighSpeed, openLoop)
#pragma config(Motor,  port10,          LeftBack,      tmotorVex393HighSpeed, PIDControl, encoder, encoderPort, I2C_1, 1000)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

When I run in Driver Control forward, 12C_2 (Right Back Motor) runs -. While 12C_1 (Left Back Motor) runs +.

The RIGHT side motors are reversed already. The LEFT side is not.

Are you looking in the “sensors” debug window or the “motors” debug window, IMEs are available to see in both, the number in the sensors window will (and should) decrease as you move forward, the number in the motors window should increase due to the “reverse” flag from you setup.

btw, turn off PID in motors and sensors dialog, not implemented yet, it may bite you later if you install a new version of ROBOTC.

I am looking at the Sensors Debug Window.

I will turn off PID.

Just the RIGHT IME goes - when I drive forward.

Ok, look in the motors window, do both encoders count forwards as you drive forwards. If so, then remove the negative signs you added in front of the nMotorEncoder[RightBack] in your code, basically put it back to how you had it in post #1. The whole negative issue has been misleading you I think. I don’t know why only the left moved originally except for the fact that you asked it to move only 3 encoder counts. You do need to send stop before the while loop exits otherwise only one side will probably stop. Try this.

void Move(int target, int speed)
{
    nMotorEncoder[RightBack] = 0;
    nMotorEncoder[LeftBack] = 0;

    while((nMotorEncoder[RightBack] < target) || (nMotorEncoder[LeftBack] < target))
        {
        if(nMotorEncoder[RightBack] < target)
            {
            motor[RightFront] = speed;
            motor[RightBack] = speed;
            }
        else
            {
            motor[RightFront] = 0;
            motor[RightBack] = 0;
            }

        if(nMotorEncoder[LeftBack] < target)
            {
            motor[LeftFront] = speed;
            motor[LeftBack] = speed;
            }
        else
            {
            motor[LeftFront] = 0;
            motor[LeftBack] = 0;
            }
            
        // Don't hog the CPU
        wait1Msec(10);
        }
        
    // Final stop   
    motor[RightFront] = 0;
    motor[RightBack] = 0;
    motor[LeftFront] = 0;
    motor[LeftBack] = 0;
}

Alright I just looked at the Debug window for the Motors and when I drove forward the encoders went forward.

I then tested the revised code you sent me and it worked.

Would I just do a - to the encoder amount if I wanted to go back?

Edit: How about turning?

Here are the turning functions we have.

void TurnLeft(int target, int speed)
{
	while((nMotorEncoder[RightBack] < target) || (nMotorEncoder[LeftBack] > -target))
	{
		if (abs(-nMotorEncoder[RightBack] ) < target)
			{
				motor[RightFront] = speed;
				motor[RightBack] = speed;
			}
			else
			{
				motor[RightFront] = 0;
				motor[RightBack] = 0;
			}
		if(nMotorEncoder[LeftBack] > -target)
			{
				motor[LeftFront] = -speed;
				motor[LeftBack] = -speed;
			}
			else
			{
				motor[LeftFront] = 0;
				motor[LeftBack] = 0;
			}

	}

}

void TurnRight(int target, int speed)
{
	while((nMotorEncoder[RightBack] > -target) || (nMotorEncoder[LeftBack] < target))
	{
		if(nMotorEncoder[RightBack] > -target)
			{
				motor[RightFront] = -speed;
				motor[RightBack] = -speed;
			}
			else
			{
				motor[RightFront] = 0;
				motor[RightBack] = 0;
			}
		if(nMotorEncoder[LeftBack] < target)
			{
				motor[LeftFront] = speed;
				motor[LeftBack] = speed;
			}
			else
			{
				motor[LeftFront] = 0;
				motor[LeftBack] = 0;
			}

	}

}

Would I put in the extra code you posted into each of these functions?

Oh, I know we had this last year, but I did not do it. In our autonomous program we had tasks (duh). If one of the tasks did not get completed it would tear up the motors and overheat them. So we inserted a line of code that would cut the motors after a few seconds of trying (and it not getting to the certain point).

How would I implement this into our code? Would this go in certain functions, or would it be a function?

Thanks!

I modified you code, you can use as is or just learn from it and change your original.

This should work for forwards and backwards.

Move( 1000, 100 );  // drive forwards
Move( -1000, 100 ); // drive backwards

I have not tested this, may have bugs so keep the original.

Also I added a timeout, the while loop take 10mS to execute (the wait1Msec(10) at the end) so 5 seconds will be 500 loops (5000mS). If we have not reached the target in 5 seconds then I stop the motors. Notice I changed the while loop to monitor motors rather than the target error, no real reason for this but just created cleaner code (imho). Anyway give it a go, I’m just writing from memory so it may have bugs.

void Move(int target, int speed)
{
    int timeout = 0;

    nMotorEncoder[RightBack] = 0;
    nMotorEncoder[LeftBack] = 0;

    // target is too close to 0
    if( abs( target ) < 50 )
        return;
    
    // we have zeroed both encoders so left and right need
    // to move the same amount
    
    // Speed should always be sent as positive
    
    // backwards
    if( target < 0 )
        speed = -speed;
        
    // Start moving
    motor[RightFront] = speed;
    motor[RightBack]  = speed;
    motor[LeftFront]  = speed;
    motor[LeftBack]   = speed;

    while((motor[RightFront] != 0) && (motor[LeftFront] != 0))
        {
        if( abs(nMotorEncoder[RightBack]) >= abs(target) )
            {
            motor[RightFront] = 0;
            motor[RightBack]  = 0;
            }

        if( abs(nMotorEncoder[LeftBack]) >= abs(target) )
            {
            motor[LeftFront] = 0;
            motor[LeftBack]  = 0;
            }
        
        // stop after 5 seconds (500 * 10 = 5000mS)
        // This should not normally occur, we sould have reached
        // thr destination before this point.
        if( ++timeout == 500 )
            {
            motor[RightFront] = 0;
            motor[RightBack]  = 0;
            motor[LeftFront] = 0;
            motor[LeftBack]  = 0;
            }
            
        // Don't hog the CPU
        wait1Msec(10);
        }        
}

OK, I read through the code and I think I understand all that it is doing.

I will test the code later this evening!

Thank you Mr. jpearman!

You are welcome Jesse.

Different targets for left and right perhaps.

I just tried it and it acted like the first Move code I tried. One side stopped, then the other side kept going.