View unanswered posts | View active topics It is currently Fri Dec 19, 2014 5:25 am






Reply to topic  [ 4 posts ] 
Need help with autonomous programming 
Author Message
Rookie

Joined: Sun Oct 27, 2013 6:42 am
Posts: 1
Post 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.


Sun Oct 27, 2013 6:53 am
Profile
Online
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1362
Post 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.


Mon Oct 28, 2013 11:30 pm
Profile
Online
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1362
Post 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.


Tue Oct 29, 2013 12:29 am
Profile
Rookie

Joined: Thu Oct 03, 2013 4:47 pm
Posts: 9
Post 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


Fri Nov 01, 2013 7:25 pm
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 4 posts ] 

Who is online

Users browsing this forum: No registered users and 2 guests


You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot post attachments in this forum

Search for:
Jump to:  



Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group.
Designed by ST Software for PTF.