Team 323Z Code Help

JesseCRN · November 24, 2012, 9:03am

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?

JMarple · November 24, 2012, 9:08am

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.

JesseCRN · November 24, 2012, 9:11am

#lol

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

So I add this wherever the old line is?

tabor473 · November 24, 2012, 9:15am

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

JMarple · November 24, 2012, 9:32am

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

ClockWorkKnights · November 24, 2012, 9:34am

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.

JMarple · November 24, 2012, 9:40am

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.

ClockWorkKnights · November 24, 2012, 9:52am

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

JesseCRN · November 26, 2012, 10:53am

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?

z_beeblebrox · November 26, 2012, 11:30am

Is one of the encoders reversed?

jpearman · November 26, 2012, 11:43am

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.

JesseCRN · November 26, 2012, 12:06pm

#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.

jpearman · November 26, 2012, 12:57pm

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.

JesseCRN · November 26, 2012, 1:14pm

I am looking at the Sensors Debug Window.

I will turn off PID.

Just the RIGHT IME goes - when I drive forward.

jpearman · November 26, 2012, 1:49pm

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

JesseCRN · November 26, 2012, 2:25pm

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!

jpearman · November 26, 2012, 2:49pm

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

JesseCRN · November 26, 2012, 3:15pm

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!

jpearman · November 26, 2012, 4:03pm

You are welcome Jesse.

Different targets for left and right perhaps.

JesseCRN · November 26, 2012, 4:12pm

jpearman:

<<>>

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

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