View unanswered posts | View active topics It is currently Mon Mar 02, 2015 5:23 pm






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

Joined: Mon Oct 18, 2010 9:31 pm
Posts: 88
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: 1498
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: 88
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
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.