Пример #1
0
// =======================================================================
// Function to drive the robot towards the IR beacon
// =======================================================================
void IR_TimeMove(int power, long maxtime)
{
	step++;							// increase step number for data log file
	int limitReached = 0;
	long current_power;
	long starttime = nPgmTime;
	ResetDriveEncoders();
	motor[LF_motor] = power;
	motor[RF_motor] = power;
	motor[LR_motor] = power;
	motor[RR_motor] = power;
	current_power = power;
	bool Done = false;
	while(!Done)
	{
		if (nPgmTime> starttime + (maxtime* 1000))
		{
			Done = true;  // the timer has run out, so stop the move
		}
		if(limitReached > 4)
		{
			Done = true;
		}
		float adj_power = (IR_Bearing-5.0)*IR_PROPORTION;
		motor[LF_motor] = (current_power - (int)adj_power);
		motor[RF_motor] = (current_power + (int)adj_power);
	}
	StopRobot();
}
Пример #2
0
//===================================================
// Function to turn the robot with gyro V2
//===================================================
void Gyro_TurnV2 (float degrees, int power, bool ConstOrRel)
{
	relHeading=0;
	if (degrees<0)
	{
		motor[LF_motor]=power;     // start turning left
		motor[LR_motor]=power;
		motor[RF_motor]=power;     // start turning left
		motor[RR_motor]=power;
	}
	else
	{
		PlaySound(soundBeepBeep);
		motor[LF_motor]=-power;      // start turning right
		motor[LR_motor]=-power;
		motor[RF_motor]=-power;      // start turning right
		motor[RR_motor]=-power;
	}
	if(gyro_mux_status==false)		// the GYRO mux seems to be active
	{
		if(ConstOrRel)						// decide whether to turn relative to start position
			// or relative to current robot orientation
		{
			while(abs((int)degrees) > abs((int)constHeading)){}	// relative to field
		}
		else
		{
			while(abs((int)degrees) > abs((int)relHeading)){}		// relative to robot start posn
		}
	}
	else												// we don't have a good gyro reading
	{
	}
	StopRobot();						// once done we shutdown all drive motors
}
Пример #3
0
// =======================================================================
// Function to move the robot by the gyro by time sideways V2
// =======================================================================
void GyroTimeS_moveV2(int time, int power, bool light,bool StopWhenDone, bool adjust, bool ConstOrRel)
{
	long adj_power;
	long adj_deg;
	long current_power;
	int i = 0;
	relHeading =0;
	wait1Msec(200);
	motor[LF_motor] = -power;
	motor[RF_motor] = -power;
	motor[LR_motor] = power;
	motor[RR_motor] = power;
	current_power = power;
	ClearTimer(T1);
	bool Done = false;
	int addPower = 0;
	while(!Done)
	{
		RequestedScreen=S_LIGHT;		// force the light value screen to be displayed
		if(light == true)
		{
			if(light_normalised > light_threshold) i++;
			if(i > 5) Done = true;			// was 10, but that may be too high, now 5 at 21 Nov 2012
		}

		if(time1[T1] > time)
		{
			Done = true;
		}

		if(adjust == true)
		{
			if(ConstOrRel) adj_deg = (long) constHeading;
			else adj_deg = (long) relHeading;
			adj_power = adj_deg*GYRO_PROPORTION;

			addPower = (current_power*5)/100;

			if(power > 0)
			{
				motor[LF_motor] = -(current_power-addPower); //more
				motor[RF_motor] = -((current_power+addPower) - adj_power);
				motor[LR_motor] = (current_power-addPower); //more
				motor[RR_motor] = ((current_power+addPower) + adj_power);
			}
			else
			{
				motor[LF_motor] = -(current_power+addPower);
				motor[RF_motor] = -((current_power-addPower) - adj_power);
				motor[LR_motor] = (current_power+addPower);
				motor[RR_motor] = ((current_power-addPower) + adj_power);
			}
		}
	}
	if(StopWhenDone==true)
	{
		StopRobot();
	}
}
// =======================================================================
// Function to move the robot by the gyro by time V2
// =======================================================================
void GyroTime45_V2(int time, int power,bool StopWhenDone, bool adjust, bool ConstOrRel, bool LorR)
{
	step++;							// increase step number for data log file
	long adj_power;
	long adj_deg;
	long current_power;
	relHeading =0;
	wait1Msec(20);
	if(LorR)
	{
		motor[LF_motor] = 0;
		motor[RF_motor] = power;
		motor[LR_motor] = -power;
		motor[RR_motor] = 0;
	}
	else
	{
		motor[LF_motor] = -power;
		motor[RF_motor] = 0;
		motor[LR_motor] = 0;
		motor[RR_motor] = power;
	}
	current_power = power;
	ClearTimer(T1);
	bool Done = false;
	while(!Done)
	{
		if(time1[T1] > time)
		{
			Done = true;
		}

		if(adjust == true)
		{
			if(ConstOrRel) adj_deg = (long) constHeading;
			else adj_deg = (long) relHeading;
			adj_power = adj_deg*GYRO_PROPORTION;

			if(LorR)
			{
				motor[LF_motor] = 0;
				motor[RF_motor] = (current_power + adj_power);
				motor[LR_motor] = (-current_power - adj_power);
				motor[RR_motor] = 0;
			}
			else
			{
				motor[LF_motor] = (-current_power - adj_power);
				motor[RF_motor] = 0;
				motor[LR_motor] = 0;
				motor[RR_motor] = (current_power + adj_power);
			}
		}
	}
	if(StopWhenDone==true)
	{
		StopRobot();
	}
}
Пример #5
0
// =======================================================================
// Function to move the robot by the gyro and the sonar sensors V2
// =======================================================================
void GyroSonar_moveV2(int time, int WhichSensors, int distanceX, int distanceY, int power,bool StopWhenDone, bool adjust, bool ConstOrRel)
{
	step++;							// increase step number for data log file
	long adj_power;			// Reset all the values
	long adj_deg;				//
	int count = 0;					//
	int sonar_adj = 0;	//-----------------
	int lastvalue=9999;
	long current_power;
	relHeading =0;			//-----------------
	wait1Msec(20);
	motor[LF_motor] = -power;	//---------------
	motor[RF_motor] = power;	// Set beginning speeds
	motor[LR_motor] = -power; // for the motors
	motor[RR_motor] = power;	//---------------
	current_power = power;
	ClearTimer(T1);
	bool Done = false;
	while(!Done)
	{
		if(WhichSensors != 2)										//----------------
		{																				//
			if(sonarLive2 > distanceY)						// we have exceeded our target distance
			{
				if (sonarLive2!=lastvalue)					// we have a new reading above the target distance
				{
					count++;													// so increase our count of values above distance
					lastvalue=sonarLive2;							// and save the current reading for next time around
				}
			}
			else count=0;													// the measured distance is below the target distance
				if(count > 2) Done = true;						// once we get two readings above target we exit the loop
		}																				//
		else if(time1[T1] > time) Done = true;	//----------------

		if(adjust == true)
		{
			if(WhichSensors != 1) sonar_adj = ((distanceX - sonarLive)*SONAR_PROPORTION);
			if(ConstOrRel) adj_deg = (long) constHeading;
			else adj_deg = (long) relHeading;
			adj_power = adj_deg*GYRO_PROPORTION;
			/*
			motor[LF_motor] = -(current_power + adj_power + sonar_adj);
			motor[RF_motor] = current_power + adj_power - sonar_adj; // was +
			motor[LR_motor] = -(current_power + adj_power - sonar_adj);
			motor[RR_motor] = current_power + adj_power + sonar_adj;
			*/
			motor[LF_motor] = -(current_power - (adj_power + sonar_adj));
			motor[RF_motor] = (current_power + (adj_power + sonar_adj));
			motor[LR_motor] = -(current_power);
			motor[RR_motor] = (current_power);
		}
	}
	if(StopWhenDone==true)
	{
		StopRobot();
	}
}
Пример #6
0
void SpinAlgorithm(void) {
	vector V(0,0);
	double omega=0.20, h=0;			// body angular velocity in rad/sec
	do {	
		V.x = 	0.07*cos(h);		// body x velocity vector in meters
		V.y = 	0.07*sin(h);		// body y velocity vector in meters
		h += omega;					// turn velocity vector by omega
		Vector_Drive(V, omega);		// drive robot using direction vector and angular velocity
	}	while (SmartWait(90) == 0); // loop will be interrupted by penDown event
	StopRobot();
}
Пример #7
0
void PenFollowAlgorithm(void) {
	unsigned long start_time;
	int pen_dist=30, dist, detected = False, lost = False, infi = 60, played = False;
	// put pen facing wheel #1
	// turn robot to the right until it finds the center of the pen
	Turn(Right, Slow);
	DisplayTxt("Scanning...");
	do {
		dist = Sensor_To_Range(Sensor(3));
		if (dist <= pen_dist) 
			detected = True;
	} while ( (detected == False) && (SmartWait(1)==0) );
	StopRobot();	// stop robot
	// begin tracking loop
	do {
		// ADJUST ANGULAR POSITION
		dist = Sensor_To_Range(Sensor(3));
		if (dist > infi) {	// target lost turn left/right
			played = False;
			Turn(Right,Slow); Wait(50); StopRobot(); // turn right for awhile
			dist = Sensor_To_Range(Sensor(3));	
			if (dist > infi) { // target is not to the right,
				Turn(Left,Slow);// turn left
				start_time = 0;
				do { 
					dist = Sensor_To_Range(Sensor(3));
				} while ((start_time++ < 1000) && (dist > infi) && (SmartWait(1)==0) );	// turn until found
				dist = Sensor_To_Range(Sensor(3));
				if (dist > infi) {
					lost = True;
					break; }
				StopRobot(); } }
		dist = Sensor_To_Range(Sensor(3));		
		// ADJUST LINEAR POSITION
		if ( (dist < infi) && (dist < (pen_dist-5)) )	{ // too close to target
			played = False;
			Move(RToServo3);
			do {
				dist = Sensor_To_Range(Sensor(3));
				if (dist > infi) 	// lost angular tracking
					break;
			} while (dist < pen_dist);	// get dist to pen_dist
			StopRobot(); }
		if ( (dist < infi) && (dist > (pen_dist+5)) )	{ // too far 
			played = False;
			Move(ToServo3);
			do {
				dist = Sensor_To_Range(Sensor(3));
				if (dist > infi)	// lost angular tracking
					break;
			} while (dist > pen_dist);	// get dist to pen_dist
			StopRobot(); }
		if ( (dist < (pen_dist+5)) && (dist > (pen_dist-5)) && (played == False) ) {
			printf("Yep\n");												
			played = True; }
	} while ( (lost == False) && (SmartWait(10)==0) );
	StopRobot();
}
Пример #8
0
main(int argc, char *argv[])
{
	char	*opt;
	int		i, n;

	init_serial("/dev/ttyS0");
	init_lircd("/var/tmp/lircd");

	if (argc == 2 && strcmp(argv[1], "test") == 0) {
		simple_test();
		exit(0);
	}
	
	StopRobot(); /* turn it off to start with */

	n = 0;
	do {
		opt = get_irkey();
		if (opt) {
			syslog(LOG_INFO, "PalmBot - %s\n", opt);
			for (i = 0; command[i].key_name; i++)
				if (strcmp(command[i].key_name, opt) == 0) {
					n = i;
					break;
				}
		}
		if (command[n].key_name) {
			syslog(LOG_INFO, "Run command '%s'\n", command[n].key_name);
			(*command[n].func)();
		}
	} while (command[n].key_name);

	StopRobot(); /* turn it off to end with */

	exit(0);
}
//===================================================
// Function to turn the robot with gyro V2
//===================================================
bool Gyro_TurnV2 (float degrees, int power, bool ConstOrRel)
{
	bool sawLine = false;
	int i = 0;
	step++;							// increase step number for data log file
	relHeading=0;
	if (degrees<0)
	{
		motor[LF_motor]=power;     // start turning left
		motor[LR_motor]=power;
		motor[RF_motor]=power;     // start turning left
		motor[RR_motor]=power;
	}
	else
	{
		PlaySound(soundBeepBeep);
		motor[LF_motor]=-power;      // start turning right
		motor[LR_motor]=-power;
		motor[RF_motor]=-power;      // start turning right
		motor[RR_motor]=-power;
	}
	if(gyro_mux_status==false)		// the GYRO mux seems to be active
	{
		if(ConstOrRel)						// decide whether to turn relative to start position
			// or relative to current robot orientation
		{
			while(abs((int)degrees) > abs((int)constHeading))
			{
				if(light_normalised > light_threshold) i++;
				if(i > 5) sawLine = true;			// was 10, but that may be too high, now 5 at 21 Nov 2012
			}	// relative to field
		}
		else
		{
			while(abs((int)degrees) > abs((int)relHeading))
			{
				if(light_normalised > light_threshold) i++;
				if(i > 5) sawLine = true;			// was 10, but that may be too high, now 5 at 21 Nov 2012
			}		// relative to robot start posn
		}
	}
	else												// we don't have a good gyro reading
	{
	}
	StopRobot();						// once done we shutdown all drive motors
	return sawLine;
}
//========================================================================
// Gyro turn with only 3 motors, for now
//========================================================================
void Gyro3MT_V2 (float degrees, int power, bool ConstOrRel, bool LorR)
{
	step++;							// increase step number for data log file
	relHeading=0;
	if (degrees<0)
	{
		if(LorR == true)
		{
			motor[LF_motor]=power/2;
			motor[RF_motor]=-1;
			motor[RR_motor]=power/2;
			motor[LR_motor]=power;
		}
		else
		{
			motor[LF_motor]=-1;
			motor[RF_motor]=power/2;
			motor[RR_motor]=power;
			motor[LR_motor]=power/2;
		}
	}
	else
	{
		PlaySound(soundBeepBeep);
		if(LorR == true)
		{
			motor[LF_motor]=power/2;
			motor[RF_motor]=-1;
			motor[RR_motor]=power/2;
			motor[LR_motor]=power;
		}
		else
		{
			motor[LF_motor]=-1;
			motor[RF_motor]=power/2;
			motor[RR_motor]=power;
			motor[LR_motor]=power/2;
		}
	}
	if(gyro_mux_status==false)				// the GYRO mux seems to be active
	{
		if(ConstOrRel) while(abs((int)degrees) > abs((int)constHeading)){}

		else while(abs((int)degrees) > abs((int)relHeading)){}
	}
	StopRobot();										// shutdown all drive motors
}
Пример #11
0
void
timed_turn(int dir, int speed)
{
	fd_set rfds;
	struct timeval tv;

	Turn(dir, speed);

	tv.tv_sec = 3;
	tv.tv_usec = 0;
	FD_ZERO(&rfds);
	FD_SET(ir_fd, &rfds);
	if (select(ir_fd + 1, &rfds, NULL, NULL, &tv) == 1)
		return;
	
	StopRobot();
}
Пример #12
0
void StopRun(void) {
	if (bRecordFlag)
		MoveRobot(XSPEED, 50, 0, 150, 0, ACCELERATION);
	else if (bExplorationFlag) {
		MoveRobot(XSPEED, 500, 0, EXPLORATION_SPEED, 0, ACCELERATION);
		bExplorationFlag = FALSE;
	} else if (bFastFlag) {
		MoveRobot(XSPEED, 550, 0, 0, 0, ACCELERATION);
		bFastFlag = FALSE;
	} else {
		MoveRobot(XSPEED, 100, 0, EXPLORATION_SPEED, 0, ACCELERATION);
//		MoveRobot(XSPEED, 2500, 0, EXPLORATION_SPEED, 0, ACCELERATION);
	}

	StopRobot();
	DisableMotor();

	bRunFlag = FALSE;
	bStopFlag = FALSE;
}
Пример #13
0
task main()
    {
    if (InitializeMain(true, true))
        {
        StartReadingTouchSensor(touchForklift);
        SpeculativelyUpdateBlackboard();
        //
        for (;;)
            {
            if (touchForklift.fValue)
                {
                StopRobot();
                break;
                }
            //
            SendOneMotorPower(motorForklift, -2);     // down *slow*
            //
            EndTimeSlice();
            }
        }
    }
Пример #14
0
void TravelerAlgorithm(void)
{
	vector Z(0,0);
	double omega, vel=0.07;	// vel is top speed of servos
	int chosen_dir = 1, quit_loop=0;
	int sensor[6], sensor_obstacle=30, sensor_clearview=40;
	Move(chosen_dir);
	do {
		do { 		// drive until robot sees an obstacle using a current sensor
			quit_loop = SmartWait(1);  	// obstacle detected
		} while ( (Sensor_To_Range(Sensor(chosen_dir)) > sensor_obstacle) && (quit_loop == 0) && (chosen_dir!=0) );
		sensor[1] = Sensor_To_Range(Sensor(1));	// get sensors
		sensor[2] = Sensor_To_Range(Sensor(2));
		sensor[3] = Sensor_To_Range(Sensor(3));		
		sensor[4] = sensor[1];		
		sensor[5] = sensor[2];		
		// sensor+1 -> obstalce & sensor+2 -> obstalce
		if ( (sensor[chosen_dir+2] < sensor_obstacle) && (sensor[chosen_dir+1] < sensor_obstacle) ) {
			Vector_Drive(Z, 0.500); 
			chosen_dir=0; }
		else {
			// sensor+1 -> clear  & sensor+2 -> obstacle
			if ( (sensor[chosen_dir+1] > sensor_clearview) && (sensor[chosen_dir+2] < sensor_obstacle) ) 
				chosen_dir+=1;
			// sensor+1 -> obstalce & sensor+2 -> clear
			if ( (sensor[chosen_dir+1] < sensor_obstacle) && (sensor[chosen_dir+2] > sensor_clearview) ) 
				chosen_dir+=2;
			// sensor+1 -> clear & sensor+2 -> clear
			if ( (sensor[chosen_dir+2] > sensor_clearview) && (sensor[chosen_dir+1] > sensor_clearview) ) 
				// pick 2 for now, change to random later
				chosen_dir+=2;
			if ((chosen_dir) > 3)
				chosen_dir-=3;	// prevent overflow
			Move(chosen_dir); }
			Display(chosen_dir);
	} while (quit_loop==0);
	StopRobot();
}
Пример #15
0
// =======================================================================
// Function to move the robot by the gyro by time V2
// =======================================================================
void GyroTime_moveV2(int time, int power,bool StopWhenDone, bool adjust, bool ConstOrRel)
{
	long adj_power;
	long adj_deg;
	long current_power;
	relHeading =0;
	wait1Msec(200);
	motor[LF_motor] = -power;
	motor[RF_motor] = power;
	motor[LR_motor] = -power;
	motor[RR_motor] = power;
	current_power = power;
	ClearTimer(T1);
	bool Done = false;
	while(!Done)
	{
		if(time1[T1] > time)
		{
			Done = true;
		}

		if(adjust == true)
		{
			if(ConstOrRel) adj_deg = (long) constHeading;
			else adj_deg = (long) relHeading;
			adj_power = adj_deg*GYRO_PROPORTION;

			motor[LF_motor] = -(current_power - adj_power);
			motor[RF_motor] = (current_power + adj_power);
			motor[LR_motor] = -(current_power);
			motor[RR_motor] = (current_power);
		}
	}
	if(StopWhenDone==true)
	{
		StopRobot();
	}
}
// =======================================================================
// Function to move the robot by the gyro and the sonar sensors V2
// =======================================================================
void GyroSonar_moveV3(int time, int WhichSensors, int distanceX, int distanceY, int power,int direction,bool StopWhenDone, bool adjust, bool ConstOrRel)
{
	step++;							// increase step number for data log file
	long adj_power;			// Reset all the values
	long adj_deg;				//
	int count = 0;					//
	int sonar_adj = 0;	//-----------------
	int lastvalue=9999;
	long current_power;
	relHeading =0;			//-----------------
	wait1Msec(20);
	current_power = power;
	ClearTimer(T1);
	bool Done = false;
	while(!Done)
	{/*
		if(WhichSensors != ByTIME || WhichSensors != SIDE)//----------------
		{																				//
		if(sonarLive2 > distanceY )						// we have exceeded our target distance
		{
		if (sonarLive2!=lastvalue)					// we have a new reading above the target distance
		{
		count++;													// so increase our count of values above distance
		lastvalue=sonarLive2;							// and save the current reading for next time around
		}
		}
		else count=0;													// the measured distance is below the target distance
		if(count > 2) Done = true;						// once we get two readings above target we exit the loop
		}																				//
		else if(time1[T1] > time) Done = true;	//----------------*/
		switch(WhichSensors)
		{
		case SIDE:
			if(time1[T1] > time) Done = true;
			break;
		case SIDE_BACK:
			if(sonarLive2 > distanceY )						// we have exceeded our target distance
			{
				if (sonarLive2!=lastvalue)					// we have a new reading above the target distance
				{
					count++;													// so increase our count of values above distance
					lastvalue=sonarLive2;							// and save the current reading for next time around
				}
			}
			else count=0;													// the measured distance is below the target distance
				if(count > 2) Done = true;
			break;
		case BACK:
			if(sonarLive2 > distanceY )						// we have exceeded our target distance
			{
				if (sonarLive2!=lastvalue)					// we have a new reading above the target distance
				{
					count++;													// so increase our count of values above distance
					lastvalue=sonarLive2;							// and save the current reading for next time around
				}
			}
			else count=0;													// the measured distance is below the target distance
				if(count > 2) Done = true;
			break;
		case ByTIME:
			if(time1[T1] > time) Done = true;
			break;
		}

		if(adjust == true)
		{
			if(WhichSensors != 1) sonar_adj = ((distanceX - sonarLive)*SONAR_PROPORTION);
			if(ConstOrRel) adj_deg = (long) constHeading;
			else adj_deg = (long) relHeading;
			adj_power = adj_deg*GYRO_PROPORTION;
			if(WhichSensors == ByTIME) sonar_adj = 0;
			SetDrive(direction+(-sonar_adj+adj_power),power);
		}
	}
	if(StopWhenDone==true)
	{
		StopRobot();
	}
}
// =======================================================================
// Function to move the robot by the gyro and the sonar sensors V2
// Note: The option to predict driven distance based on time (in the event
// of loss of sonar signals) is dependent on the sonar distance having been
// zero at the start of the move - for example, the robot should be positioned
// against a wall.
// =======================================================================
bool GyroSonar_moveV2(int time, int WhichSensors, int distanceX, int distanceY, int power,bool StopWhenDone, bool adjust, bool ConstOrRel, bool prediction = false)
{
	step++;							// increase step number for data log file
	long adj_power;			// Reset all the values
	long adj_deg;				//
	int count = 0;					//
	int sonar_adj = 0;	//-----------------
	int lastvalue=9999;
	long current_power;
	bool seenOpponent = false;
	long MoveStartTime = nPgmTime-1;		// fudge start time to avoid devide by zero
	float AverageSpeed = 0;			// units of speed will be centimeters per millisecond
	float PredictedTime = 999999;
	int i = 0;
	relHeading =0;			//-----------------
	wait1Msec(20);
	motor[LF_motor] = -power;	//---------------
	motor[RF_motor] = power;	// Set beginning speeds
	motor[LR_motor] = -power; // for the motors
	motor[RR_motor] = power;	//---------------
	current_power = power;
	ClearTimer(T1);
	bool Done = false;
	while(!Done)
	{
		if(WhichSensors != SIDE)										//----------------
		{																				//
			if(sonarLive2 > distanceY)						// we have exceeded our target distance
			{
				if (sonarLive2!=lastvalue)					// we have a new reading above the target distance
				{
					count++;													// so increase our count of values above distance
					lastvalue=sonarLive2;							// and save the current reading for next time around
					AverageSpeed = sonarLive2/(nPgmTime-MoveStartTime);
					PredictedTime = distanceY/AverageSpeed;				// calculate predicted time it will take to drive desired distance
				}
			}
			else																	// readings are still below our taregt distance
			{
				count=0;														// reset our count since we are still seeing numbers below the target distance
			}
			if(WhichSensors == BACK)
			{
				if(sonarLive < 124)
				{
					i++;
					if(i>8) seenOpponent = true;
				}
			}
			if(count > 2)													// we have seen two different values about our target distance
			{
				Done = true;												// once we get two readings above target we exit the loop
			}
		}																				//
		else																		// we are controlling based on time
		{
			if (time1[T1] > time)									// if the timer exceeds the desired time then we exit the loop
			{
				Done = true;												// timer will also stop the move
			}
		}//----------------
		if ((time1[T1] > (long)PredictedTime) && (prediction == true)) 	// if speed based prediction is enabled then check the calculated value
		{
			Done = true;													// optionally the predicted time will also stop the mve
		}
		if(adjust == true)											// then we use sensors to maintain distance and orientation
		{
			if(WhichSensors != 1) 								// then we use the side sensor for wall alignment
			{
				sonar_adj = ((distanceX - sonarLive)*SONAR_PROPORTION);	// calculate the correction based on current distance
			}
			if(ConstOrRel) 												// decide whether adjustment is this move, or field relative
			{
				adj_deg = (long) constHeading;			// field relative, so calculate adjustment accordingly
			}
			else
			{
				adj_deg = (long) relHeading;				// relative to start of this move, so calculate adjustment accordingly
			}
			adj_power = adj_deg*GYRO_PROPORTION;	// now add together the two adjstment values
			/*
			motor[LF_motor] = -(current_power + adj_power + sonar_adj);
			motor[RF_motor] = current_power + adj_power - sonar_adj; // was +
			motor[LR_motor] = -(current_power + adj_power - sonar_adj);
			motor[RR_motor] = current_power + adj_power + sonar_adj;
			*/
			motor[LF_motor] = -(current_power - (adj_power + sonar_adj));	// drive the motors using the adjustment and current power values
			motor[RF_motor] = (current_power + (adj_power + sonar_adj));
			motor[LR_motor] = -(current_power);
			motor[RR_motor] = (current_power);
		}
	}
	if(StopWhenDone==true)
	{
		StopRobot();
	}
	return(seenOpponent);
}
// =======================================================================
// Function to move the robot by the gyro by time sideways V2
// =======================================================================
void GyroTimeS_moveV2(int time, int power, bool light,bool StopWhenDone, bool adjust, bool ConstOrRel)
{
	step++;							// increase step number for data log file
	long adj_power;
	long adj_deg;
	long current_power;
	int i = 0;
	relHeading =0;
	wait1Msec(200);
	motor[LF_motor] = -power;
	motor[RF_motor] = -power;
	motor[LR_motor] = power;
	motor[RR_motor] = power;
	current_power = power;
	ClearTimer(T1);
	bool Done = false;
	int addPower = 0;
	while(!Done)
	{
		RequestedScreen=S_LIGHT;		// force the light value screen to be displayed
		if(light == true)
		{
			if(light_normalised > light_threshold) i++;
			if(i > 5) Done = true;			// was 10, but that may be too high, now 5 at 21 Nov 2012
		}

		if(time1[T1] > time)
		{
			Done = true;
		}

		if(adjust == true)
		{
			if(ConstOrRel) adj_deg = (long) constHeading;
			else adj_deg = (long) relHeading;
			adj_power = adj_deg*GYRO_PROPORTION;

			//======================================

			//addPower = (current_power*5)/100;

			//if(power > 0)
			//{
			//	motor[LF_motor] = -(current_power-addPower); //more
			//	motor[RF_motor] = -((current_power+addPower) - adj_power);
			//	motor[LR_motor] = (current_power-addPower); //more
			//	motor[RR_motor] = ((current_power+addPower) + adj_power);
			//}
			//else
			//{
			//	motor[LF_motor] = -(current_power+addPower);
			//	motor[RF_motor] = -((current_power-addPower) - adj_power);
			//	motor[LR_motor] = (current_power+addPower);
			//	motor[RR_motor] = ((current_power-addPower) + adj_power);
			//}
			//==========================================================
			motor[LF_motor] = -current_power+adj_power;   // adj_power will be either plus or minus based on GYRO rotation
			motor[RF_motor] = -current_power+adj_power;   // if any rotation has been detected then power to all four
			motor[LR_motor] = current_power+adj_power;    // wheels needs to be adjusted to correct for it
			motor[RR_motor] = current_power+adj_power;
			//==========================================================
		}
	}
	if(StopWhenDone==true)
	{
		StopRobot();
	}
}
Пример #19
0
//=========================================
// Main Program
//=========================================
task main()
{
	initializeRobot();
	waitForStart();									// wait for FCS to tell us to go!

	if(calibrate != 2)							// GYRO calibration hasn't been run during the wait time
	{
		gyroCalTime = 3;							// so setup the default calibrate time
		calibrate = 1;								// start the calibration going
		while(calibrate != 2)					// and wait for it to complete before moving the robot
		{
			EndTimeSlice();
		}
	}
	constHeading = 0;								// reset the GYRO headings to eliminate any drift while we waited
	relHeading = 0;									// same thing for relative heading

	wait1Msec(Start_Delay*1000);		// implement the user configurable delay before moving
	PlaySound(soundBeepBeep);				// tell everyone we're about to start going

	servo[wrist] = WRIST_CLOSED;
	servo[shoulder] = SHOULDER_DOWN;
	servo[right_servo] = RIGHT_GRIPPER_START;
	servo[left_servo] = LEFT_GRIPPER_START;

	switch(MissionNumber)						// now go run whichever mission we have been asked to run
	{
		//================================================
		// start on the right side of the blue dispenser delivers to IR, then stops
		//================================================
	case 1:

		GyroSonar_moveV2(0, SIDE_BACK, 100, 108, -55,true, true, CONSTANT);
		GyroTimeS_moveV2(750,40,true,false,false,true);
		Gyro_TurnV2(36,-15,CONSTANT);

		if(abs(IR_Bearing) <=1) column = 2;
		if(IR_Bearing <-1 ) column = 1;
		if(IR_Bearing > 1) column = 3;

		wait1Msec(800);
		GyroTime_moveV2(1200,-30,true,false,false);
		GyroTimeS_moveV2(120,-15,true,false,false,false);
		motor[motorA] = -20;
		wait1Msec(1000);
		motor[motorA] = -3;
		GyroTimeS_moveV2(8000,18,true,true,false,false);
		GyroTimeS_moveV2(8000,-15,true,true,false,false);
		GyroTime_moveV2(1200,-30,true,false,false);

		//if(abs(IR_Bearing) <=1) column = 2;
		//if(IR_Bearing <-1 ) column = 1;
		//if(IR_Bearing > 1) column = 3;

		switch(column)
		{
		case 1:
			GyroTimeS_moveV2(700,-30,false,true,false,false);
			break;
			GyroTimeS_moveV2(8000,30,true,true,false,false);
			GyroTime_moveV2(250,-20,true,false,false);
			GyroTimeS_moveV2(140,-15,false,true,false,false);
			wait1Msec(1500);
			servo[shoulder] = SHOULDER_UP;
			wait1Msec(2000);
			servo[wrist] = WRIST_OPEN;
			wait1Msec(3000);
			GyroTime_moveV2(300,30,true,false,false);
			wait1Msec(3000);
			servo[shoulder] = SHOULDER_DOWN;
			wait1Msec(3000);
			break;

		case 2:
			wait1Msec(1500);
			servo[shoulder] = SHOULDER_UP;
			wait1Msec(1000);
			servo[wrist] = WRIST_OPEN;
			wait1Msec(3000);
			GyroTime_moveV2(400,25,true,false,false);
			wait1Msec(3000);
			servo[shoulder] = SHOULDER_DOWN;
			wait1Msec(3000);
			break;

		case 3:
			GyroTimeS_moveV2(700,30,false,true,false,false);
			break;
			GyroTimeS_moveV2(8000,-30,true,true,false,false);
			GyroTime_moveV2(250,20,true,false,false);
			GyroTimeS_moveV2(140,-15,false,true,false,false);
			wait1Msec(1500);
			servo[shoulder] = SHOULDER_UP;
			wait1Msec(2000);
			servo[wrist] = WRIST_OPEN;
			wait1Msec(3000);
			GyroTime_moveV2(300,30,true,false,false);
			wait1Msec(3000);
			servo[shoulder] = SHOULDER_DOWN;
			wait1Msec(3000);
			break;
		}
		break;
		//================================================
		// CENTER PEG
		//================================================

	case 2:
		GyroSonar_moveV2(0, SIDE_BACK, 100, 108, -45,true, true, CONSTANT);
		GyroTimeS_moveV2(750,40,true,false,false,true);
		Gyro_TurnV2(36,-15,CONSTANT);

		wait1Msec(800);
		GyroTime_moveV2(1080,-30,true,false,false);
		moveLightServo(DOWN);
		GyroTimeS_moveV2(8000,-15,true,true,false,false);
		GyroTimeS_moveV2(8000,18,true,true,false,false);
		GyroTimeS_moveV2(8000,-15,true,true,false,false);
		GyroTimeS_moveV2(90,10,true,false,false,false);
		GyroTime_moveV2(1200,-30,true,false,false);

		wait1Msec(1500);
		servo[shoulder] = SHOULDER_UP;
		wait1Msec(2000);
		servo[wrist] = WRIST_OPEN;
		wait1Msec(1000);
		GyroTime_moveV2(400,25,true,false,false);
		wait1Msec(1000);
		servo[shoulder] = SHOULDER_DOWN;

		switch(fieldRed)
		{
		case STOP:
			StopRobot();
			break;

		case RED:
			Gyro_TurnV2(42,-15,REL);
			GyroTime_moveV2(1200,50,true,false,REL);
			GyroTime45_V2(490,50,false, true, REL, true);
			GyroTime_moveV2(400,50,true,false,REL);
			break;

		case BLUE:
			Gyro_TurnV2(42,15,REL);
			GyroTime_moveV2(1600,45,true,false,false);
			break;
		}

		break;
		//================================================
		// LEFT PEG
		//================================================

	case 3:
		GyroSonar_moveV2(0, SIDE_BACK, 100, 108, -45,true, true, CONSTANT);
		GyroTimeS_moveV2(750,40,true,true,false,true);
		Gyro_TurnV2(36,-15,CONSTANT);

		wait1Msec(800);
		GyroTime_moveV2(1080,-30,true,false,false);
		motor[motorA] = -20;
		wait1Msec(1000);
		motor[motorA] = -3;
		GyroTimeS_moveV2(8000,-15,true,true,false,false);
		GyroTimeS_moveV2(8000,18,true,true,false,false);
		GyroTimeS_moveV2(8000,-15,true,true,false,false);
		Gyro_TurnV2(36,-15,CONSTANT);

		GyroTime_moveV2(1300,-30,true,false,false);
		GyroTime_moveV2(120,30,true,false,false);

		GyroTimeS_moveV2(480,20,true,false,false,false);
		GyroTimeS_moveV2(8000,15,true,true,false,false);
		GyroTimeS_moveV2(8000,-15,true,true,false,false);
		Gyro_TurnV2(36,-15,CONSTANT);
		GyroTimeS_moveV2(80,-18,true,false,false,false);
		GyroTimeS_moveV2(80,15,true,false,false,false);
		GyroTime_moveV2(400,-30,true,false,false);
		wait1Msec(1500);
		servo[shoulder] = SHOULDER_UP;
		wait1Msec(2000);
		servo[wrist] = WRIST_OPEN;
		wait1Msec(1000);
		GyroTime_moveV2(600,25,true,false,false);
		wait1Msec(3000);
		servo[shoulder] = SHOULDER_DOWN;

		switch(fieldRed)
		{
		case STOP:
			StopRobot();
			break;

		case RED:
			wait1Msec(3000);
			Gyro_TurnV2(42,-15,REL);
			GyroTime_moveV2(1200,50,true,false,REL);
			GyroTime45_V2(490,50,false, true, REL, true);
			GyroTime_moveV2(400,50,true,false,REL);
			break;

		case BLUE:
			wait1Msec(3000);
			Gyro_TurnV2(42,15,REL);
			GyroTime_moveV2(1600,45,true,false,false);
			break;
		}
		break;
		//================================================
		// RIGHT PEG
		//================================================

	case 4:

		GyroSonar_moveV2(0, SIDE_BACK, 100, 108, -45,true, true, CONSTANT);
		GyroTimeS_moveV2(750,40,true,true,false,true);
		Gyro_TurnV2(36,-15,CONSTANT);

		wait1Msec(800);
		GyroTime_moveV2(1080,-30,true,false,false);
		motor[motorA] = -20;
		wait1Msec(1000);
		motor[motorA] = -3;
		GyroTimeS_moveV2(8000,-15,true,true,false,false);
		GyroTimeS_moveV2(8000,18,true,true,false,false);
		GyroTimeS_moveV2(8000,-15,true,true,false,false);
		Gyro_TurnV2(36,-15,CONSTANT);

		GyroTime_moveV2(1300,-30,true,false,false);
		GyroTime_moveV2(100,30,true,false,false);

		Gyro_TurnV2(42,-15,CONSTANT);

		GyroTimeS_moveV2(460,-20,true,false,false,false);

		GyroTimeS_moveV2(8000,-18,true,true,false,false);
		GyroTimeS_moveV2(8000,18,true,true,false,false);
		GyroTimeS_moveV2(8000,-18,true,true,false,false);

		GyroTime_moveV2(240,-30,true,false,false);

		wait1Msec(1500);
		servo[shoulder] = SHOULDER_UP;
		wait1Msec(2000);
		servo[wrist] = WRIST_OPEN;
		wait1Msec(1000);
		GyroTime_moveV2(600,25,true,false,false);
		wait1Msec(3000);
		servo[shoulder] = SHOULDER_DOWN;

		switch(fieldRed)
		{
		case STOP:
			StopRobot();
			break;

		case RED:
			wait1Msec(3000);
			Gyro_TurnV2(42,-15,REL);
			GyroTime_moveV2(1200,50,true,false,REL);
			GyroTime45_V2(490,50,false, true, REL, true);
			GyroTime_moveV2(400,50,true,false,REL);
			break;

		case BLUE:
			wait1Msec(3000);
			Gyro_TurnV2(42,15,REL);
			GyroTime_moveV2(1600,45,true,false,false);
			break;
		}
		break;
		//================================================
		// CENTER PED SECOND POSITION
		//================================================
	case 5:

		GyroSonar_moveV2(0, BACK, 100, 120, -55,true, true, CONSTANT);
		GyroTimeS_moveV2(750,-40,true,true,false,true);
		Gyro_TurnV2(40,15,CONSTANT);

		wait1Msec(800);
		GyroTime_moveV2(1200,-30,true,false,false);
		motor[motorA] = -50;
		wait1Msec(1000);
		motor[motorA] = -3;
		GyroTimeS_moveV2(8000,-15,true,true,false,false);

		GyroTimeS_moveV2(8000,18,true,true,false,false);
		GyroTimeS_moveV2(8000,-15,true,true,false,false);

		Gyro_TurnV2(36,-15,CONSTANT);

		GyroTime_moveV2(360,-20,true,false,false);
		wait1Msec(1500);
		servo[shoulder] = SHOULDER_UP;
		wait1Msec(2000);
		servo[wrist] = WRIST_OPEN;
		wait1Msec(1000);
		GyroTime_moveV2(600,25,true,false,false);
		wait1Msec(1000);
		servo[shoulder] = SHOULDER_DOWN;

		switch(fieldRed)
		{
		case STOP:
			StopRobot();
			break;

		case RED:
			Gyro_TurnV2(42,-15,REL);
			GyroTime_moveV2(1200,50,true,false,REL);
			GyroTime45_V2(490,50,false, true, REL, true);
			GyroTime_moveV2(400,50,true,false,REL);
			break;

		case BLUE:
			Gyro_TurnV2(42,15,REL);
			GyroTime_moveV2(1600,45,true,false,false);
			break;
		}
		break;

		wait1Msec(1500);
		servo[shoulder] = SHOULDER_UP;
		wait1Msec(2000);
		servo[wrist] = WRIST_OPEN;
		wait1Msec(1000);
		GyroTime_moveV2(400,25,true,false,false);
		wait1Msec(3000);
		servo[shoulder] = SHOULDER_DOWN;

		switch(fieldRed)
		{
		case STOP:
			StopRobot();
			break;

		case RED:
			wait1Msec(3000);
			Gyro_TurnV2(42,-15,REL);
			GyroTime_moveV2(1200,50,true,false,REL);
			GyroTime45_V2(490,50,false, true, REL, true);
			GyroTime_moveV2(400,50,true,false,REL);
			break;

		case BLUE:
			wait1Msec(3000);
			Gyro_TurnV2(42,15,REL);
			GyroTime_moveV2(1600,45,true,false,false);
			break;
		}

		break;

		//================================================
		// RIGHT PEG SECOND POSITION
		//================================================
	case 6:

		GyroSonar_moveV2(0, BACK, 100, 120, -55,true, true, CONSTANT);
		GyroTimeS_moveV2(750,-40,true,true,false,true);
		Gyro_TurnV2(40,15,CONSTANT);

		wait1Msec(800);
		GyroTime_moveV2(1200,-30,true,false,false);
		motor[motorA] = -50;
		wait1Msec(1000);
		motor[motorA] = -3;
		GyroTimeS_moveV2(8000,-15,true,true,false,false);

		GyroTimeS_moveV2(8000,18,true,true,false,false);
		GyroTimeS_moveV2(8000,-15,true,true,false,false);

		Gyro_TurnV2(36,-15,CONSTANT);

		GyroTime_moveV2(1300,-30,true,false,false);
		GyroTime_moveV2(100,30,true,false,false);

		GyroTimeS_moveV2(360,-20,true,false,false,false);
		GyroTimeS_moveV2(8000,-18,true,true,false,false);
		GyroTimeS_moveV2(8000,18,true,true,false,false);
		GyroTimeS_moveV2(8000,-18,true,true,false,false);

		GyroTime_moveV2(240,-30,true,false,false);

		wait1Msec(1500);
		servo[shoulder] = SHOULDER_UP;
		wait1Msec(2000);
		servo[wrist] = WRIST_OPEN;
		wait1Msec(1000);
		GyroTime_moveV2(600,25,true,false,false);
		wait1Msec(3000);
		servo[shoulder] = SHOULDER_DOWN;

		switch(fieldRed)
		{
		case STOP:
			StopRobot();
			break;

		case RED:
			wait1Msec(3000);
			Gyro_TurnV2(42,-15,REL);
			GyroTime_moveV2(1200,50,true,false,REL);
			GyroTime45_V2(490,50,false, true, REL, true);
			GyroTime_moveV2(400,50,true,false,REL);
			break;

		case BLUE:
			wait1Msec(3000);
			Gyro_TurnV2(42,15,REL);
			GyroTime_moveV2(1600,45,true,false,false);
			break;
		}
		break;

		//================================================
		// LEFT PED SECOND POSITION
		//================================================

	case 7:

		GyroSonar_moveV2(0, BACK, 100, 120, -55,true, true, CONSTANT);
		GyroTimeS_moveV2(750,-40,true,true,false,true);
		Gyro_TurnV2(40,15,CONSTANT);

		wait1Msec(800);
		GyroTime_moveV2(1200,-30,true,false,false);
		motor[motorA] = -50;
		wait1Msec(1000);
		motor[motorA] = -3;
		GyroTimeS_moveV2(8000,-15,true,true,false,false);

		GyroTimeS_moveV2(8000,18,true,true,false,false);
		GyroTimeS_moveV2(8000,-15,true,true,false,false);

		Gyro_TurnV2(36,-15,CONSTANT);

		GyroTime_moveV2(1300,-30,true,false,false);
		GyroTime_moveV2(120,30,true,false,false);

		GyroTimeS_moveV2(260,20,true,false,false,false);
		GyroTimeS_moveV2(8000,15,true,true,false,false);
		GyroTimeS_moveV2(8000,-15,true,true,false,false);
		Gyro_TurnV2(36,-15,CONSTANT);
		GyroTimeS_moveV2(80,-18,true,false,false,false);
		GyroTime_moveV2(400,-30,true,false,false);
		wait1Msec(1500);
		servo[shoulder] = SHOULDER_UP;
		wait1Msec(2000);
		servo[wrist] = WRIST_OPEN;
		wait1Msec(1000);
		GyroTime_moveV2(600,25,true,false,false);
		wait1Msec(3000);
		servo[shoulder] = SHOULDER_DOWN;

		switch(fieldRed)
		{
		case STOP:
			StopRobot();
			break;

		case RED:
			wait1Msec(3000);
			Gyro_TurnV2(42,-15,REL);
			GyroTime_moveV2(1200,50,true,false,REL);
			GyroTime45_V2(490,50,false, true, REL, true);
			GyroTime_moveV2(400,50,true,false,REL);
			break;

		case BLUE:
			wait1Msec(3000);
			Gyro_TurnV2(42,15,REL);
			GyroTime_moveV2(1600,45,true,false,false);
			break;
		}
		break;


		//================================================
		// DEFENSE
		//================================================

	case 8:

		GyroTime_moveV2(1200,-50,true,false,REL);

		GyroSonar_moveV2(0, BACK, 100, 120, -55,true, true, CONSTANT);

		break;

		//================================================
		// OPPONENTS SIDE
		//================================================

	case 9:
		for (int i=0;i<360;i++)
		{SetDrive(i,50);
			wait1Msec(10);
		}
		StopRobot();
		wait1Msec(30000);
		break;

		//================================================
		// HIGH LIGHT
		//================================================

	case 10:
		for (int i=0;i<360;i++)
		{SetDrive(i,50);
			wait1Msec(10);
		}
		StopRobot();
		wait1Msec(30000);
		break;

		//================================================
		// HIGH LIGHT OPPONENTS
		//================================================

	case 11:
		for (int i=0;i<360;i++)
		{SetDrive(i,50);
			wait1Msec(10);
		}
		StopRobot();
		wait1Msec(30000);
		break;

		//================================================
		// ROBOT EVADE
		//================================================

	case 12:
		for (int i=0;i<360;i++)
		{SetDrive(i,50);
			wait1Msec(10);
		}
		StopRobot();
		wait1Msec(30000);
		break;
	}
}
//=========================================
// Main Program
//=========================================
task main()
{
	initializeRobot();
	waitForStart();									// wait for FCS to tell us to go!
	if(done)
	{
		StartTask(lightDiagnostic);
	}

	if(calibrate != 2)							// GYRO calibration hasn't been run during the wait time
	{
		gyroCalTime = 3;							// so setup the default calibrate time
		calibrate = 1;								// start the calibration going
		while(calibrate != 2)					// and wait for it to complete before moving the robot
		{
			EndTimeSlice();
		}
	}
	constHeading = 0;								// reset the GYRO headings to eliminate any drift while we waited
	relHeading = 0;									// same thing for relative heading


	wait1Msec(Start_Delay*1000);		// implement the user configurable delay before moving
	PlaySound(soundBeepBeep);				// tell everyone we're about to start going

	step = MissionNumber*100;				// this records the actual mission number that we ran within the data log file
	LogData=true;										// start saving data to the log file

	servo[wrist] = WRIST_CLOSED;
	servo[shoulder] = SHOULDER_DOWN;
	servo[right_servo] = RIGHT_GRIPPER_START;
	servo[left_servo] = LEFT_GRIPPER_START;

	switch(MissionNumber)						// now go run whichever mission we have been asked to run
	{
		//================================================
		// start on the right side of the blue dispenser delivers to IR
		//================================================
	case 1:

		IRside = position1();//go here

		if(IRside < -75) column = 1;
		else if(IRside < -25) column = 2;
		else column = 3;

		wait1Msec(800);

		switch(column)
		{
		case 1://left
			diagnosticBeeps(1);
			leftPeg();
			break;

		case 2://center
			diagnosticBeeps(2);
			centerPeg();
			break;

		case 3://right
			diagnosticBeeps(3);
			rightPeg();
			break;
		}
		switch(fieldRed)
		{
		case STOP:
			StopRobot();
			break;

		case RED:
			redReturn();
			break;

		case BLUE:
			blueReturn();
			break;
		}
		break;
		//================================================
		// CENTER PEG
		//================================================

	case 2:
		position1();
		centerPeg();

		switch(fieldRed)
		{
		case STOP:
			StopRobot();
			break;

		case RED:
			redReturn();
			break;

		case BLUE:
			blueReturn();
			break;
		}

		break;
		//================================================
		// LEFT PEG
		//================================================

	case 3:

		position1();
		leftPeg();

		switch(fieldRed)
		{
		case STOP:
			StopRobot();
			break;

		case RED:
			redReturn();
			break;

		case BLUE:
			blueReturn();
			break;
		}
		break;
		//================================================
		// RIGHT PEG
		//================================================

	case 4:

		position1();
		rightPeg();

		switch(fieldRed)
		{
		case STOP:
			StopRobot();
			break;

		case RED:
			redReturn();
			break;

		case BLUE:
			blueReturn();
			break;
		}
		break;
		//================================================
		// IR SECOND POSITION
		//================================================
	case 5:

		IRside = position2();//go here

		if(IRside < -75) column = 1;
		else if(IRside < -25) column = 2;
		else column = 3;

		wait1Msec(800);

		switch(column)
		{
		case 1://left
			diagnosticBeeps(1);
			leftPeg();
			break;

		case 2://center
			diagnosticBeeps(2);
			centerPeg();
			break;

		case 3://right
			diagnosticBeeps(3);
			rightPeg();
			break;
		}
		switch(fieldRed)
		{
		case STOP:
			StopRobot();
			break;

		case RED:
			redReturn();
			break;

		case BLUE:
			blueReturn();
			break;
		}
		break;
		//================================================
		// CENTER PED SECOND POSITION
		//================================================
	case 6:

		position2();
		centerPeg();

		break;

		//================================================
		// LEFT PEG SECOND POSITION
		//================================================
	case 7:

		position2();
		leftPeg();
		break;

		//================================================
		// RIGHT PED SECOND POSITION
		//================================================

	case 8:

		position2();
		rightPeg();

		break;

		//================================================
		// DEFENSE RIGHT
		//================================================

	case 9:

		GyroSonar_moveV2(0, BACK, 100, 124, -60,true, true, CONSTANT);
		GyroTime_moveV2(700,-50,true,true,REL);
		GyroTimeS_moveV2(1800,40,true,true,false,true);

		break;

		//================================================
		// DEFENSE LEFT
		//================================================

	case 10:

		GyroSonar_moveV2(0, BACK, 100, 124, -60,true, true, CONSTANT);
		GyroTime_moveV2(700,-50,true,true,REL);
		GyroTimeS_moveV2(1800,-40,true,true,false,true);

		break;

		//================================================
		// OPPONENT IR
		//================================================

	case 11:
		for (int i=0;i<90;i++)
		{SetDrive(i,50,i);
			wait1Msec(30);
		}
		StopRobot();
		wait1Msec(30000);
		break;

		//================================================
		// OPPONENT CENTER
		//================================================

	case 12:
		opponentRight();
		centerPeg();
		break;

		//================================================
		// OPPONENT LEFT
		//================================================

	case 13:
		opponentRight();
		rightPeg();
		break;

		//================================================
		// OPPONENT RIGHT
		//================================================

	case 14:
		opponentRight();
		leftPeg();
		break;
		//================================================
		// OPPONENTL IR
		//================================================

	case 15:
		opponentLeft();
		leftPeg();
		break;
		//================================================
		// OPPONENTL CENTER
		//================================================

	case 16:
		opponentLeft();
		centerPeg();
		break;
		//================================================
		// OPPONENTL LEFT
		//================================================

	case 17:
		opponentLeft();
		rightPeg();
		break;
		//================================================
		// OPPONENTL RIGHT
		//================================================

	case 18:
		opponentLeft();
		leftPeg();
		break;
		//================================================
		// See Opponent test
		//================================================

	case 19:
		sawOpponent = GyroSonar_moveV2(0, BACK, 100, 112, -50,true, true, CONSTANT);
		StopRobot();
		if(sawOpponent) diagnosticBeeps(6);
		break;
		//================================================
		// See Opponent test2
		//================================================

	case 20:
		sawOpponent = GyroSonar_moveV2(0, BACK, 100, 112, -50,true, true, CONSTANT);
		StopRobot();
		if(sawOpponent) diagnosticBeeps(6);
		break;
	}

	LogData=false;		// stop logging IR data and close the file
	StopRobot();
}
Пример #21
0
//=========================================
// Main Program
//=========================================
task main()
{
	initializeRobot();
	waitForStart();									// wait for FCS to tell us to go!
	if(done)
	{
		StartTask(lightDiagnostic);
	}

	if(calibrate != 2)							// GYRO calibration hasn't been run during the wait time
	{
		gyroCalTime = 3;							// so setup the default calibrate time
		calibrate = 1;								// start the calibration going
		while(calibrate != 2)					// and wait for it to complete before moving the robot
		{
			EndTimeSlice();
		}
	}
	constHeading = 0;								// reset the GYRO headings to eliminate any drift while we waited
	relHeading = 0;									// same thing for relative heading


	wait1Msec(Start_Delay*1000);		// implement the user configurable delay before moving
	PlaySound(soundBeepBeep);				// tell everyone we're about to start going

	step = MissionNumber*100;				// this records the actual mission number that we ran within the data log file
	LogData=true;										// start saving data to the log file

	servo[wrist] = WRIST_CLOSED;
	servo[shoulder] = SHOULDER_DOWN;
	servo[right_servo] = RIGHT_GRIPPER_START;
	servo[left_servo] = LEFT_GRIPPER_START;

	switch(MissionNumber)						// now go run whichever mission we have been asked to run
	{
		//================================================
		// start on the right side of the blue dispenser delivers to IR
		//================================================
	case 1:

		int IRside = position1();//go here

		if(IRside < -75) column = 1;
		else if(IRside < -25) column = 2;
		else column = 3;

		wait1Msec(800);

		switch(column)
		{
		case 1://left
			diagnosticBeeps(1);
			leftPeg();
			break;

		case 2://center
			diagnosticBeeps(2);
			centerPeg();
			break;

		case 3://right
			diagnosticBeeps(3);
			rightPeg();
			break;
		}
		switch(fieldRed)
		{
		case STOP:
			StopRobot();
			break;

		case RED:
			redReturn();
			break;

		case BLUE:
			blueReturn();
			break;
		}
		break;
		//================================================
		// CENTER PEG
		//================================================

	case 2:
		position1();
		centerPeg();

		switch(fieldRed)
		{
		case STOP:
			StopRobot();
			break;

		case RED:
			redReturn();
			break;

		case BLUE:
			blueReturn();
			break;
		}

		break;
		//================================================
		// LEFT PEG
		//================================================

	case 3:

		position1();
		leftPeg();

		switch(fieldRed)
		{
		case STOP:
			StopRobot();
			break;

		case RED:
			redReturn();
			break;

		case BLUE:
			blueReturn();
			break;
		}
		break;
		//================================================
		// RIGHT PEG
		//================================================

	case 4:

		position1();

		switch(fieldRed)
		{
		case STOP:
			StopRobot();
			break;

		case RED:
			redReturn();
			break;

		case BLUE:
			blueReturn();
			break;
		}
		break;
		//================================================
		// CENTER PED SECOND POSITION
		//================================================
	case 5:

		GyroSonar_moveV2(0, BACK, 100, 114, -48,true, true, CONSTANT);
		GyroTime_moveV2(400,-30,true,false,false);
		GyroTimeS_moveV2(750,-40,false,true,true,CONSTANT);
		Gyro_TurnV2(40,15,CONSTANT);

		wait1Msec(800);
		GyroTime_moveV2(1200,-30,true,false,false);
		moveLightServo(DOWN);

		//GyroTimeS_moveV2(8000,15,true,true,false,false);
		GyroTimeS_moveV2(8000,-15,true,true,false,false);
		GyroTimeS_moveV2(8000,18,true,true,false,false);
		GyroTimeS_moveV2(8000,-15,true,true,false,false);
		//GyroTimeS_moveV2(90,10,true,false,false,false);
		GyroTime_moveV2(1200,-30,true,false,false);

		wait1Msec(1500);
		servo[shoulder] = SHOULDER_UP;
		wait1Msec(2000);
		servo[wrist] = WRIST_OPEN;
		wait1Msec(1000);
		GyroTime_moveV2(400,25,true,false,false);
		wait1Msec(1000);
		servo[shoulder] = SHOULDER_DOWN;

		switch(fieldRed)
		{
		case STOP:
			StopRobot();
			break;

		case RED:
			Gyro_TurnV2(42,-15,REL);
			GyroTime_moveV2(1200,50,true,false,REL);
			GyroTime45_V2(490,50,false, true, REL, true);
			GyroTime_moveV2(400,50,true,false,REL);
			break;

		case BLUE:
			Gyro_TurnV2(42,15,REL);
			GyroTime_moveV2(1600,45,true,false,false);
			break;
		}
		break;

		//================================================
		// LEFT PEG SECOND POSITION
		//================================================
	case 6:

		/*
		GyroSonar_moveV2(0, BACK, 100, 114, -48,true, true, CONSTANT); // was this
		GyroTime_moveV2(400,-30,true,false,false);
		GyroTimeS_moveV2(750,-40,true,true,false,CONSTANT);
		Gyro_TurnV2(40,15,CONSTANT);
		*/


		GyroSonar_moveV2(0, BACK, 100, 114, -48,true, true, CONSTANT);  //trying this
		GyroTime_moveV2(400,-30,true,false,false);
		GyroTimeS_moveV2(750,-40,false,true,true,CONSTANT);
		Gyro_TurnV2(40,15,CONSTANT);

		wait1Msec(800);
		GyroTime_moveV2(1200,-30,true,false,false);
		moveLightServo(DOWN);

		//GyroTimeS_moveV2(8000,15,true,true,false,false);
		GyroTimeS_moveV2(8000,-15,true,true,false,false);
		GyroTimeS_moveV2(8000,18,true,true,false,false);
		GyroTimeS_moveV2(8000,-15,true,true,false,false);
		Gyro_TurnV2(36,-15,CONSTANT);

		GyroTime_moveV2(1300,-30,true,false,false);
		GyroTime_moveV2(120,30,true,false,false);

		GyroTimeS_moveV2(500,25,true,false,false,false);
		GyroTimeS_moveV2(2000,15,true,true,false,false);
		GyroTimeS_moveV2(8000,-10,true,true,false,false);
		Gyro_TurnV2(36,-15,CONSTANT);
		//GyroTimeS_moveV2(80,15,true,false,false,false);
		GyroTimeS_moveV2(8000,-25,true,true,false,false);
		GyroTimeS_moveV2(50,-20,true,false,false,false);
		GyroTime_moveV2(400,-30,true,false,false);
		wait1Msec(1500);
		servo[shoulder] = SHOULDER_UP;
		wait1Msec(2000);
		servo[wrist] = WRIST_OPEN;
		wait1Msec(1000);
		GyroTime_moveV2(600,25,true,false,false);
		wait1Msec(3000);
		servo[shoulder] = SHOULDER_DOWN;

		switch(fieldRed)
		{
		case STOP:
			StopRobot();
			break;

		case RED:
			break;

		case BLUE:
			wait1Msec(1000);
			Gyro_TurnV2(30,15,CONSTANT);
			GyroTime_moveV2(800,50,true,false,false);
			break;
		}
		break;

		//================================================
		// RIGHT PED SECOND POSITION
		//================================================

	case 7:

		/*
		GyroSonar_moveV2(0, BACK, 100, 114, -48,true, true, CONSTANT); //was this
		GyroTime_moveV2(400,-30,true,false,false);
		GyroTimeS_moveV2(750,-40,false,true,true,CONSTANT);
		Gyro_TurnV2(40,15,CONSTANT);
		*/


		GyroSonar_moveV2(0, BACK, 100, 114, -48,true, true, CONSTANT);  //trying this
		GyroTime_moveV2(400,-30,true,false,false);
		GyroTimeS_moveV2(750,-40,false,true,true,CONSTANT);
		Gyro_TurnV2(40,15,CONSTANT);

		break;

		wait1Msec(800);
		GyroTime_moveV2(1200,-30,true,false,false);
		moveLightServo(DOWN);
		//GyroTimeS_moveV2(8000,-15,true,true,false,false);

		GyroTimeS_moveV2(8000,18,true,true,false,false);
		GyroTimeS_moveV2(8000,-15,true,true,false,false);

		Gyro_TurnV2(36,-15,CONSTANT);

		GyroTime_moveV2(1300,-30,true,false,false);
		GyroTime_moveV2(120,30,true,false,false);

		GyroTimeS_moveV2(260,20,true,false,false,false);
		GyroTimeS_moveV2(8000,15,true,true,false,false);
		GyroTimeS_moveV2(8000,-15,true,true,false,false);
		Gyro_TurnV2(36,-15,CONSTANT);
		GyroTimeS_moveV2(80,-18,true,false,false,false);
		GyroTime_moveV2(400,-30,true,false,false);
		wait1Msec(1500);
		servo[shoulder] = SHOULDER_UP;
		wait1Msec(2000);
		servo[wrist] = WRIST_OPEN;
		wait1Msec(1000);
		GyroTime_moveV2(600,25,true,false,false);
		wait1Msec(3000);
		servo[shoulder] = SHOULDER_DOWN;

		switch(fieldRed)
		{
		case STOP:
			StopRobot();
			break;

		case RED:
			wait1Msec(3000);
			Gyro_TurnV2(42,-15,REL);
			GyroTime_moveV2(1200,50,true,false,REL);
			GyroTime45_V2(490,50,false, true, REL, true);
			GyroTime_moveV2(400,50,true,false,REL);
			break;

		case BLUE:
			wait1Msec(3000);
			Gyro_TurnV2(42,15,REL);
			GyroTime_moveV2(1600,45,true,false,false);
			break;
		}
		break;


		//================================================
		// DEFENSE RIGHT
		//================================================

	case 8:

		GyroSonar_moveV2(0, BACK, 100, 124, -60,true, true, CONSTANT);
		GyroTime_moveV2(700,-50,true,true,REL);
		GyroTimeS_moveV2(1800,40,true,true,false,true);

		break;

		//================================================
		// DEFENSE LEFT
		//================================================

	case 9:

		GyroSonar_moveV2(0, BACK, 100, 124, -60,true, true, CONSTANT);
		GyroTime_moveV2(700,-50,true,true,REL);
		GyroTimeS_moveV2(1800,-40,true,true,false,true);

		break;

		//================================================
		// OPPONENTS SIDE
		//================================================

	case 10:
		for (int i=0;i<360;i++)
		{SetDrive(i,50);
			wait1Msec(10);
		}
		StopRobot();
		wait1Msec(30000);
		break;

		//================================================
		// TEST ORBIT
		//================================================

	case 11:
		for (int i=0;i<360;i++)
		{SetDrive(i,50,i);
			wait1Msec(10);
		}
		StopRobot();
		wait1Msec(30000);
		break;

		//================================================
		// HIGH LIGHT OPPONENTS
		//================================================

	case 12:
		for (int i=0;i<50;i++)
		{
			moveLightServo(UP);
			wait1Msec(1000);
			moveLightServo(DOWN);
			wait1Msec(1000);
		}
		break;

		//================================================
		// ROBOT EVADE
		//================================================

	case 13:
		wait1Msec(30000);
		break;
	}

	LogData=false;		// stop logging IR data and close the file
	StopRobot();
}