I have a standard VEX Clawbot, which I've been trying to make go straight for some time.
I've been following this guide: http://www.education.rec.ri.cmu.edu/products/cortex_video_trainer/lesson/3-5AutomatedStraightening2.html
This is my code:
#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1, , sensorQuadEncoderOnI2CPort, , AutoAssign )
#pragma config(Sensor, I2C_2, , sensorQuadEncoderOnI2CPort, , AutoAssign )
#pragma config(Motor, port1, leftMotor, tmotorVex393_HBridge, openLoop, driveLeft, encoderPort, I2C_1)
#pragma config(Motor, port10, rightMotor, tmotorVex393_HBridge, openLoop, reversed, driveRight, encoderPort, I2C_2)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
void GOforwards()
{
nMotorEncoder[rightMotor]=0;
nMotorEncoder[leftMotor]=0;
int rightEncoder = abs(nMotorEncoder[rightMotor]);
int leftEncoder = abs(nMotorEncoder[leftMotor]);
wait1Msec(2000);
motor[rightMotor] = 60;
motor[leftMotor] = 60;
while (rightEncoder < 2000)
{
if (rightEncoder > leftEncoder)
{
motor[rightMotor] = 50;
motor[leftMotor] = 60;
}
if (rightEncoder < leftEncoder)
{
motor[rightMotor] = 60;
motor[leftMotor] = 50;
}
if (rightEncoder == leftEncoder)
{
motor[rightMotor] = 60;
motor[leftMotor] = 60;
}
}
motor[rightMotor] = 0;
motor[leftMotor] = 0;
}
task main()
{
GOforwards();
}
I am using integrated Encoders.
When I run the code my robot runs without stopping and the Encoder values diverge quickly. This is a video of the code running from the debugger windows:
https://www.youtube.com/watch?time_continue=2&v=vs1Cc3xnDtM
I am not sure why the power to the wheels never changes, or why it seems to believe that the Encoder values are equal... much less why it runs off into oblivion when the code should exit the while loop once the right encoder's absolute value exceeds 2000.
Any help would be appreciated.