View unanswered posts | View active topics It is currently Fri Oct 19, 2018 5:49 pm






Reply to topic  [ 2 posts ] 
Our FTC team has MAJOR IR problems... 
Author Message
Rookie

Joined: Thu Oct 30, 2014 10:18 pm
Posts: 1
Post Our FTC team has MAJOR IR problems...
We are having MAJOR IR problems with our robot. I am trying to get it to do something where if it reads a value of 5 it will go back, dump the ball into the tube in the center autonomous structure, and then bring its arm back down. This is an example of what our code should be doing:

Code:
#pragma config(Hubs,  S1, HTMotor,  none,     none,     none)
#pragma config(Hubs,  S2, HTServo,  none,     none,     none)
#pragma config(Hubs,  S4, HTMotor,  none,     none,     none)
#pragma config(Sensor, S3,     HTSMUX,         sensorI2CCustom)
#pragma config(Motor,  motorA,          Sweeper,       tmotorNXT, openLoop, encoder)
#pragma config(Motor,  motorB,          BackDoor,      tmotorNXT, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_1,     motorLeft,     tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     motorRight,    tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S4_C1_1,     ArmRaiser,     tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S4_C1_2,     ArmExtender,   tmotorTetrix, openLoop, encoder)
#pragma config(Servo,  srvo_S2_C1_1,    servo1,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_3,    bucket_servo,         tServoStandard)
#pragma config(Servo,  srvo_S2_C1_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_5,    back_left,            tServoStandard)
#pragma config(Servo,  srvo_S2_C1_6,    back_right,           tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                           Autonomous Mode A
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
#include "drivers/suite/drivers/hitechnic-sensormux.h"
#include "lib/common6819.c"
//#include "lib/auto6819.c"  // Team methods common between AU and UC modes


// Assuming the Sensor MUX is connected to NXT sensor port 4 (S4)
// Assuming the following sensors are connected to the Sensor MUX ports:
// Port 1: IR
// Port 2: Sonar
// Port 3: Gyro
// Port 4: Not used
#include "drivers/suite/drivers/hitechnic-gyro.h"    //for gyro sensor
#include "drivers/suite/drivers/lego-ultrasound.h"    //for sonar sensor
#include "drivers/suite/drivers/hitechnic-irseeker-v2.h"    //for IR seeker sensor

#define IRSensor                msensor_S3_2
#define sonarSensor             msensor_S3_1
#define gyroSensor              msensor_S3_3


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                    initializeRobot
//
// Prior to the start of autonomous mode, you may want to perform some initialization on your robot.
// Things that might be performed during initialization include:
//   1. Move motors and servos to a preset position.
//   2. Some sensor types take a short while to reach stable values during which time it is best that
//      robot is not moving. For example, gyro sensor needs a few seconds to obtain the background
//      "bias" value.
//
// In many cases, you may not have to add any code to this function and it will remain "empty".
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

void initializeRobot()
{
  // 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.

   // Play to indicate we are ready to go.
  Muppets();
  clearDebugStream();
  return;
}


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//  Main Autonomous Routine
//
//  The following is the main code for the autonomous robot operation.
//
//  When AU mode is done, the system will run the paired UC script.
//
////////////////////////////////////////////////////////////////////////////////////////////////////

const int BACKWARDS_DRIVE_TIME_MS   = 100;
const int LONG_SONAR_VALUE   = 60;

const int TURNRIGHT = 0;
const int TURNLEFT = 1;

const int MS_PER_DEGREE = 10;

void SetIR(){

   // Drive backwards, approach structure
   motor[motorRight] = -100;
  motor[motorLeft] = -100;
  wait10Msec( BACKWARDS_DRIVE_TIME_MS );

  // Read the IR sensor
  int IRVal = HTIRS2readACDir(IRSensor);
   writeDebugStreamLine("IR Value: %d", IRVal);
  // Read the Ultrasonic Distance Sensor
   int Sonar1value = USreadDist(sonarSensor);
   writeDebugStreamLine("Sonar Value: %d", Sonar1value);

   // Is this position 1?
   if( IRVal == 5 && Sonar1value < LONG_SONAR_VALUE ) {

      // Position 1, we are close.  Move straight ahead carefully, then dump.

      motor[motorRight] = -50;
    motor[motorLeft] = -50;
    wait10Msec(20);

    IRDump();

  // Is this position 3?
   } else if( Sonar1value > LONG_SONAR_VALUE) {

      // Turn, drive and reapproach the 90deg structure

         // Left Turn Forward
         motor[motorRight] = 50;
      motor[motorLeft] = 0;
      wait10Msec(50);

      // Backwards
         motor[motorRight] = -100;
      motor[motorLeft] = -100;
      wait10Msec(75);

      // Right Turn Forward
      motor[motorLeft] = 50;
      motor[motorRight] = 0;
      wait10Msec(50);
      IRDump();

  // Must be position 2
  } else {

      // Back up, turn and reapproach the 45deg structure
    motor[motorLeft] = 50;
    motor[motorRight] = 0;
    wait10Msec(25);

    motor[motorRight] = 50;
    motor[motorLeft] = 0;
    wait10Msec(25);
    IRDump();

   }
  // Read the IR sensor
  IRVal = HTIRS2readACDir(IRSensor);
   writeDebugStreamLine("IR Value: %d", IRVal);
  // Read the Ultrasonic Distance Sensor
   Sonar1value = USreadDist(sonarSensor);
   writeDebugStreamLine("Sonar Value: %d", Sonar1value);
}

// Turn the bot -- left, right,
void turnbot( int direction, int degrees ) {

  // Turn the direction we want until we exceed the number of degrees
  while( degrees > 0  ) {
    if ( direction == TURNLEFT ) {
      motor[motorRight] = 50;
      motor[motorLeft] = -50;
      wait10Msec(MS_PER_DEGREE);
    } else {
      motor[motorRight] = -50;
      motor[motorLeft] = 50;
      wait10Msec(MS_PER_DEGREE);
    }
    degrees = degrees - 1;
  }

  motor[motorRight] = 0;
  motor[motorLeft] = 0;

  return;

}


task main()
{
  initializeRobot();

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

    int seconds = 0;
      Arm45();


      int Sonar1value = USreadDist(sonarSensor);
    writeDebugStreamLine("Sonar Value: %d", Sonar1value);
    int IR1value = HTIRS2readACDir(IRSensor);
    writeDebugStreamLine("IR Value: %d", IR1value);
    int Gyro1value = HTGYROreadRot(gyroSensor);
    writeDebugStreamLine("Gyro Value: %d", Gyro1value);

//    HTGYROstartCal(gyroSensor);

    SetIR();
// int gyroRotation = HTGYROreadRot(gyroSensor);
// int distanceInCM = USreadDist(sonarSensor);
// int irACDirection = HTIRS2readACDir(IRSensor);


}



Yeah, we've got problems. It's not doing what its supposed to be doing.


Wed Feb 04, 2015 8:02 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1523
Post Re: Our FTC team has MAJOR IR problems...
So you told us what it is supposed to do. Now please tell us what the current code is doing instead?


Wed Feb 04, 2015 8:19 pm
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 2 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:  
cron



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