[FTC]: IR Block for Block Party

[FTC4211]

If you don't want to mount the IR sensor at an angle, one approach is to wait until the sensor changes state to a certain zone, and then use a set time/encoder distance to get the scoring mechanism to the center of the basket.


The benefits of this approach is that it is easily adjustable for where your block scoring mechanism is, it doesn't require hardware changes to adjust where it scores and it doesn't really matter what orientation/zone you seek the beacon with.

The downside is that the code is slightly more complex and as such could take longer to debug and get working properly.


We use this approach for scoring the block; so here is our code that we used for this. I took out our robot specific commands and just left the framework together to give you a jump start.

#include "C:\Program Files (x86)\Robomatter Inc\ROBOTC Development Environment\Sample Programs\NXT\3rd Party Sensor Drivers\drivers\hitechnic-irseeker-v2.h";

task main()
{
	time1[T1]=0;
	motor[Left_Drive_Motor]=15;
	motor[Right_Drive_Motor]=15;
	while (time1[T1]<6500) // used to provide a timeout on the drive time along the baskets, adjust if needed
	{
		if (HTIRS2readACDir(IR_Seeker)==2) // adjust the zone for your robot
		{
			motor[Left_Drive_Motor]=0;
			motor[Right_Drive_Motor]=0;
			wait1Msec(350);
			
			motor[Left_Drive_Motor]=-15;// change the power/sign of this to alter the direction the robot goes after finding the zone
			motor[Right_Drive_Motor]=-15;
			
			wait1Msec(500); // adjust this time to change how far robot goes after seeing the zone
			
			motor[Left_Drive_Motor]=0;
			motor[Right_Drive_Motor]=0;
			wait1Msec(350);
			
			// insert code here to dump the block
			
			motor[Left_Drive_Motor]=15; // restart the drive motors to reach the end
			motor[Right_Drive_Motor]=15;
		}
	}
	motor[Left_Drive_Motor]=0;
	motor[Right_Drive_Motor]=0;
	// rest of auto code ...
}

Hope this helps,
Team 4211

[TheThings5926]

Thanks for the help! This will help a lot, this is what I wanted to do, just couldn't quite figure out how to do it.

I am also looking at putting a NXT ultrasonic sensor on the front of the robot. I will use this to keep from getting blocked or hitting our alliance partners robot.

Say the normal program would drive past the baskets, dump block, and then take a 180 degree left turn onto the ramp. This code would run unless the ultrasonic sensor sees something within 20cm. If an object is within 20cm the robot would turn around, and go up the other side of the ramp.

Thoughts?

Thanks,

Matt, The Things 5926

[FTC4211]

Hello,

Here is what I have written up for your application. This code contains the section of driving past the baskets and then tries to get on the ramp. It checks both sides for an opponent and if there is one it tries the other side of the ramp.

Adjust the times and powers as needed for your robot. As is it should work decently for a robot with 4" wheels with a 1:1 gear ratio on the drive.

#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Sensor, S2,     IR_Seeker,      sensorI2CCustom)
#pragma config(Sensor, S3,     Sonar,          sensorSONAR)
#pragma config(Motor,  mtr_S1_C1_1,     Right_Drive_Motor, tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     Left_Drive_Motor, tmotorTetrix, openLoop, reversed)
#pragma config(Servo,  srvo_S1_C2_1,    servo1,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "C:\Program Files (x86)\Robomatter Inc\ROBOTC Development Environment\Sample Programs\NXT\3rd Party Sensor Drivers\drivers\hitechnic-irseeker-v2.h";
#include "joystickdriver.c";

const int AngleTurn_90 = 900; // time for a 90deg angle turn at 50% power (adjust for your robot)
const int AngleTurn_45 = AngleTurn_90/2+AngleTurn_90*0.07; // time for a 45deg angle turn at 50% power (adjust formula for your specific robot)

// Basic movement commands for the robot.  Allows easy modifications to auto
void stopRobot() // stop after each movement/turn to increase accuracy/repeatability
{
	motor[Left_Drive_Motor]=0;
	motor[Right_Drive_Motor]=0;
	wait1Msec(350);
}

void turnLeft(int time,int power = 50)
{
	motor[Left_Drive_Motor]=-power;
	motor[Right_Drive_Motor]=power;
	wait1Msec(time);
	stopRobot();
}

void turnRight(int time,int power = 50)
{
	motor[Left_Drive_Motor]=-power;
	motor[Right_Drive_Motor]=power;
	wait1Msec(time);
	stopRobot();
}

void driveForward(int time, int power = 30)
{
	motor[Left_Drive_Motor]=power;
	motor[Right_Drive_Motor]=power;
	wait1Msec(time);
	stopRobot();
}

void driveBackward(int time, int power = 30)
{
	motor[Left_Drive_Motor]=-power;
	motor[Right_Drive_Motor]=-power;
	wait1Msec(time);
	stopRobot();
}
// End of basic movement commands for the robot.

bool pathBlocked(int dist = 25) // returns if object in front of robot
{
	return (SensorValue[Sonar]==-1) ? false : (SensorValue[Sonar]<dist); // -1 is if can't see anything so ignore that condition and <25 b/c of reflection errors
}

void initRobot()
{
	SensorType[Sonar]=sensorSONAR;
	HTIRS2setDSPMode(IR_Seeker, DSP_1200);
	// rest of initialization for servos etc...
}

task main()
{
	initRobot();
	
	waitForStart();
	
	time1[T1]=0;
	motor[Left_Drive_Motor]=15;
	motor[Right_Drive_Motor]=15;
	while (time1[T1]<7200) // used to provide a timeout on the drive time along the baskets, adjust if needed
	{
		if (HTIRS2readACDir(IR_Seeker)==2) // adjust the zone for your robot
		{
			stopRobot();
			driveForward(500, -15); // adjust this time to change how far robot goes after seeing the zone and change the power/sign of this to alter the direction the robot goes after finding the zone
			// insert code here to dump the block

			motor[Left_Drive_Motor]=15; // restart the drive motors to reach the end
			motor[Right_Drive_Motor]=15;
		}
	}
	motor[Left_Drive_Motor]=0;
	motor[Right_Drive_Motor]=0;
	
	// get around end of baskets (more reliable than two 90deg turns)
	turnLeft(AngleTurn_45);
	driveForward(700);
	turnLeft(AngleTurn_45);
	
	driveForward(1500); // drive infront of first spot on the ramp (adjust time if needed)
	
	
	// if sonar points straight infront of robot:
	turnLeft(AngleTurn_90);
	
	if (pathBlocked(50)) // if robot is in the way (dist is 50 b/c want to ensure no robot even if there are reflection problems off their robot)
	{
		turnRight(AngleTurn_90);
		driveForward(850); // drive infront of the next spot on the ramp (adjust time if needed)
		turnLeft(AngleTurn_90);
		if (pathBlocked(50)) // if robot is in the way (dist is 50 b/c want to ensure no robot even if there are reflection problems off their robot)
		{
			driveForward(1350, 100); // try to push them out of the way and get on ramp (might want something else)
		}
		else // drive up on the ramp
		{
			driveForward(1350);
		}
	}
	else // drive up on the ramp
	{
		driveForward(1350);
	}
	// end of section for if sonar points striaght ahead
	
	// if sonar points off the side of the robot
	if (pathBlocked(50)) // if robot is in the way (dist is 50 b/c want to ensure no robot even if there are reflection problems off their robot)
	{
		driveForward(850); // drive infront of the next spot on the ramp (adjust time if needed)
		if (pathBlocked(50)) // if robot is in the way (dist is 50 b/c want to ensure no robot even if there are reflection problems off their robot)
		{
			turnLeft(AngleTurn_90);
			driveForward(1350, 100); // try to push them out of the way and get on ramp (might want something else)
		}
		else // drive up on the ramp
		{
			turnLeft(AngleTurn_90);
			driveForward(1350);
		}
	}
	else // drive up on the ramp
	{
		turnLeft(AngleTurn_90);
		driveForward(1350);
	}
	// end of section for if sonar points off the side of the robot
}

I didn't know how your sonar was mounted so included sections at the end for it pointing forward or sideways off the robot.

Hope this works for you,
Team 4211