Short answers first, then we will go into more detail.
Two main problems
You allow both autonomous and driver control tasks to run at the same time.
You will need to wait at the end of the pre-auton function for the competition to start otherwise the default competition template will write on the LCD.
FromEncoder is a variable, it will not update on itâs own, you need to read the encoder.
This is probably not a programming issue, check gearing etc. as others have said.
Same as issue 1, block at the end of pre-auton.
Some more details.
LCD
The robotic competition template is code that implements a main task and one or two useful functions, nothing special about it. When this task starts it first calls the pre_auton() function and then enters an infinite loop monitoring the internal flags that indicate the enabled/disabled and autonomous/driver states. When disabled it displays a running counter so you know the code is running. The problem you have found is that anything you may write to the LCD in the pre_auton() function is overwritten by this code, there are three ways to deal with this.
- Modify the competition template (or a copy of it) so it doesnât write to the LCD
- Use you own competition template code
- Stop at the end of pre_auton and only return to the template code when the competition starts. This only works once after power on, disabling the robot after being enabled does not run pre_auton again.
I modified you code to implement scheme 3, the end of pre_auton now looks like this.
// delay so we can see selection when not in competition mode
wait1Msec(1000);
// Now we will block here until enabled
while (bIfiRobotDisabled)
wait1Msec(10);
}
ROBOTC also has a habit of writing the program name to the LCD every time a task is started and can leave characters where not wanted on the LCD. Usually a good idea to clear the LCD at the beginning of each task.
There are several ways to write numbers onto the LCD. The most simple is to use the function displayLCDNumber, you had tried to use displayLCDChar in several places by mistake. So code like this
displayLCDChar(0,7,FromEncoder);
should have been this
displayLCDNumber(0, 7, FromEncoder, 5);
you were also trying to display encoder values incorrectly in the driver control code;
this
displayLCDChar(1,0,RightDriveEncoder);
should be this
displayLCDNumber(1, 0, SensorValue RightDriveEncoder ] , 5);
There is also another way to display a number by using a function called sprint to create a string variable in a format you define. The above would look like this using sprintf.
char str[32]; // define this only once somewhere
sprintf( str, "%5d", SensorValue RightDriveEncoder ] );
displayLCDString(1, 0, str);
IME not moving
The original code is as follows.
void ClawScoreTOLoaderRED()
{
//Start Cycle
OKToMove = 1;
while(OKToMove == 1)
{
if(FromEncoder > RedPickUpPoint2 - RedPickUpAccuracy || FromEncoder < RedPickUpPoint2 + RedPickUpAccuracy)
{
motor[TurntableMotor] = 0;
OKToMove = 0;
}
else
{
Distance = RedPickUpPoint2 - FromEncoder;
Distance = abs(Distance);
displayLCDString(0,0, "IME =");
displayLCDChar(0,7,FromEncoder);
if(Distance > 500)
{
TargetSpeed = 127;
}
else
{
TargetSpeed = Distance / 1100 * 127;
if(TargetSpeed < 23)
{
TargetSpeed = 23;
}
}
motor[TurntableMotor] = TargetSpeed;
}
}
}
You compare a variable âFromEncoderâ to an upper and lower limit trying to move the turntable. There may be other issues with the logic but the fundamental problem is that none of the variables in the comparison will ever change. I assume you were hoping that FromEncoder would change as the IME moved, sorry, not in the C language. So perhaps this was what you were after.
void ClawScoreTOLoaderRED()
{
//Start Cycle
OKToMove = 1;
while(OKToMove == 1)
{
FromEncoder = SensorValue[TurntableEncoder];
if(FromEncoder > RedPickUpPoint2 - RedPickUpAccuracy || FromEncoder < RedPickUpPoint2 + RedPickUpAccuracy)
{
motor[TurntableMotor] = 0;
OKToMove = 0;
}
else
{
Distance = RedPickUpPoint2 - FromEncoder;
Distance = abs(Distance);
displayLCDString(0,0, "IME = ");
displayLCDNumber(0, 6, FromEncoder, 5);
//displayLCDChar(0,7,FromEncoder);
if(Distance > 500)
{
TargetSpeed = 127;
}
else
{
TargetSpeed = Distance / 1100 * 127;
if(TargetSpeed < 23)
{
TargetSpeed = 23;
}
}
motor[TurntableMotor] = TargetSpeed;
}
wait1Msec(10);
}
}
The encoders in the code are a little mis-configured, what in fact is connected? You have defined only one IME as IME_3, IME 1 and 2 are not defined, not sure that will actually work if there is only one. You also have two quad-encoders on the digital ports, are these used?
Anyway, here is some revised code that you can work with on the next phase of debugging.
skyrise_state_rev1.c (14.7 KB)
Some of the problems I have include;