View unanswered posts | View active topics It is currently Sun May 24, 2015 4:11 am






Reply to topic  [ 12 posts ] 
Is it possible. IR scanning.... 
Author Message
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 91
Post Is it possible. IR scanning....
We are a sister team of Team 2844, Team 8640 using the same account. The code we are working off of is their old code from last year.

We understand from this code, that is was written to drive a straight line, until it hit a certain value, in this case 2, and stop, and swing the servo over, to drop a block in a basket. What we wanted to know is, is it possible to start in the parking zone, scan the field before moving, and depending on what zone the beacon would be in, (2,3,4 maybe in our setup), could it run a different program depending on the position/zone the tower is in?

Code:
#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     IR,             sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S3,     Gyro,           sensorI2CHiTechnicGyro)
#pragma config(Sensor, S4,     none,           sensorNone)
#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,     wheel,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     none,          tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     rear,          tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     front,         tmotorTetrix, openLoop, reversed)
#pragma config(Servo,  srvo_S1_C4_1,    Latch,                tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    Push,                 tServoStandard)
#pragma config(Servo,  srvo_S1_C4_3,    Door,                 tServoStandard)
#pragma config(Servo,  srvo_S1_C4_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.
#include "C:\Users\Trojans\Documents\Trojans8640\2013-2014\gyro.h"
#include "C:\Users\Trojans\Documents\Trojans8640\2013-2014\menu_helper.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.0040)

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_forkTarget = 0.0;
bool  g_forkEnabled = false;
float g_forkTolerance = 1.0;
float g_forkKp = 80.0;
float g_irSeekKp = 12.0;
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[IR] - 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), -100, 100);
            motor[motorLeft] = drivePower;
            motor[motorRight] = drivePower;
        }
        else
        {
            motor[motorLeft] = 0;
            motor[motorRight] = 0;
            g_irSeekEnabled = false;
            nxtDisplayTextLine(7, "err=%d", error);
        }
    }
}

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 SetIRDriveTarget(float distance)
{
    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 SetForkTarget(float angle)
{
  g_forkTarget = angle;
  g_forkEnabled = true;
}

float GetForkPosition()
{
    return (float)nMotorEncoder[front]*DEGREES_PER_CLICK;
}

void ForkTask()
{
  if (g_forkEnabled)
  {
    float error = g_forkTarget - GetForkPosition();
    nxtDisplayTextLine(4, "ForkPos=%f", GetForkPosition());
    if (abs(error) > g_forkTolerance)
    {
      motor[front] = BOUND((int)(g_forkKp*error), -100, 100);
    }
    else
    {
      motor[front] = 0;
      g_forkEnabled = false;
    }
  }
}

void initializeRobot()
{
   servoTarget[latch] = 128;
   servoTarget[Push] = 128;
  servoTarget[Door] = 128;
  // 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;

    eraseDisplay();
    initializeRobot();

    bDisplayDiagnostics = false;
     StartTask(runMenu);
     eraseDisplay();
    waitForStart(); // Wait for the beginning of autonomous phase.
    nMotorEncoder[motorLeft] = 0;
    nMotorEncoder[motorRight] = 0;
    nMotorEncoder[front] = 0;

    waitForStart(); // Wait for the beginning of autonomous phase.


     StopTask(runMenu);
     eraseDisplay();
     bDisplayDiagnostics = true;


     wait1Msec(delay*1000);

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

        switch (step)
        {


        //*********************WAIT TIME********************************

            case 0:
                // wait for set time. Motor at 0.
                motor[front] = 0;
                ClearTimer(T1);
                step++;
                break;

            case 1:
                // 1 second = 1000
                if (time1[T1] > 2000)//<------ Time change here
                {
                    step++;
                }
                break;

       //*********************Regular Autonomous********************************

      case 2:
                // step 12: arm 30 degrees.
                SetForkTarget(5);
                step++;
                break;

            case 3:
                // step 3: wait to complete.
                if (g_forkEnabled == false)
                {
                    step++;
                }
                break;
               
            case 4:
                // step 0: seek ir beacon on the right side.
                // If seeking ir beacon on the left side, use 2 instead.
                    SetIRSeekTarget(2);
                step++;
                break;

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

            case 6:
                // step 2: dump the block.
                servoTarget[door] = 200;
                ClearTimer(T1);
                step++;
                break;

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


        }

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


Mon Jan 19, 2015 10:52 pm
Profile
Professor
User avatar

Joined: Sat Aug 31, 2013 9:15 am
Posts: 256
Post Re: Is it possible. IR scanning....
It all depends on your setup. For instance, with my team, we get a reading of 0 for center structure position 3, and then a reading of 3 in positions 2 and 1. Though, it could be that a different setup would give different numbers for each position. I would recommend creating a program just to display the IR sensor position value on the screen, and see what readings it gives you for the three positions.

_________________
FTC Team 6100 Chariots of Fire - Programmer (2012-2013)
FTC Team 7468 Blue Chariots of Fire - Programmer (2013-2014)
FTC Team 7468 Blue Chariots of Fire - Mentor (2014-2015)
Check out our team website at http://bluechariotsoffire.com/.


Tue Jan 20, 2015 12:27 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1516
Post Re: Is it possible. IR scanning....
Team2844 wrote:
We are a sister team of Team 2844, Team 8640 using the same account. The code we are working off of is their old code from last year.

We understand from this code, that is was written to drive a straight line, until it hit a certain value, in this case 2, and stop, and swing the servo over, to drop a block in a basket. What we wanted to know is, is it possible to start in the parking zone, scan the field before moving, and depending on what zone the beacon would be in, (2,3,4 maybe in our setup), could it run a different program depending on the position/zone the tower is in?

Yes, this is possible. See the videos of our autonomous scoring the center goal at all 3 positions.
https://www.youtube.com/watch?v=9YlX7rq9Gxg
https://www.youtube.com/watch?v=ystuHF5e85s
https://www.youtube.com/watch?v=R3-2054IrQs

For our arrangement, we read 7, 8 and 9 for positions 1, 2 and 3.


Tue Jan 20, 2015 7:31 am
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 91
Post Re: Is it possible. IR scanning....
Alright, now just to figure out how to program it. This is going to take awhile.. haha


Wed Jan 21, 2015 1:05 am
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 91
Post Re: Is it possible. IR scanning....
What are we doing wrong here??

The program is able to see the different locations, and start whichever program is listed, but then starts to have problems as the robot turns and heads a different direction. The problem right now, as it seems, is that the IR seeker is still running throughout the program. So as it moves closer to center and turns, the sensor then reads a new value, and jumps to whatever that case says depending on what case its on. Example 1: of what I have seen.... The tower is at position 3, it will run, and go all the way to case 6/7, but then the sensor reads a value of 3, and it jumps to case 8 of position 2. (90 degree left turn instead of a 45 degree right turn). Example 2: Robot back in the starting position, with the ir beacon OFF on position 1. The robot will run the program correctly. If I turn the ir beacon back on, the position 1 program goes crazy after moving forward and turning, making the sensor have a different value.

How would I go about telling the sensor to stop reading after case 0 or 1?

Code:
#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S2,     Gyro,           sensorI2CHiTechnicGyro)
#pragma config(Sensor, S3,     HTIRS2,         sensorI2CCustom)
#pragma config(Motor,  mtr_S1_C1_1,     motorRight,    tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorLeft,     tmotorTetrix, openLoop, reversed)
#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,     belt,          tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C3_2,     none,          tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C4_1,    Lock,                 tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    tube,                 tServoStandard)
#pragma config(Servo,  srvo_S1_C4_3,    SIR,                  tServoStandard)
#pragma config(Servo,  srvo_S1_C4_4,    locktwo,              tServoStandard)
#pragma config(Servo,  srvo_S1_C4_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.
#include "C:\Users\student\documents\code\gyro.h"
#include "C:\Users\Student\Documents\Code\Drivers\rdpartyrobotcdr-3.3.1\drivers\hitechnic-irseeker-v2.h"

#define BOUND(n, l, h) (((n) < (l))? (l): ((n) > (h))? (h): (n))
#define PI                      3.14159
#define CLICKS_PER_REVOLUTION   1220.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.0078633) //(WHEEL_CIRCUMFERENCE*GEAR_RATIO/CLICKS_PER_REVOLUTION)
#define DEGREES_PER_CLICK       (0.0048196)

GYRO  g_Gyro;
float g_turnTarget = 0.0;
bool  g_turnEnabled = false;
float g_turnTolerance = 0.1;    //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 = 1.0;
float g_armKp = 40.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 SetArmTarget(float angle)
{
   g_armTarget = angle;
   g_armEnabled = true;
}

float GetArmPostion()
{
   return (float)nMotorEncoder[tower]*DEGREES_PER_CLICK;
}

void ArmTask()
{
   if (g_armEnabled)
   {
      float error = g_armTarget - GetArmPostion();
      nxtDisplayTextLine(4, "ArmErr=%f",error);
      if (abs(error) > g_armTolerance)
      {
         motor[tower] = BOUND((int)(g_armKp*error), -50,50);
      }
      else
      {
         motor[tower] = 0;
         g_armEnabled = false;
      }
   }
}

void initializeRobot()
{
   servoTarget[tube] = 87;
   servoTarget[SIR] = 30;
   servoTarget[Lock] = 84;
   servoTarget[locktwo]= 255;
  // 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();

    nxtDisplayTextLine(4, "Hello Gents",);
    nxtDisplayTextLine(6, "Im ready",);


    waitForStart(); // Wait for the beginning of autonomous phase.
    int dirAC = 0;
    int acS1, acS2, acS3, acS4, acS5 = 0;

    HTIRS2setDSPMode(HTIRS2, DSP_1200);

    PlaySound(soundBeepBeep);

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



    while (true)
    {
        HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 );

        int dirAC = HTIRS2readACDir(HTIRS2);

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

        if(dirAC < 0){
         //quits loop because something is wrong with the ir seeker
         break;
         }
      //loop begins to seek the ir
      //-------------------------------------------------
         //***********************************Position 1****************************************
         if (dirAC    < 3)
         {
            nxtDisplayTextLine(5, "Step=%d", step);
            GyroTask(g_Gyro);
           switch (step)
           {

            case 0:
                nxtDisplayTextLine(2, "Position 1",);
                // wait for set time. Motor at 0.
                servoTarget[SIR] = 30;
                ClearTimer(T1);
                step++;
                break;

            case 1:
                // 1 second = 1000
                if (time1[T1] > 250)//<------ Time change here
                {
                    step++;
                }
                break;

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


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

            case 4:
                // step 2: dump the block.
                SetTurnTarget(30.0);
                step++;
                break;

            case 5:
                // step 3: wait some time to complete the dumping
               if (g_turnEnabled == false)
                {
                   step++;
                }
                break;

            case 6:
                // step 0: seek ir beacon on the right side.
                // If seeking ir beacon on the left side, use 2 instead.
                    SetDriveTarget(25.0);
                    step++;
                break;


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

            case 8:
                // step 2: dump the block.
                SetTurnTarget(-155.0);
                step++;
                break;

            case 9:
                // step 3: wait some time to complete the dumping
               if (g_turnEnabled == false)
                {
                   step++;
                }
                break;

            case 10:
                // step 0: seek ir beacon on the right side.
                // If seeking ir beacon on the left side, use 2 instead.
                    SetDriveTarget(5.0);
                         step++;
                         break;

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

               }

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

//***********************************Position 2****************************************
   else if (dirAC    == 3)
         {
            nxtDisplayTextLine(5, "Step=%d", step);
            GyroTask(g_Gyro);
           switch (step)
           {

            case 0:
               nxtDisplayTextLine(2, "Position 2",);
                // wait for set time. Motor at 0.
                servoTarget[SIR] = 30;
                ClearTimer(T1);
                step++;
                break;

            case 1:
                // 1 second = 1000
                if (time1[T1] > 250)//<------ Time change here
                {
                    step++;
                }
                break;

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


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

            case 4:
                // step 2: dump the block.
                SetTurnTarget(45.0);
                step++;
                break;

            case 5:
                // step 3: wait some time to complete the dumping
               if (g_turnEnabled == false)
                {
                   step++;
                }
                break;

            case 6:
                // step 0: seek ir beacon on the right side.
                // If seeking ir beacon on the left side, use 2 instead.
           SetDriveTarget(15.0);
              step++;
                break;


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

            case 8:
                // step 2: dump the block.
                SetTurnTarget(45.0);
                step++;
                break;

            case 9:
                // step 3: wait some time to complete the dumping
               if (g_turnEnabled == false)
                {
                   step++;
                }
                break;

            case 10:
                // step 0: seek ir beacon on the right side.
                // If seeking ir beacon on the left side, use 2 instead.
           SetDriveTarget(10.0);
          step++;
         break;

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

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

        }
//***********************************Position 3****************************************
        else if (dirAC  > 3)
         {
            nxtDisplayTextLine(5, "Step=%d", step);
            GyroTask(g_Gyro);
           switch (step)
           {

            case 0:
                nxtDisplayTextLine(2, "Position 3",);
                // wait for set time. Motor at 0.
                servoTarget[SIR] = 30;
                ClearTimer(T1);
                step++;
                break;

            case 1:
                // 1 second = 1000
                if (time1[T1] > 250)//<------ Time change here
                {
                    step++;
                }
                break;

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


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

            case 4:
                // step 2: dump the block.
                SetTurnTarget(-45.0);
                step++;
                break;

            case 5:
                // step 3: wait some time to complete the dumping
               if (g_turnEnabled == false)
                {
                   step++;
                }
                break;

            case 6:
                // step 0: seek ir beacon on the right side.
                // If seeking ir beacon on the left side, use 2 instead.
                    SetDriveTarget(15.0);
                    step++;
                break;


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

            case 8:
                // step 2: dump the block.
                SetTurnTarget(45.0);
                step++;
                break;

            case 9:
                // step 3: wait some time to complete the dumping
               if (g_turnEnabled == false)
                {
                   step++;
                }
                break;

            case 10:
                // step 0: seek ir beacon on the right side.
                // If seeking ir beacon on the left side, use 2 instead.
           SetDriveTarget(30.0);
             step++;
             break;

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

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


Thanks


Fri Mar 13, 2015 2:10 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1516
Post Re: Is it possible. IR scanning....
How about something like this:
Code:
int g_step = 0;

void position1Task()
{
    switch (g_step)
    {
        case 0:
            nxtDisplayTextLine(2, "Position 1");
            // wait for set time. Motor at 0.
            servoTarget[SIR] = 30;
            ClearTimer(T1);
            g_step++;
            break;

        case 1:
            // 1 second = 1000
            if (time1[T1] > 250)//<------ Time change here
            {
                g_step++;
            }
            break;

        case 2:
            // step 0: seek ir beacon on the right side.
            // If seeking ir beacon on the left side, use 2 instead.
            SetDriveTarget(15.0);
            g_step++;
            break;

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

        case 4:
            // step 2: dump the block.
            SetTurnTarget(30.0);
            g_step++;
            break;

        case 5:
            // step 3: wait some time to complete the dumping
            if (g_turnEnabled == false)
            {
                g_step++;
            }
            break;

        case 6:
            // step 0: seek ir beacon on the right side.
            // If seeking ir beacon on the left side, use 2 instead.
            SetDriveTarget(25.0);
            g_step++;
            break;

        case 7:
            if (g_driveEnabled == false)
            {
                g_step++;
            }
            break;

        case 8:
            // step 2: dump the block.
            SetTurnTarget(-155.0);
            g_step++;
            break;

        case 9:
            // step 3: wait some time to complete the dumping
            if (g_turnEnabled == false)
            {
                g_step++;
            }
            break;

        case 10:
            // step 0: seek ir beacon on the right side.
            // If seeking ir beacon on the left side, use 2 instead.
            SetDriveTarget(5.0);
            g_step++;
            break;

        case 11:
            if (g_driveEnabled == false)
            {
                g_step++;
            }
            break;

        default:
            break;
   }
}

void position2Task()
{
    switch (g_step)
    {
        case 0:
            nxtDisplayTextLine(2, "Position 2",);
            // wait for set time. Motor at 0.
            servoTarget[SIR] = 30;
            ClearTimer(T1);
            g_step++;
            break;

        case 1:
            // 1 second = 1000
            if (time1[T1] > 250)//<------ Time change here
            {
                g_step++;
            }
            break;

        case 2:
            // step 0: seek ir beacon on the right side.
            // If seeking ir beacon on the left side, use 2 instead.
            SetDriveTarget(24.0);
            g_step++;
            break;

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

        case 4:
            // step 2: dump the block.
            SetTurnTarget(45.0);
            g_step++;
            break;

        case 5:
            // step 3: wait some time to complete the dumping
            if (g_turnEnabled == false)
            {
                g_step++;
            }
            break;

        case 6:
            // step 0: seek ir beacon on the right side.
            // If seeking ir beacon on the left side, use 2 instead.
            SetDriveTarget(15.0);
            g_step++;
            break;

        case 7:
            if (g_driveEnabled == false)
            {
                g_step++;
            }
            break;

        case 8:
            // step 2: dump the block.
            SetTurnTarget(45.0);
            g_step++;
            break;

        case 9:
            // step 3: wait some time to complete the dumping
            if (g_turnEnabled == false)
            {
                g_step++;
            }
            break;

        case 10:
            // step 0: seek ir beacon on the right side.
            // If seeking ir beacon on the left side, use 2 instead.
            SetDriveTarget(10.0);
            g_step++;
            break;

        case 11:
            if (g_driveEnabled == false)
            {
                g_step++;
            }
            break;

        default:
            break;
    }
}

void position3Task()
{
    switch (g_step)
    {
        case 0:
            nxtDisplayTextLine(2, "Position 3",);
            // wait for set time. Motor at 0.
            servoTarget[SIR] = 30;
            ClearTimer(T1);
            g_step++;
            break;

        case 1:
            // 1 second = 1000
            if (time1[T1] > 250)//<------ Time change here
            {
                g_step++;
            }
            break;

        case 2:
            // step 0: seek ir beacon on the right side.
            // If seeking ir beacon on the left side, use 2 instead.
            SetDriveTarget(15.0);
            g_step++;
            break;

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

        case 4:
            // step 2: dump the block.
            SetTurnTarget(-45.0);
            g_step++;
            break;

        case 5:
            // step 3: wait some time to complete the dumping
            if (g_turnEnabled == false)
            {
                g_step++;
            }
            break;

        case 6:
            // step 0: seek ir beacon on the right side.
            // If seeking ir beacon on the left side, use 2 instead.
            SetDriveTarget(15.0);
            g_step++;
            break;

        case 7:
            if (g_driveEnabled == false)
            {
                g_step++;
            }
            break;

        case 8:
            // step 2: dump the block.
            SetTurnTarget(45.0);
            g_step++;
            break;

        case 9:
            // step 3: wait some time to complete the dumping
            if (g_turnEnabled == false)
            {
                g_step++;
            }
            break;

        case 10:
            // step 0: seek ir beacon on the right side.
            // If seeking ir beacon on the left side, use 2 instead.
            SetDriveTarget(30.0);
            g_step++;
            break;

        case 11:
            if (g_driveEnabled == false)
            {
                g_step++;
            }
            break;

        default:
            break;
    }
}

task main()
{
    StopTask(displayDiagnostics);
    eraseDisplay();
    initializeRobot();
    HTIRS2setDSPMode(HTIRS2, DSP_1200);  //should put this in initializeRobot

    nxtDisplayTextLine(4, "Hello Gents",);
    nxtDisplayTextLine(6, "Im ready",);

    waitForStart(); // Wait for the beginning of autonomous phase.

    PlaySound(soundBeepBeep);

    nMotorEncoder[motorLeft] = 0;  //should put this in initializeRobot
    nMotorEncoder[motorRight] = 0;  //should put this in initializeRobot
    nMotorEncoder[tower] = 0;  //should put this in initializeRobot

//    int acS1, acS2, acS3, acS4, acS5;  // You never use these, why don't you just remove them?
//    HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5 );
    int dirAC = HTIRS2readACDir(HTIRS2);

    if (dirAC > 0)
    {
        while (true)
        {
            nxtDisplayTextLine(5, "Step=%d", g_step);
            GyroTask(g_Gyro);

            if (dirAC < 3)
            {
                position1Task();
            }
            else if (dirAC == 3)
            {
                position2Task();
            }
            else
            {
                position3Task();
            }

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


Last edited by MHTS on Fri Mar 13, 2015 2:13 pm, edited 2 times in total.



Fri Mar 13, 2015 5:22 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1516
Post Re: Is it possible. IR scanning....
BTW, do you guys realize you have multi-tasking capabilities? Meaning that you can do multiple non-dependent tasks at the same time. For example, I would guess the servo SIR can move at the same time as you drive forward. If I guess correctly, they are actions that don't depend on each other's completion. This would help in case you want to do very complex autonomous within 30 seconds.
Code:
void position1Task()
{
    switch (g_step)
    {
        case 0:
            nxtDisplayTextLine(2, "Position 1");
            // wait for set time. Motor at 0.
            servoTarget[SIR] = 30;
            ClearTimer(T1);
            SetDriveTarget(15.0);
            g_step++;
            break;

        case 1:
            // 1 second = 1000
            if (time1[T1] > 250 && g_driveEnabled == false)//<------ Time change here
            {
                g_step++;
            }
            break;

        case 2:
            // step 2: dump the block.
            SetTurnTarget(30.0);
            g_step++;
            break;

        case 3:
            // step 3: wait some time to complete the dumping
            if (g_turnEnabled == false)
            {
                g_step++;
            }
            break;

        case 4:
            // step 0: seek ir beacon on the right side.
            // If seeking ir beacon on the left side, use 2 instead.
            SetDriveTarget(25.0);
            g_step++;
            break;

        case 5:
            if (g_driveEnabled == false)
            {
                g_step++;
            }
            break;

        case 6:
            // step 2: dump the block.
            SetTurnTarget(-155.0);
            g_step++;
            break;

        case 7:
            // step 3: wait some time to complete the dumping
            if (g_turnEnabled == false)
            {
                g_step++;
            }
            break;

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

        case 9:
            if (g_driveEnabled == false)
            {
                g_step++;
            }
            break;

        default:
            break;
   }
}


Fri Mar 13, 2015 5:38 am
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 91
Post Re: Is it possible. IR scanning....
Everything works until it comes to the last position listed. Position 1. If I turn the center to position 2 or 3 while the robot is still on, jt will then run those two programs. But as soon as I go back to position 1, nothing.

As for the multi tasking, yes. We have it in multiple other programs allowing us to save some time. I just haven't gotten to it here. I forgot to take out the one case and re-number, with the servo anyways, as we don't even use the servo anymore.

Thanks for the help!


Fri Mar 13, 2015 3:27 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1516
Post Re: Is it possible. IR scanning....
Team2844 wrote:
Everything works until it comes to the last position listed. Position 1. If I turn the center to position 2 or 3 while the robot is still on, jt will then run those two programs. But as soon as I go back to position 1, nothing.

As for the multi tasking, yes. We have it in multiple other programs allowing us to save some time. I just haven't gotten to it here. I forgot to take out the one case and re-number, with the servo anyways, as we don't even use the servo anymore.

Thanks for the help!

Would you describe exactly what's not working? What is the "last position"? Are you saying if the center goal is turned to position 2 or 3, the code worked as expected but when the center goal is turned to position 1, the robot did not move? Would you check what is the value of dirAC when the center goal is at position 1?


Sun Mar 15, 2015 1:30 am
Profile
Novice

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 91
Post Re: Is it possible. IR scanning....
Our Ir sensor sits on the left side of our robot, about 17 inches off of the ground, at about a 45 degree angle. Depending on the beacon that is used on the field, the values differ for position 1. With the new beacons, it has a value position of 1. For the old beacon, it shows a value position of 0, as it can not see the IR lights. Positions 2 and 3 show a value of 3 or 5 for either beacon. We are having problems with position 1. Normally for position 2 and 3, it would scan and then beep, and continue on to the cases. For position 1 though, it doesn't beep. Right after starting the program, it will then switch over to a screen saying the program is over less than a second later. The beeping comes after it has found a position, and then continues to the cases right? I know that the West Super Regionals will be using the old beacons, so we are going to be having the 0 value. The way the program is written, it should run it even with it at 0, right? I'm not sure what is happening here with this.


Tue Mar 17, 2015 5:12 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1516
Post Re: Is it possible. IR scanning....
I see. I didn't realize you can't read any IR at position 1. Upon careful examination of your original code, you need to change my suggested code to the following. Then it will be completely equivalent to your original code which will treat dirAC==0 as position 1. When I first translated it, I thought 0 is invalid just like negative values and since you break out of the while loop when dirAC is negative, I thought it was appropriate to do the same when dirAC is zero. But if you are sure position 1 could read a zero, then the following code will run position 1 when dirAC is zero..
Code:
    int dirAC = HTIRS2readACDir(HTIRS2);

    if (dirAC >= 0)
    {
        while (true)
        {
            nxtDisplayTextLine(5, "Step=%d", g_step);
            GyroTask(g_Gyro);

            if (dirAC < 3)
            {
                position1Task();
            }
            else if (dirAC == 3)
            {
                position2Task();
            }
            else
            {
                position3Task();
            }

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


Tue Mar 17, 2015 6:59 pm
Profile
Rookie

Joined: Fri Apr 24, 2015 11:48 pm
Posts: 2
Post Re: Is it possible. IR scanning....
You have posted the great code here. Thanks for sharing well.


Fri May 15, 2015 4:58 am
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 12 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.