ROBOTC.net forums
http://robotc.net/forums/

Need help with autonomous programming
http://robotc.net/forums/viewtopic.php?f=1&t=7092
Page 1 of 1

Author:  ROBOC [ Sun Oct 27, 2013 6:53 am ]
Post subject:  Need help with autonomous programming

Hey guys,
I'm part of an FTC team and I am currently trying to program the autonomous for block party. I'm fairly new to robot c, I know all the basics and some of the commands but still not very fluent with the language. I'm trying to program the autonomous using virtual worlds. When I tried to program the robot to stop when it's facing the IR beacon, it doesn't work. The robot turns towards the beacon and then starts shaking erratically for some reason. Here's my code.


#pragma config(Hubs, S1, HTMotor, HTMotor, none, none)
#pragma config(Sensor, S1, , sensorI2CMuxController)
#pragma config(Sensor, S2, IRSeeker, sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S3, light, sensorLightActive)
#pragma config(Sensor, S4, sonar, sensorSONAR)
#pragma config(Motor, motorA, gripperMotor, tmotorNXT, PIDControl, encoder)
#pragma config(Motor, mtr_S1_C1_1, rightMotor, tmotorTetrix, openLoop, encoder)
#pragma config(Motor, mtr_S1_C1_2, leftMotor, tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor, mtr_S1_C2_1, armMotor, tmotorTetrix, openLoop, encoder)
#pragma config(Motor, mtr_S1_C2_2, spinnerMotor, tmotorTetrix, openLoop, encoder)
task main()
{
while(SensorValue(IRSeeker) != 5)
{
if(SensorValue(IRSeeker) == 1)
{
motor(rightMotor) = 60;
motor(leftMotor) = -60;
}
if(SensorValue(IRSeeker) == 2)
{
motor(rightMotor) = 45;
motor(leftMotor) = -45;
}
if(SensorValue(IRSeeker) == 3)
{
motor(rightMotor) = 30;
motor(leftMotor) = -30;
}
if(SensorValue(IRSeeker) == 4)
{
motor(rightMotor) = 20;
motor(leftMotor) = -20;
}

if(SensorValue(IRSeeker) == 6)
{
motor(rightMotor) = -20;
motor(leftMotor) = 20;
}
if(SensorValue(IRSeeker) == 7)
{
motor(rightMotor) = -35;
motor(leftMotor) = 35;
}
if(SensorValue(IRSeeker) == 8 )
{
motor(rightMotor) = -50;
motor(leftMotor) = 50;
}
if(SensorValue(IRSeeker) == 9)
{
motor(rightMotor) = -60;
motor(leftMotor) = 60;
}
if(SensorValue(IRSeeker) == 0)
{
motor(rightMotor) = 100;
motor(leftMotor) = -100;
}

}

motor(rightMotor) = -100;
motor(leftMotor) = -100;
wait1Msec(10000);

}


The last bit was just for testing.

I'm also having problems with the ultrasonic sensor. The robot stops at different distances each time when I try it. Here's my code.


#pragma config(Hubs, S1, HTMotor, HTMotor, none, none)
#pragma config(Sensor, S1, , sensorI2CMuxController)
#pragma config(Sensor, S2, gyro, sensorI2CHiTechnicGyro)
#pragma config(Sensor, S3, light, sensorLightActive)
#pragma config(Sensor, S4, sonar, sensorSONAR)
#pragma config(Motor, mtr_S1_C1_1, rightMotor, tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor, mtr_S1_C1_2, leftMotor, tmotorTetrix, openLoop, encoder)
#pragma config(Motor, mtr_S1_C2_1, armMotor, tmotorTetrix, openLoop, encoder)
#pragma config(Motor, mtr_S1_C2_2, intakeMotor, tmotorTetrix, openLoop, encoder)

task main()
{
nMotorEncoder (rightMotor) = 0;
nMotorEncoder (leftMotor) = 0;

while(nMotorEncoder (rightMotor) < 400)
{
motor(rightMotor) = 100;
motor(leftMotor) = 100;

}

motor(rightMotor) = 0;
motor(leftMotor) = 0;

while(SensorValue[gyro] < 695)
{
//Quickly turn to the right
motor[rightMotor] = -75;
motor[leftMotor] = 75;
}

//Stop the robot
motor[rightMotor] = 0;
motor[leftMotor] = 0;

if(SensorValue(sonar) > 82)
{

while(SensorValue(sonar) > 82)
{
motor[rightMotor] = 100;
motor[leftMotor] = 100;

}
}

motor[rightMotor] = 0;
motor[leftMotor] = 0;




}

Help is much appreciated, thanks.

Author:  MHTS [ Mon Oct 28, 2013 11:30 pm ]
Post subject:  Re: Need help with autonomous programming

You need to post your code with the code tags so it will preserve the indentation. I formatted your code and repost it with code tags below.
Code:
#pragma config(Hubs, S1, HTMotor, HTMotor, none, none)
#pragma config(Sensor, S1, , sensorI2CMuxController)
#pragma config(Sensor, S2, IRSeeker, sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S3, light, sensorLightActive)
#pragma config(Sensor, S4, sonar, sensorSONAR)
#pragma config(Motor, motorA, gripperMotor, tmotorNXT, PIDControl, encoder)
#pragma config(Motor, mtr_S1_C1_1, rightMotor, tmotorTetrix, openLoop, encoder)
#pragma config(Motor, mtr_S1_C1_2, leftMotor, tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor, mtr_S1_C2_1, armMotor, tmotorTetrix, openLoop, encoder)
#pragma config(Motor, mtr_S1_C2_2, spinnerMotor, tmotorTetrix, openLoop, encoder)

task main()
{
    while(SensorValue(IRSeeker) != 5)
    {
        if(SensorValue(IRSeeker) == 1)
        {
            motor(rightMotor) = 60;
            motor(leftMotor) = -60;
        }
        if(SensorValue(IRSeeker) == 2)
        {
            motor(rightMotor) = 45;
            motor(leftMotor) = -45;
        }
        if(SensorValue(IRSeeker) == 3)
        {
            motor(rightMotor) = 30;
            motor(leftMotor) = -30;
        }
        if(SensorValue(IRSeeker) == 4)
        {
            motor(rightMotor) = 20;
            motor(leftMotor) = -20;
        }

        if(SensorValue(IRSeeker) == 6)
        {
            motor(rightMotor) = -20;
            motor(leftMotor) = 20;
        }
        if(SensorValue(IRSeeker) == 7)
        {
            motor(rightMotor) = -35;
            motor(leftMotor) = 35;
        }
        if(SensorValue(IRSeeker) == 8 )
        {
            motor(rightMotor) = -50;
            motor(leftMotor) = 50;
        }
        if(SensorValue(IRSeeker) == 9)
        {
            motor(rightMotor) = -60;
            motor(leftMotor) = 60;
        }
        if(SensorValue(IRSeeker) == 0)
        {
            motor(rightMotor) = 100;
            motor(leftMotor) = -100;
        }
    }

    motor(rightMotor) = -100;
    motor(leftMotor) = -100;
    wait1Msec(10000);
}

I don't quite understand your code. It looks like you were trying to do Proportional control the hard way. If that's what you were doing, it could be simplified like below:
Code:
#pragma config(Hubs, S1, HTMotor, HTMotor, none, none)
#pragma config(Sensor, S1, , sensorI2CMuxController)
#pragma config(Sensor, S2, IRSeeker, sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S3, light, sensorLightActive)
#pragma config(Sensor, S4, sonar, sensorSONAR)
#pragma config(Motor, motorA, gripperMotor, tmotorNXT, PIDControl, encoder)
#pragma config(Motor, mtr_S1_C1_1, rightMotor, tmotorTetrix, openLoop, encoder)
#pragma config(Motor, mtr_S1_C1_2, leftMotor, tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor, mtr_S1_C2_1, armMotor, tmotorTetrix, openLoop, encoder)
#pragma config(Motor, mtr_S1_C2_2, spinnerMotor, tmotorTetrix, openLoop, encoder)

#define KP              15.0    //Need to tune this value
#define TARGET_VALUE    5

task main()
{
    while (true)
    {
        int irDir = SensorValue[IRSeeker];
        int motorPower = KP*(irDir - TARGET_VALUE);
        motor[leftMotor] = motorPower;
        motor[rightMotor] = -motorPower;
        wait1Msec(50);
    }
}

I think the above code is approximately equivalent to what you want to do. However, you need to understand the characteristic of the IRSeeker. Sometimes, the IRSeeker will lose track of the beacon and return a value 0. When that happens, the above code will turn left hard. Imagine you are already facing the IR beacon right on and all the sudden the IRSeeker decided to give you a zero, then your robot will jerk left and really losing the IR beacon. That's very frustrating. The way to prevent this from happening is that you never allow zero to be a valid value (except possibly for the very first time before the robot "finds" the beacon). So you need to keep track of the previous IRSeeker value. If you ever get a zero from the IRSeeker, you throw the zero away and use the previous value instead.
Regarding the explanation of why your code behaved the way you described it. There are two possibilities. The first one is just like what I said above that the IRSeeker may lose the target and return you a zero from time to time that jerk it off the target. But more likely, it's the oscillation of the "proportional" control. Imagine when the IRSeeker gives you a value 9, your code will turn right very hard. It turns so hard that it quickly passes the beacon and now it gives you a 1. Then your robot will turn left very hard. It turns so hard that it quickly passes the beacon again and gives you a 9 again. The cycle goes on and on. This is called oscillation. The fix is to lower the +60 and -60 values to something smaller. Now that we explained what might have happened, instead of changing your code and tuning all the 20, 30, 35, 45, 60 numbers, you can just tune KP in the new code above. Lowering KP will diminish the oscillation. However, if KP is too low, then motorPower will be too small when it gets close to the target so it will never reach target. That's call steady state error. So it is important to tune KP correctly.

Author:  MHTS [ Tue Oct 29, 2013 12:29 am ]
Post subject:  Re: Need help with autonomous programming

Regarding your ultrasonic sensor question. Again, your formatted code is reposted below:
Code:
#pragma config(Hubs, S1, HTMotor, HTMotor, none, none)
#pragma config(Sensor, S1, , sensorI2CMuxController)
#pragma config(Sensor, S2, gyro, sensorI2CHiTechnicGyro)
#pragma config(Sensor, S3, light, sensorLightActive)
#pragma config(Sensor, S4, sonar, sensorSONAR)
#pragma config(Motor, mtr_S1_C1_1, rightMotor, tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor, mtr_S1_C1_2, leftMotor, tmotorTetrix, openLoop, encoder)
#pragma config(Motor, mtr_S1_C2_1, armMotor, tmotorTetrix, openLoop, encoder)
#pragma config(Motor, mtr_S1_C2_2, intakeMotor, tmotorTetrix, openLoop, encoder)

task main()
{
    nMotorEncoder (rightMotor) = 0;
    nMotorEncoder (leftMotor) = 0;
    //
    // Move forward.
    //
    while(nMotorEncoder (rightMotor) < 400)
    {
        motor(rightMotor) = 100;
        motor(leftMotor) = 100;

    }

    motor(rightMotor) = 0;
    motor(leftMotor) = 0;
    //
    // Turn right.
    // Note: this code doesn't work.
    // Gyro gives you turn rate not heading.
    //
    while(SensorValue[gyro] < 695)
    {
        //Quickly turn to the right
        motor[rightMotor] = -75;
        motor[leftMotor] = 75;
    }

    //Stop the robot
    motor[rightMotor] = 0;
    motor[leftMotor] = 0;
    //
    // Move forward until obstacle is 82cm in front.
    //
    if(SensorValue(sonar) > 82)
    {

        while(SensorValue(sonar) > 82)
        {
            motor[rightMotor] = 100;
            motor[leftMotor] = 100;

        }
    }

    motor[rightMotor] = 0;
    motor[leftMotor] = 0;
}

There are several problems in your code. The first one is about the turn code using the gyro. I assume you want to turn a fixed angle but you were reading the gyro as if it were an encoder. That doesn't work for gyro. Encoders give you the current wheel position but gyro does not give you the current heading of the robot. Instead, it gives you the "turn rate" (i.e. how fast you are turning). In order to determine the heading of the robot, you need to integrate the gyro value against time. That's a relatively complex operation. Please refer to the following discussing thread on how to do this.
viewtopic.php?f=52&t=3152

Alternatively, you can use the encoders to determine how much you have turned. For example:
Code:
task main()
{
    nMotorEncoder[rightMotor] = 0;
    nMotorEncoder[leftMotor] = 0;
    while(nMotorEncoder[leftMotor] - nMotorEncoder[rightMotor] < 4000)
    {
        //Quickly turn to the right
        motor[rightMotor] = -75;
        motor[leftMotor] = 75;
    }
}

The next problem is about why the ultrasonic code doesn't stop at the same distance every time. Again, there are several possible explanations but most likely, it is a combination of the fact that you are using 100% motor power which means you are running full speed. What happens when you reached 82cm? Your code will try to stop the robot but will the robot stop immediately? It won't. That's just physics. You are going to run pass 82cm. But still, it might stop at 75cm and should still be approximately consistent every time. I suspect when the robot is running full speed, the vibration of the robot may generate "noise" with the ultrasonic sensor (i.e. the sensor may give you incorrect values). Slowing down the robot close to the end may help in that regard. Therefore, you may want to use proportion control here too. For example:
Code:
#define KP                   2.0     //need to tune this value
#define TARGET_DIST  82
task main()
{
    while (true)
    {
        int sonarValue = SensorValue(sonar);
        int motorPower = (sonarValue - TARGET_DIST)*KP;
        motor[leftMotor] = motorPower;
        motor[rightMotor] = motorPower;
        wait1Msec(50);
    }
}

Finally, adding a filter to the ultrasonic data stream may filter out the undesirable noise. But that's more advanced stuff. Just try the above first and see how it goes.

Author:  homerun33ss [ Fri Nov 01, 2013 7:25 pm ]
Post subject:  Re: Need help with autonomous programming

MHTS wrote:
Code:
#pragma config(Hubs, S1, HTMotor, HTMotor, none, none)
#pragma config(Sensor, S1, , sensorI2CMuxController)
#pragma config(Sensor, S2, IRSeeker, sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S3, light, sensorLightActive)
#pragma config(Sensor, S4, sonar, sensorSONAR)
#pragma config(Motor, motorA, gripperMotor, tmotorNXT, PIDControl, encoder)
#pragma config(Motor, mtr_S1_C1_1, rightMotor, tmotorTetrix, openLoop, encoder)
#pragma config(Motor, mtr_S1_C1_2, leftMotor, tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor, mtr_S1_C2_1, armMotor, tmotorTetrix, openLoop, encoder)
#pragma config(Motor, mtr_S1_C2_2, spinnerMotor, tmotorTetrix, openLoop, encoder)

#define KP              15.0    //Need to tune this value
#define TARGET_VALUE    5

task main()
{
    while (true)
    {
        int irDir = SensorValue[IRSeeker];
        int motorPower = KP*(irDir - TARGET_VALUE);
        motor[leftMotor] = motorPower;
        motor[rightMotor] = -motorPower;
        wait1Msec(50);
    }
}

I think the above code is approximately equivalent to what you want to do. However, you need to understand the characteristic of the IRSeeker. Sometimes, the IRSeeker will lose track of the beacon and return a value 0. When that happens, the above code will turn left hard. Imagine you are already facing the IR beacon right on and all the sudden the IRSeeker decided to give you a zero, then your robot will jerk left and really losing the IR beacon. That's very frustrating. The way to prevent this from happening is that you never allow zero to be a valid value (except possibly for the very first time before the robot "finds" the beacon). So you need to keep track of the previous IRSeeker value. If you ever get a zero from the IRSeeker, you throw the zero away and use the previous value instead.
Regarding the explanation of why your code behaved the way you described it. There are two possibilities. The first one is just like what I said above that the IRSeeker may lose the target and return you a zero from time to time that jerk it off the target. But more likely, it's the oscillation of the "proportional" control. Imagine when the IRSeeker gives you a value 9, your code will turn right very hard. It turns so hard that it quickly passes the beacon and now it gives you a 1. Then your robot will turn left very hard. It turns so hard that it quickly passes the beacon again and gives you a 9 again. The cycle goes on and on. This is called oscillation. The fix is to lower the +60 and -60 values to something smaller. Now that we explained what might have happened, instead of changing your code and tuning all the 20, 30, 35, 45, 60 numbers, you can just tune KP in the new code above. Lowering KP will diminish the oscillation. However, if KP is too low, then motorPower will be too small when it gets close to the target so it will never reach target. That's call steady state error. So it is important to tune KP correctly.


Thanks for posting this. I too had a code that was ridiculously long and confusing and not that effective. This code works brilliantly with some tweaking and small additions. :D

Page 1 of 1 All times are UTC - 5 hours [ DST ]
Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group
http://www.phpbb.com/