[FTC]: IR Block for Block Party

[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