Code instantly starting and stopping

VEX V5 General Discussion Technical Discussion
1

When I run this code, it instantly stops. If you were to download and use the popup window that has the button “Start”, press that button, it quickly changes to “Stop” and then back to “Start”. It indicates the code is starting, but then quickly ending. I don’t know why my code can’t run. I’m thinking it’s something to do with my control loops (like the if’s and the while’s), however, I can’t find the mistake. The code is below, thank you in advance (and I think it’s a little too complicated, but I made it anyway).

void driveChassisStraightWithPID(float distance /*in inches*/, int direction /*only a 1 or -1, used for going forward or backward*/) {

int setPoint = (int)round((distance) * (1 / wheelDiameter) * (1 / PI) * (ticksPerRevolutionOfIME)); //calculate encoder value
int leftError, rightError, leftIntegral, rightIntegral, leftDerivative, rightDerivative, leftPreviousError, rightPreviousError, leftPower, rightPower; //declare variables

if(direction != 1 || direction != -1) { //safety check if direction wasn't 1 or -1 (a different value might mess up the speed value)

	driveLeftChassis(0);
	driveRightChassis(0);
	SensorValue[leftBackChassisIME] = 0;
	SensorValue[rightBackChassisIME] = 0;

}

else { //otherwise continue on

	while(fabs(SensorValue[leftBackChassisIME]) < setPoint || fabs(SensorValue[rightBackChassisIME]) < setPoint) { //while the left or right side isn't there at setPoint, keep looping

		if(fabs(SensorValue[leftBackChassisIME]) < setPoint) { //if left side isn't there at setPoint, do stuff in brackets

			leftError = setPoint - fabs(SensorValue[leftBackChassisIME]); //PID stuff below
			leftIntegral = leftIntegral + leftError;
			if(leftError <= 0) { leftIntegral = 0; }
			leftDerivative = leftError - leftPreviousError;
			leftPreviousError = leftError;
			leftPower = (((leftError * chassisKP) + (leftIntegral * chassisKI) + (leftDerivative * chassisKD)) * direction);
			if(fabs(leftPower) > maxChassisSpeed) { leftPower = leftPower < 0 ? -maxChassisSpeed : maxChassisSpeed; } //if calculated power is greater than specified maxChassisSpeed, reassign it to the maxChassisSpeed
			driveLeftChassis(leftPower); //assign left motors their calculated speed value

		}

		else { //once finished stop the left side and reset encoder so that if the function is called again, old sensor value isn't used

			driveLeftChassis(0);
			SensorValue[leftBackChassisIME] = 0;

		}

		if(fabs(SensorValue[rightBackChassisIME]) < setPoint) { //if right side isn't there at setPoint, do stuff in brackets

			rightError = setPoint - fabs(SensorValue[rightBackChassisIME]); //PID stuff below
			rightIntegral = rightIntegral + rightError;
			if(rightError <= 0) { rightIntegral = 0; }
			rightDerivative = rightError - rightPreviousError;
			rightPreviousError = rightError;
			rightPower = (((rightError * chassisKP) + (rightIntegral * chassisKI) + (rightDerivative * chassisKD)) * direction);
			if(fabs(rightPower) > maxChassisSpeed) { rightPower = rightPower < 0 ? -maxChassisSpeed : maxChassisSpeed; } //if calculated power is greater than specified maxChassisSpeed, reassign it to the maxChassisSpeed
			driveRightChassis(rightPower); //assign right motors their calculated speed value

		}

		else { //once finished stop the right side and reset encoder so that if the function is called again, old sensor value isn't used

			driveRightChassis(0);
			SensorValue[rightBackChassisIME] = 0;

		}

		wait1Msec(cycleTime); //buffer time that is also used in PID

	}

}

driveLeftChassis(0); //once whole thing is done, stop the motors and reset values (i have paranoia so I do this make me feel safe)
SensorValue[leftBackChassisIME] = 0;
driveRightChassis(0);
SensorValue[rightBackChassisIME] = 0;

}

When I call the function in task main, it looks like this:


driveChassisStraightWithPID(12, 1);
2

Actually, now that I see my code, I think the problem is when I calculate the setPoint value and the conversion/rounding stuff I do just make any calculated value turn into 0. If that’s true, then, of course, the code would instantly finish because the sensors are already at zero. I might be wrong though and I don’t have a robot to test it on so… what do you guys think?

3

All that code is doing is defining a function; it is never actually being called. It needs to be called in the main task for the robot to actually do anything.

4

Yes I had it in task main when I encountered this problem

5

You are correct, setPoint is always zero. When you mix integers and floats, it’s pretty hard to predict exactly what will happen. My suggestion would be to just convert everything to floats.


int setPoint = distance * (1.0f / (float) wheelDiameter) * (1.0f / PI) * (float) ticksPerRevolutionOfIME

When you store a float in an int, it is automatically truncated and converted for you, so you don’t have to do it yourself (although if you want it rounded you could just surround the whole expression with


round()

). the


1.0f

s are just floats of


1

, in case they looked weird to you.

6

Whoops, I didn’t see the bottom of your post. My bad.

7

@ZachDaChampion I use #define to “set” values for wheelDiamter and ticksPerRevolutionOfIME so should I put #define wheelDiameter 4.0f or just 4? I assume 4.0f. Should I just float all numerical values (like put .0f for all whole number)?

8

Yes, use 4.0f. For any constants (like wheel wheel diameter, ticks per revolution, etc), you should use floats (unless they are only operated on with ints). In that case, you don’t even need some of the typecasts, so you can take them out if you want.

9