Hello, I have been trying to get my robot to drive straight using a variation of the code from the robotc tutorials, but my robot simply
goes straight for a sec then starts spinning, any help would be appreciated!
#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1, rightEncoder, sensorQuadEncoderOnI2CPort, , AutoAssign )
#pragma config(Sensor, I2C_2, leftEncoder, sensorQuadEncoderOnI2CPort, , AutoAssign )
#pragma config(Motor, port1, rightMotor, tmotorVex393_HBridge, openLoop, encoderPort, I2C_1)
#pragma config(Motor, port2, frontLeftMotor, tmotorServoContinuousRotation, openLoop)
#pragma config(Motor, port4, frontRightMotor, tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor, port10, leftMotor, tmotorVex393_HBridge, openLoop, reversed, encoderPort, I2C_2)
//!!Code automatically generated by ‘ROBOTC’ configuration wizard !!//
#define abs(X) ((X < 0) ? -1 * X : X)
void driveStraightDistance(int tenthsOfIn, int masterPower)
{
int tickGoal = (42 * tenthsOfIn) / 10;
//This will count up the total encoder ticks despite the fact that the encoders are constantly reset.
int totalTicks = 0;
//Initialise slavePower as masterPower - 5 so we don’t get huge error for the first few iterations. The
//-5 value is based off a rough guess of how much the motors are different, which prevents the robot from
//veering off course at the start of the function.
int slavePower = masterPower - 5;
int error = 0;
int kp = 5;
SensorValue[leftEncoder] = 0;
SensorValue[rightEncoder] = 0;
//Monitor ‘totalTicks’, instead of the values of the encoders which are constantly reset.
while(abs(totalTicks) < tickGoal)
{
//Proportional algorithm to keep the robot going straight.
motor[leftMotor] = masterPower;
motor[rightMotor] = slavePower;
motor[frontLeftMotor] = masterPower;
motor[frontRightMotor] = slavePower;
error = SensorValue[leftEncoder] - SensorValue[rightEncoder];
slavePower += error / kp;
SensorValue[leftEncoder] = 0;
SensorValue[rightEncoder] = 0;
wait1Msec(100);
//Add this iteration's encoder values to totalTicks.
totalTicks+= SensorValue[leftEncoder];
}
motor[leftMotor] = 0; // Stop the loop once the encoders have counted up the correct number of encoder ticks.
motor[rightMotor] = 0;
motor[frontLeftMotor] = 0;
motor[frontRightMotor] = 0;
}
task main()
{
//Distances specified in tenths of an inch.
driveStraightDistance(1200,100);
wait1Msec(500); //Stop in between to prevent momentum causing wheel skid.
driveStraightDistance(1200,-100);
wait1Msec(500);
driveStraightDistance(1200,100);
wait1Msec(500);
driveStraightDistance(1200,-100);
}