View unanswered posts | View active topics It is currently Sat Dec 20, 2014 3:04 pm






Reply to topic  [ 13 posts ] 
IR Seeker Auto? 
Author Message
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post IR Seeker Auto?
Sent to MHTS, posted here for other teams to see.

2844:
Hey man, our team would was wondering if you could help us out with some of the programming, as we are having troubles with it once again for this years challenge. Below is one of the many codes we have written, but we would like to be able to integrate the IR seeker into it. Would you be able to help us again?

Code:
#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S2,     Gyro,           sensorI2CHiTechnicGyro)
#pragma config(Sensor, S3,     IRright,        sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S4,     IRleft,         sensorHiTechnicIRSeeker1200)
#pragma config(Motor,  mtr_S1_C1_1,     motorRight,    tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     motorLeft,     tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     tower,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     none,          tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     claw,          tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C3_2,     flag,          tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C4_1,    brake,                tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    RightFlap,            tServoStandard)
#pragma config(Servo,  srvo_S1_C4_3,    LeftFlap,             tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.
#include "C:\Users\Matt\Documents\Valley X Robotics\2013-2014\gyro.h"

#define BOUND(n, l, h) (((n) < (l))? (l): ((n) > (h))? (h): (n))
#define PI                      3.14159
#define CLICKS_PER_REVOLUTION   1440.0          //encoder clicks per revolution
#define GEAR_RATIO              (80.0/120.0)     //motor:wheel=40:80 (use your own gear ratio)
#define WHEEL_DIAMETER          4.0  //use your own wheel size
#define WHEEL_CIRCUMFERENCE     (12.580636)
#define INCHES_PER_CLICK        (0.00615612) //(WHEEL_CIRCUMFERENCE*GEAR_RATIO/CLICKS_PER_REVOLUTION)
#define DEGREES_PER_CLICK       (0.0156)

GYRO  g_Gyro;
float g_turnTarget = 0.0;
bool  g_turnEnabled = false;
float g_turnTolerance = 0.2;    //needs to be tuned
float g_turnKp = 15.0;             //needs to be tuned
float g_driveTarget = 0.0;
bool  g_driveEnabled = false;
float g_driveTolerance = 0.3;
float g_driveKp = 30.0;
float g_armTarget = 0.0;
bool g_armEnabled = false;
float g_armTolerance = 0.5;
float g_armKp = 10.0;

 void SetTurnTarget(float angle)
{
    GyroReset(g_Gyro);
    g_turnTarget = angle;
    g_turnEnabled = true;
}

void TurnTask()
{
    if (g_turnEnabled)
    {
        float error = GyroGetHeading(g_Gyro) - g_turnTarget;
        if (abs(error) > g_turnTolerance)
        {
            //
           // Simple proportional PID control.
            // Limit the outpout to the range of -70 to 70.
            //
            int turnPower = BOUND((int)(g_turnKp*error), -100, 100);
            motor[motorLeft] = turnPower;
            motor[motorRight] = -turnPower;
        }
        else
        {
            motor[motorLeft] = 0;
            motor[motorRight] = 0;
            g_turnEnabled = false;
            nxtDisplayTextLine(7, "err=%f", error);
        }
        nxtDisplayTextLine(3, "Heading=%f", GyroGetHeading(g_Gyro));
        nxtDisplayTextLine(6, "target=%f,tol=%f", g_turnTarget, g_turnTolerance);
    }
}

float GetDriveDistance()
{
    return (float)(nMotorEncoder[motorLeft] + nMotorEncoder[motorRight])/2.0*INCHES_PER_CLICK;
}

void SetDriveTarget(float distance)
{
    nMotorEncoder[motorLeft] = 0;
    nMotorEncoder[motorRight] = 0;
    GyroReset(g_Gyro);
    g_driveTarget = distance;
    g_driveEnabled = true;
}

void DriveTask()
{
    if (g_driveEnabled)
    {
        float driveErr = g_driveTarget - GetDriveDistance();

        if (abs(driveErr) > g_driveTolerance)
        {
            //
            // Simple proportional PID control.
            // Limit the outpout to the range of -100 to 100 for drive
            // and -50 to 50 for turn
            //
            int drivePower = BOUND((int)(g_driveKp*driveErr), -100, 100);
            float turnErr = GyroGetHeading(g_Gyro);
            int turnPower = BOUND((int)(g_turnKp*turnErr), -100, 100);
            motor[motorLeft] = BOUND(drivePower + turnPower, -100, 100);
            motor[motorRight] = BOUND(drivePower - turnPower, -100, 100);
            nxtDisplayTextLine(7, "drvErr=%f", driveErr);
        }
        else
        {
            motor[motorLeft] = 0;
            motor[motorRight] = 0;
            g_driveEnabled = false;
        }
        nxtDisplayTextLine(4, "Distance=%f", GetDriveDistance());
   }
}

void initializeRobot()
{
   servoTarget[brake] = 65;
   servoTarget[RightFlap] = 241;
  servoTarget[LeftFlap] = 18;
  // Place code here to sinitialize servos to starting positions.
  // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
  GyroInit(g_Gyro, Gyro, 0);

  return;
}

task main()
{
    int step = 0;

    StopTask(displayDiagnostics);
    eraseDisplay();
    initializeRobot();

    waitForStart(); // Wait for the beginning of autonomous phase.
    nMotorEncoder[tower] = 0;
   
    while (true)
    {
        GyroTask(g_Gyro);

        nxtDisplayTextLine(5, "Step=%d", step);
        switch (step)
        {
//*****************************Drive Back Parking Zone*************************************

        }

        ArmTask();
        TurnTask();
        DriveTask();
        wait1Msec(10);
    }
}


I know the basics of the IR, like below, but having trouble intergrating it into the code.

Code:
if(SensorValue[IRright] == 7)
            {
               servoTarget[RightFlap] = 140;
            }
           if(SensorValue[IRright] > 7)
            {
               servoTarget[RightFlap] = 241;
            }
             if(SensorValue[IRright] < 7)
            {
               servoTarget[RightFlap] = 241;
            } 



MHTS:
Quote:
The code is very well done. I see you really learn from last year. Good job. First, one thing at a time. I need to get more info to understand your robot design. Also, I would like to help you on the forum instead of via private message because that will also benefit other teams. So I don't have to answer similar questions from another team. You can post smaller issues than the whole thing to the forum. Here are the questions I need answers with:
1. According to your pragmas, you have two IR seekers, IRleft and IRright but the IR code you have only dealt with IRright.
2. Also, the center zone of the IR is 5 but your code is testing with 7. Why? Are you mounting your IR sensor straight ahead or at an angle?
3. What is the RightFlap servo and what do the angle values do?


2844:
1: There are two IR's on the robot, one mounted on the left, and one on the right. Allowing for different lineups from all sides. only one used per autonomous, either left side, or the right side.
2: Yes, right now the IR's are on a mount that can bend back and forth until we figured out our programming issues. Right now they are at a 45 degree angle from the robot frame. Can be changed easily
3: The RightFlap servo is a servo already on the bot, just being used right now to see if the coding would work when it saw the IR. Angles are just the folding down of a block flap.


Sun Jan 05, 2014 1:56 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1368
Post Re: IR Seeker Auto?
Assuming the IR seeker is mounted on the side of the robot pointing to the pendulum direction so when the robot moves forward, it is moving parallel to the pendulum. Your current main code is implementing a state machine. So you could just have the state machine moving the robot forward until the IRSeeker is reading a value 5. However, because of the inaccuracy of the wide zone 5 (see thread viewtopic.php?f=52&t=2780&p=16146&hilit=IR+seeker+inaccuracy#p16146), it may be better to mount the IR seeker pointing forward where the robot is moving so that when the IR beacon is directly on the right side of the robot, the IR seeker will read a value of 8 (or 2 if the IR beacon is directly on the left side of the robot). Then, you can code the state machine to do the following:
Code:
int g_irSeekTarget;
bool g_irSeekEnabled = false;

void SetIRSeekTarget(int irValue)
{
    g_irSeekTarget = irValue;
    g_irSeekEnabled = true;
}

void IRSeekTask()
{
    if (g_irSeekEnabled)
    {
        int error = SensorValue[irSeeker] - g_irSeekTarget;
        if (abs(error) > 0)
        {
            //
            // Simple proportional PID control.
            // Limit the outpout to the range of -50 to 50.
            //
            int drivePower = BOUND(g_irSeekKp*abs(error), -50, 50);
            motor[motorLeft] = drivePower;
            motor[motorRight] = drivePower;
        }
        else
        {
            motor[motorLeft] = 0;
            motor[motorRight] = 0;
            g_irSeekEnabled = false;
            nxtDisplayTextLine(7, "err=%d", error);
        }
    }
}

task main()
{
    int step = 0;

    StopTask(displayDiagnostics);
    eraseDisplay();
    initializeRobot();

    waitForStart(); // Wait for the beginning of autonomous phase.
    servoTarget[RightFlap] = 241;

    while (true)
    {
        nxtDisplayTextLine(5, "Step=%d", step);
        switch (step)
        {
            case 0:
                // step 0: seek ir beacon on the right side.
                // If seeking ir beacon on the left side, use 2 instead.
                SetIRSeekTarget(8);
                step++;
                break;

            case 1:
                // step 1: wait for drive to complete.
                if (g_irSeekEnabled == false)
                {
                    step++;
                }
                break;

            case 2:
                // step 2: dump the block, move up to ramp....
                servoTarget[RightFlap] = 140;
                break;
        }

        IRSeekTask();
        wait1Msec(10);
    }
}


Sun Jan 05, 2014 3:03 am
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: IR Seeker Auto?
Alright lets say the robot is facing forward, and the IR gets put on 2. It goes forward and hits the IR basket, now, what would I need to put in there for it to go forward enough to clear the baskets/ramp, and continue on? The same goes for if its put on basket 1,3, and 4, how do we get it to know to go forward enough afterwards to clear the baskets/ramp?

Do we just add the set inches forward in case 0 and 1 like we have before?


Sun Jan 05, 2014 5:43 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1368
Post Re: IR Seeker Auto?
Team2844 wrote:
Alright lets say the robot is facing forward, and the IR gets put on 2. It goes forward and hits the IR basket, now, what would I need to put in there for it to go forward enough to clear the baskets/ramp, and continue on? The same goes for if its put on basket 1,3, and 4, how do we get it to know to go forward enough afterwards to clear the baskets/ramp?

Do we just add the set inches forward in case 0 and 1 like we have before?

Okay, what is facing forward? Facing the basket or parallel to the pendulum? Assuming you are facing the direction parallel to the pendulum, so now at step zero, the robot starts moving forward until the IR beacon is directly aligned on the side of the robot, then it transitions to step 2. Are you asking what's next? If so, let's think about it for a moment. Since you don't know which basket had the IR beacon, the only thing you can do is to note your starting encoder position before the game starts. If you zero the motor encoders at the beginning and knowing what exact spot you park your robot to begin with and how far is your robot away from the point where it can clear the pendulum (let's say 36 inches). So when you are aligned with the IR beacon, you can read the encoders and figure out how many inches you have traveled after autonomous started. So subtract it from the 36 inches, that's how much you should go forward to clear the pendulum, right?


Sun Jan 05, 2014 10:15 pm
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: IR Seeker Auto?
Yes, sorry about the confusion. We would be parallel to the pendulum, so it hits the beacon, flips the servo, then subtracts the distance traveled, and continues on around the pendulum. I'm now trying to work on programming that, yet, confusing myself again how to program it.

I have the zeroing out of the motor encoder at the beginning

So after the IR:?
(nMotorEncoder[motorLeft] + nMotorEncoder[motorRight]) - drive distance still needed

I'm completely confused

I've tried something like this, but it gets stuck on step 2, flips the servo out at the right time though, and stops after the 48 inches... which is step 3, but showing step 2 on the screen, stuck.
Code:
 

case 0:
SetIRTarget(8);
SetDriveTarget(48.0);
step++;
break;

case 1:
if (g_irseekEnabled == false)
{
step++;
}
break;

case 2:
servoTarget[RightFlap] = 241;
step++;
break;

case 3:
if (g_driveEnabled == false)
{
step++;
}
break;

case 4:
SetTurnTarget(-90.0)
servoTarget[RightFlap] = 140;
step++;
break;

case 5:
if (g_turnEnabled == false)
{
step++;
}
break;

And so on....

}


Sun Jan 05, 2014 11:28 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1368
Post Re: IR Seeker Auto?
Team2844 wrote:
Yes, sorry about the confusion. We would be parallel to the pendulum, so it hits the beacon, flips the servo, then subtracts the distance traveled, and continues on around the pendulum. I'm now trying to work on programming that, yet, confusing myself again how to program it.

I have the zeroing out of the motor encoder at the beginning

So after the IR:?
(nMotorEncoder[motorLeft] + nMotorEncoder[motorRight]) - drive distance still needed

I'm completely confused

I've tried something like this, but it gets stuck on step 2, flips the servo out at the right time though, and stops after the 48 inches... which is step 3, but showing step 2 on the screen, stuck.

First, SetIRSeekTarget is already starting the robot to move forward, so you don't need the SetDriveTarget(48.0). Basically, SetIRSeekTarget is going to move the robot forward until it reaches the IR beacon on the right side and then stopped. Then at step 2, you dump the block but it may take time to complete the dump, so you need to give it a delay (300 msec). At step 4, you read the left and right motor encoders, average them and convert it back to number of inches traveled so far. Then you subtract it from 36.0. That's how far you need to travel to clear the pendulum. Then you call SetDriveTarget to drive the robot forward that much. Then I don't know what you want to do... Probably turn some amount to go around the pendulum, then probably go forward some amount and then turn some more to point towards the ramp, then go forward some distance up the ramp...
Code:
task main()
{
    int step = 0;

    StopTask(displayDiagnostics);
    eraseDisplay();
    initializeRobot();

    waitForStart(); // Wait for the beginning of autonomous phase.
    servoTarget[RightFlap] = 241;
    //
    // Zero the left and right motor encoders.
    //
    nMotorEncoder[motorLeft] = 0;
    nMotorEncoder[motorRight] = 0;

    while (true)
    {
        nxtDisplayTextLine(5, "Step=%d", step);
        GyroTask(g_Gyro);

        switch (step)
        {
            case 0:
                // step 0: seek ir beacon on the right side.
                // If seeking ir beacon on the left side, use 2 instead.
                SetIRSeekTarget(8);
                step++;
                break;

            case 1:
                // step 1: wait for drive to complete.
                if (g_irSeekEnabled == false)
                {
                    step++;
                }
                break;

            case 2:
                // step 2: dump the block.
                servoTarget[RightFlap] = 140;
                ClearTimer(T1);
                break;

            case 3:
                // step 3: wait some time to complete the dumping
                if (time1[T1] > 300)
                {
                    step++;
                }
                break;

            case 4:
                // step 4: continue to go forward.
                float distTraveled = (nMotorEncoder[motorLeft] + nMotorEncoder[motorRight])*INCHES_PER_CLICK/2.0;
                SetDriveTarget(36.0 - distTraveled);
                break;

            case 5:
                // step 5: wait for drive to complete.
                if (g_driveEnabled == false)
                {
                    step++;
                }
                break;

            case 6:
                 ...
        }

        IRSeekTask();
        ArmTask();
        TurnTask();
        DriveTask();
        wait1Msec(10);
    }
}


Mon Jan 06, 2014 12:36 am
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: IR Seeker Auto?
Its still getting stuck at step 2 though. And i don't see why. The time is in there.

That, and doesn't the SetDriveTarget you have in case 4, zero the encoders when it starts its case, before looking at the left and right motor encoders values from the IR case. Making the encoders useless to look at since they would already reset?


Mon Jan 06, 2014 1:06 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1368
Post Re: IR Seeker Auto?
Team2844 wrote:
Its still getting stuck at step 2 though. And i don't see why. The time is in there.

That, and doesn't the SetDriveTarget you have in case 4, zero the encoders when it starts its case, before looking at the left and right motor encoders values from the IR case. Making the encoders useless to look at since they would already reset?

Good call, you found a bug. But you already knew where it was. It got stuck in step 2. So the question is: who is supposed to advance step 2 to step 3? So the answer is: After clearing the timer, it needs to advance to step 3. So what do you think how it should be fixed? And yes, SetDriveTarget will zero the encoders but do you see we read the encoders before calling SetDriveTarget? So you shouldn't have to worry about that.
BTW, there are two bugs. After fixing the "stuck at step 2", you will be stuck at step 4 as well. Same problem though.


Mon Jan 06, 2014 2:27 am
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: IR Seeker Auto?
Would it be the fact that there is no step++; in the 2 cases?


Mon Jan 06, 2014 12:03 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1368
Post Re: IR Seeker Auto?
Team2844 wrote:
Would it be the fact that there is no step++; in the 2 cases?

Yep. If the step++ is not there, step 2 will be executed over and over again, keep setting the RightFlap angle and clearing Timer T1 over and over again. Generally, every step in a state machine should have a way to get out of that state and move on to another one.


Mon Jan 06, 2014 2:51 pm
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: IR Seeker Auto?
Alright. I added in the step++; to the cases, but im running into a new problem. After it does the IR, it comes out readin negative -35inches, when really it ran 35 inches forward. Now, when it tries to go the 65 inches that I have set in. It really is telling itself 100 inches, since its starting from -35in. Working on fixing this.. trying to atleast.


Mon Jan 06, 2014 8:33 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1368
Post Re: IR Seeker Auto?
Make sure you connected the motors with the correct polarity (i.e. red wire goes to + and black goes to - on the Tetrix motor controller). If the wiring is correct, run the following program and manually rotate the wheels and see what the encoder numbers look like. Also make sure the encoders are connected in the correct orientation.
Code:
#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S2,     Gyro,           sensorI2CHiTechnicGyro)
#pragma config(Sensor, S3,     IRright,        sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S4,     IRleft,         sensorHiTechnicIRSeeker1200)
#pragma config(Motor,  mtr_S1_C1_1,     motorRight,    tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     motorLeft,     tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     tower,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     none,          tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     claw,          tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C3_2,     flag,          tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C4_1,    brake,                tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    RightFlap,            tServoStandard)
#pragma config(Servo,  srvo_S1_C4_3,    LeftFlap,             tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

task main()
{
    StopTask(displayDiagnostics);
    eraseDisplay();

    nMotorEncoder[motorLeft] = 0;
    nMotorEncoder[motorRight] = 0;

    while (true)
    {
        nxtDisplayTextLine(0, "LEnc=%d", nMotorEncoder[motorLeft]);
        nxtDisplayTextLine(1, "REnc=%d", nMotorEncoder[motorRight]);
        wait1Msec(100);
    }
}


Mon Jan 06, 2014 8:41 pm
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 86
Post Re: IR Seeker Auto?
Im an idiot. Forgot to zero the encoders at the beginning. Fixed it.

Thanks for the help man!


Mon Jan 06, 2014 8:48 pm
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 13 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.