Robot won't move normally after Gyro Pivot? HELP!!!

csiv23 · March 1, 2018, 1:30pm

Hey guys, I am currently writing a skills autonomous and I have a bit of a problem. After the last pivot, the robot won’t move forward through a normal base function but will move with my PID function if I choose so. However, my PID function limits the robot and doesn’t make it drive fast enough so it can reach the zone. How can I fix this? Is there any way that I can make the PID function double my power, or is it an interruption from one of my tasks? Also, I know my gyro’s 100 degrees turn isn’t “really” 100 degrees in the code, I’m just too lazy to rescale my gyro. My drive function at the end is also way too long, simply out of my frustration. I can answer any questions below. Thanks in advance!

 
task Battery()
{
bLCDBacklight = true;
string mainBattery, backupBattery;

while(true)
{
	clearLCDLine(0);
	clearLCDLine(1);

	//display

	displayLCDString(0, 0, "Primary: ");
	sprintf(mainBattery, "%1.2f%c", nImmediateBatteryLevel/1000.0,'V'); //Build the value to be displayed
	displayNextLCDString(mainBattery);

	displayLCDString(1, 0, "Backup:  ");
	sprintf(backupBattery, "%1.2f%c", BackupBatteryLevel/1000.0,'V');
	displayNextLCDString(backupBattery);

	wait1Msec(100);
	}
}

void PIDbaseControl (int speed)
{
		motor[leftSideDrive] = speed;
		motor[rightSideDrive] = speed;
}

void straightBaseControl (int leftSide, int rightSide)
{
	leftSide = motor[leftSideDrive];
	rightSide = motor[rightSideDrive];
}

void baseControl(int leftPower, int rightPower, int baseTime)
{
	motor[leftSideDrive] = leftPower;
	motor[rightSideDrive] = rightPower;
	baseTime = wait1Msec(baseTime);
}

void rawBaseControl(int leftSide, int rightSide)
{
	motor[leftSideDrive] = leftSide;
	motor[rightSideDrive] = rightSide;
}

void rawliftControl(int leftSideMogo, int rightSideMogo)
{
	motor[leftMobileGoal] = leftSideMogo;
	motor[rightMobileGoal] = rightSideMogo;
}
void liftControl (int leftLift, int rightLift, int liftTime)
{
	motor[leftMobileGoal] = leftLift;
	motor[rightMobileGoal] = rightLift;
	liftTime = wait1Msec(liftTime);
}

void tipControl (int leftTip, int rightTip, int tipTime)
{
	motor[rightTipper] = rightTip;
	motor[leftTipper]= leftTip;
	tipTime = wait1Msec(tipTime);
}

//reading: ticks -- 627.2ticks/revolution
// 1 turn of the wheels -- 4 inch wheels = 627.2 *2 ticks
//1 turn of each wheel 4*pi inches
//4*pi inches = 627.2*2 ticks

//ticks/inch = 627.2 * 2 /4*pi= 99.82198


int inchToTicks (float inch)
{
		int ticks;
		ticks = inch*99.82198;
		return ticks;
 }

int fixTimerValue (float rawSeconds)
{
	int miliseconds;
	miliseconds = rawSeconds*1000;
	if (miliseconds < 250)
	{
			miliseconds = 250;
	}
	return miliseconds;
}
void BaseControlPID (float target, float waitTime, float maxPower = 1)
{
		float Kp = 0.26;
		float Ki = 0.05;
		float Kd = 0.62;

		int RightSensor;
		int LeftSensor;

		RightSensor = SensorValue[RightEncoder]*-1;
		LeftSensor = SensorValue[LeftEncoder];

		int error;

		float proportion;
		int integralRaw;
		float integral;
		int lastError;
		int derivitave;

		float integralActiveZone = inchToTicks(3);


		int finalPower;

		bool timerBool = true;

		SensorValue(LeftEncoder) = 0;
		SensorValue(RightEncoder) = 0;


		clearTimer(T1);

		while(time1[T1] < fixTimerValue(waitTime))
		{
			error = inchToTicks(target/2) - (2*(SensorValue(LeftEncoder)));

			proportion = Kp*error;

			if (abs(error) < integralActiveZone && error != 0)
			{
				integralRaw = integralRaw+error;
			}
			else
			{
				integralRaw = 0;
			}

			integral = Ki*integralRaw;

			derivitave = Kd*(error-lastError);
		 	lastError = error;

		 	if (error == 0)
		 	{
		 			derivitave = 0;
			}

			finalPower = proportion+integral+derivitave; //proportion+derivitave + intergral

			if(finalPower > maxPower*127)
			{
				finalPower = maxPower*127;
			}
			else if(finalPower < -maxPower*127)
			{
				finalPower= -maxPower*127;
			}

			PIDbaseControl(finalPower);

			wait1Msec(40);

			if (error < 30)
			{
					timerBool = false;
			}

			if (timerBool)
			{
				clearTimer(T1);
			}
	}
	PIDbaseControl(0);
}

	task GyroReset()
	{
		//Completely clear out any previous sensor readings by setting the port to "sensorNone"
 SensorType[in4] = sensorNone;
 wait1Msec(1000);
 //Reconfigure Analog Port 8 as a Gyro sensor and allow time for ROBOTC to calibrate it
 SensorType[in4] = sensorGyro;
 wait1Msec(2000);
}

	task PivotLeft100()
{
//Adjust SensorScale to correct the scaling for your gyro
SensorScale[in4] = 103.86;
 //Adjust SensorFullCount to set the "rollover" point. 3600 sets the rollover point to +/-3600
SensorFullCount[in4] = 3600;

//Specify the number of degrees for the robot to turn (1 degree = 10, or 900 = 90 degrees)
 int degrees100 = 1105;

//While the absolute value of the gyro is less than the desired rotation...
 while(abs(SensorValue[in4]) < degrees100)
 {
 //...continue turning
 motor[rightSideDrive] = 50;
 motor[leftSideDrive] = -50;
 }

//Brief brake to stop some drift
 motor[rightSideDrive] = -25;
 motor[leftSideDrive] = 25;
 wait1Msec(250);
 
 baseControl(0,0, 100);
 stopTask(PivotLeft100);
}

	task Straighten()
{

int difference;
int leftStart;
int rightStart;


leftStart = 100;
rightStart = 83;


SensorValue(RightEncoder) = 0;
SensorValue(LeftEncoder) = 0;


	while(1 == 1)
	{
		difference = abs(SensorValue(LeftEncoder) - SensorValue(RightEncoder))/1000;

		if(SensorValue(RightEncoder) == SensorValue(LeftEncoder))
		{
			straightBaseControl(leftStart, rightStart);
		}

		if(SensorValue(RightEncoder) > SensorValue(LeftEncoder))
		{
			straightBaseControl(leftStart, (rightStart - difference));
		}

		if(SensorValue(LeftEncoder) > SensorValue(RightEncoder))
		{
			straightBaseControl((leftStart - difference), rightStart);
		}
	}
}

task BackStraighten()
{
float difference;
float leftStart;
float rightStart;


leftStart = -57.5;
rightStart = -47.5;


SensorValue(RightEncoder) = 0;
SensorValue(LeftEncoder) = 0;

	while(1 == 1)
	{

		difference = abs(SensorValue(LeftEncoder) - SensorValue(RightEncoder))/1000;

		if(SensorValue(RightEncoder) == SensorValue(LeftEncoder))
		{
			straightBaseControl(leftStart, rightStart);
		}

		if(SensorValue(RightEncoder) > SensorValue(LeftEncoder))
		{
			straightBaseControl(leftStart, (rightStart - difference));
		}

		if(SensorValue(LeftEncoder) > SensorValue(RightEncoder))
		{
			straightBaseControl((leftStart - difference), rightStart);
		}
	}
}

task BackStraightenFast()
{
float difference;
float leftStart;
float rightStart;


leftStart = -101;
rightStart = -95;


SensorValue(RightEncoder) = 0;
SensorValue(LeftEncoder) = 0;

	while(1 == 1)
	{

		difference = abs(SensorValue(LeftEncoder) - SensorValue(RightEncoder))/1000;

		if(SensorValue(RightEncoder) == SensorValue(LeftEncoder))
		{
			straightBaseControl(leftStart, rightStart);
		}

		if(SensorValue(RightEncoder) > SensorValue(LeftEncoder))
		{
			straightBaseControl(leftStart, (rightStart - difference));
		}

		if(SensorValue(LeftEncoder) > SensorValue(RightEncoder))
		{
			straightBaseControl((leftStart - difference), rightStart);
		}
	}
}

task main()
{
	while(1==1)
	{
		if(vexRT(Btn8U) == 1)
		{
			startTask(Battery);
			wait1Msec(100);
		}

		if(vexRT[Btn5U] == 1)
		{
			motor[rightTipper] = -63;
			motor[leftTipper] = -63;
		}
		else if(vexRT[Btn5D] == 1)
		{
			motor[rightTipper] = 63;
			motor[leftTipper] = 63;
		}
		else
		{
			motor[rightTipper] = 0;
			motor[leftTipper] = 0;
		}

		if(vexRT(Btn8R) == 1)
		{
			tipControl(120, 120, 650);
			tipControl(0, 0, 0);
			//move tip up
			
				rawliftControl(63, 63); untilPotentiometerLessThan(1400, in1);
			rawliftControl(0, 0);
			wait1Msec(100);
			rawliftControl(-63, -63); untilPotentiometerGreaterThan(1070, in1);
			rawliftControl(0, 0);
			//bring down mobile goal
			
			liftControl(-63, -63, 200);
			rawliftControl(0,0);
			wait1Msec(250);
			//bring up mobile goal
			
			BaseControlPID(42, 0, 0.7);
			//drive to cone

			baseControl(0, 0, 100);
			//stop

			startTask(GyroReset);
			rawliftControl(-63, -63);untilPotentiometerGreaterThan(1800, in1);
			rawliftControl(0, 0);
			wait1Msec(250);
			//bring up mobile goal, gyro

		  startTask(BackStraighten);
		  rawBaseControl(-50, -50);
		  untilLight(2850, in2);
		  //back up to line
		  
		  baseControl(-63, -63, 130);
		  //back up more
		  
		  stopTask(BackStraighten);
		  baseControl(0, 0, 100);
		  stopTask(GyroReset);
		  //back up, end gyro

		  startTask(PivotLeft100);
		  wait1Msec(1000);
		  baseControl(0, 0, 250);
		  startTask(GyroReset);
		  //pivot to the left
		  
		  BaseControlPID(16, 0, 0.5);
		  baseControl(0, 0, 2000);
		  //drive forwards
		  
		  stopTask(GyroReset);
		  startTask(PivotLeft100);
		  wait1Msec(1000);
		  baseControl(0, 0, 250);
		  //pivot left

		  BaseControlPID(1, 0, 1);
			baseControl(153, 100, 25000);	
			//drive to 20 pt
			
			// liftControl(-63, -63, 500);
			// rawliftControl(0, 0);
			//bring mobile goal in
			
		//	BaseControlPID(-36, 0, 1);
			// baseControl(0, 0, 250);
			//back out
		  

		}
	}


}

csiv23 · March 1, 2018, 1:30pm

^^Also ignore the battery function, I had to delete some of it in order for the post to be under the character limit.

biglesliep · March 1, 2018, 1:53pm

Is there a reason in this function that you are passing it leftSide & rightSide, and then re-setting those variables to the value of the motor (I think that’s what’s happening)?

void straightBaseControl (int leftSide, int rightSide)
{
	leftSide = motor[leftSideDrive];
	rightSide = motor[rightSideDrive];
}

csiv23 · March 1, 2018, 4:49pm

I think that might be the issue, maybe the variables aren’t resetting on the straighten function? How could I get it to reset them then?

kypyro · March 1, 2018, 5:40pm

That’s certainly an issue; you’re relying heavily on straightBaseControl changing the value of its parameters. And C doesn’t work that way.

There are many solutions, but given that you only do this in three places, and further that the functions are only two lines long, just replace the function call with what those two lines do. Which is two assignment statements.

A standard solution for something so simple would be a preprocessor macro. Those look like functions, but aren’t; they just substitute text.

A standard solution for having a function change the value of a variable is to pass a pointer to the variable rather than the variable itself.

Your case is so simple, though, you should really consider the first solution: replace the function call with the two assignment statements.

callen · March 1, 2018, 5:42pm

@biglesliep 's point is that passing those values and immediately setting to something else means you’ve never recorded the values passed to the function inside the function. At that point, why bother to pass the function any values at all. I think you meant to do the other way around:

motor[leftSideDrive] = leftSide;
motor[rightSideDrive] = rightSide;

That way you’re setting those motors to the values you put into the function.

callen · March 1, 2018, 5:48pm

I don’t use RobotC, but something looks weird to me about wait1Msec. I’ve seen it used a lot, but never as you’re doing it. Maybe it’s OK, though.

Why this:
liftTime = wait1Msec(liftTime);

instead of:
wait1Msec(liftTime);

each time?

It’s not like you’re returning liftTime (or the others’) value, so there wouldn’t even be a need to record a change in it if it were to change. You’ve done that in at least three different functions.

csiv23 · March 1, 2018, 5:56pm

Thank you guys so much for your replies! I’m really learning a lot. I am going to try the fixes tomorrow as I don’t have my robot with me now, but thank you in advance for helping. I’ll let you guys know how it goes. Thanks a lot!

biglesliep · March 1, 2018, 7:47pm

@callen - I was also kind of stupefied by the wait usage. @csiv23 - as mentioned above, if you just want the program to wait (i.e., keep doing what it’s doing), you just simply write:


wait1Msec(tipTime);

instead of


tipTime = wait1Msec(tipTime);

I’m not exactly sure what the line above does as you’ve got it written.

I.K. this is not your question, but typically the Gyro is initialized/reset just once, at the start of your program while the robot is sitting stock still. I see you’re resetting it twice in the middle of your robot’s pattern.

The gyro is very sensitive to vibration, so you don’t want any part of the robot doing anything during this initialization period; most people put this is the “pre-autonomous” part of the competition template for this reason (I realize that your code is not in a competition template at the moment, but you’d want to do all this before your main while(1 == 1) loop starts).
You’re trying to reset the gyro twice while the robot is driving around and/or has other parts of the robot moving. That’s really not going to work well, methinks.
Why do you need to reset your gyro in the middle of the robot’s movement anyway? Typically one would initialize it at the start of the program, and then just take readings from it as you go along. If you want to turn, say, 90º from where it is now, then just take a reading at the start of the movement, and move 90º from that point (i.e., using relative values, not absolute values).

Again, I realize that none of this probably has anything to do with your OP (sorry), but that’s all my brain could figure out about your code at the moment.

kypyro · March 1, 2018, 8:20pm

biglesliep:

@callen - I was also kind of stupefied by the wait usage. @csiv23 - as mentioned above, if you just want the program to wait (i.e., keep doing what it’s doing), you just simply write:
wait1Msec(tipTime);  
instead of
tipTime = wait1Msec(tipTime);
I’m not exactly sure what the line above does as you’ve got it written.

It just does the wait. The value assigned into tipTime at the end is probably undefined and likely implementation specific. wait1Msec is defined as void, meaning it has no return value. For a variety of historical reasons, it’s possible that something will actually be stored in tipTime after the call to wait1Msec completes. But it’s unimportant, because I believe he never tries to use that value anywhere.

csiv23 · March 2, 2018, 11:26am

So I don’t need to reset the gyro after the start? In other words, I only need to reset the gyro once?

callen · March 2, 2018, 11:38am

In PROS we do two different things. The first is initializing the gyro, for which we use gyroInit. The gyro need to sit still for 2-3 seconds while doing this. So it’s something you really don’t want to do while the robot is driving around. The second is gyroReset, which is a very different beast. All gyroReset does is set the value to 0, meaning you’ve changed your coordinate system. But gyroReset is truly not necessary and is sometimes risky. Yes, you’ll get some drift in your gyro, but don’t worry about that for now. Even if you don’t know the angle your at and what you want to turn to, you can just ask the gyro for the current angle, add or subtract your desired turn, and then rotate until the new angle is acquired.