Driving Straight with encoders help

  1. 3 months ago

    RednaxNewo

    Feb 13 Its a secret 48778A

    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! :D

    #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);
    }

  2. jpearman

    Feb 16 Moderator, ROBOTC Tech Support, V5 Beta Moderator Los Angeles 8888

    It sounds like slavePower becomes significantly different from masterPower. I would make both globals so you can monitor them in the globals debug window. Make sure that both encoders count in the same direction, that is, as the robot moves forward do they both increase ? You should probably use nMotorEncoder rather than SensorValue as that will compensate for the reverse flag that is set on the right side motors.

 

or Sign Up to reply!