autonomous_test.c

#pragma config(Hubs,  S4, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S1,     HTDIR,          sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S2,     TOUCH,          sensorTouch)
#pragma config(Sensor, S3,     HTDIR,          sensorHiTechnicIRSeeker1200)
#pragma config(Motor,  mtr_S4_C1_1,     motorD,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S4_C1_2,     motorE,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S4_C2_1,     motorF,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S4_C2_2,     motorG,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S4_C3_1,     motorH,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S4_C3_2,     motorI,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S4_C4_1,    servo1,               tServoStandard)
#pragma config(Servo,  srvo_S4_C4_2,    servo2,               tServoStandard)
#pragma config(Servo,  srvo_S4_C4_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S4_C4_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S4_C4_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S4_C4_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.
#include "axel_rose_constants.c"

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                           Autonomous Mode Code Template
//
// This file contains a template for simplified creation of an autonomous program for an TETRIX robot
// competition.
//
// You need to customize two functions with code unique to your specific robot.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////



/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                    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.
	servo[servo2] = AUTO_ARM_STARTING_POSITION;
	servo[servo1] = CLAW_STARTING_POSITION;
}


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                         Main Task
//
// The following is the main code for the autonomous robot operation. Customize as appropriate for
// your specific robot.
//
// The types of things you might do during the autonomous phase (for the 2008-9 FTC competition)
// are:
//
//   1. Have the robot follow a line on the game field until it reaches one of the puck storage
//      areas.
//   2. Load pucks into the robot from the storage bin.
//   3. Stop the robot and wait for autonomous phase to end.
//
// This simple template does nothing except play a periodic tone every few seconds.
//
// At the end of the autonomous period, the FMS will autonmatically abort (stop) execution of the program.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

task main(){
  initializeRobot();
  waitForStart(); // Wait for the beginning of autonomous phase.


  motor[motorH] = 30;
  motor[motorI] = -30;
  wait1Msec(750);

  motor[motorH] = 0;
  motor[motorI] = 0;

  ///////////////////////////////////////////////////////////
  ///////////////////////////////////////////////////////////
  ////                                                   ////
  ////    Add your robot specific autonomous code here.  ////
  ////                                                   ////
  ///////////////////////////////////////////////////////////
  ///////////////////////////////////////////////////////////

  nxtDisplayString(0, "Running guess beacon 101...");
  nxtDisplayString(0, "Beacon position");
  nxtDisplayString(2, "Sensor 1 value");
  nxtDisplayString(4, "Sensor 2 value");
  nxtDisplayString(3, "%d", SensorValue[S1]);

  // Find the beacon.
  int numtick=0;
  long total_ticks = 0;
  while (numtick < IR_SENSOR_SENSITIVITY){
  	total_ticks++;
  	if (SensorValue[S1]<=5 && SensorValue[S1]>0) {
  		numtick++;
  	}
  	else {
  		numtick=0;
  	}
	  motor[motorD]=IR_SEEKING_SPEED;
	  motor[motorE]=-IR_SEEKING_SPEED;
	}
  motor[motorD]=0;
  motor[motorE]=0;

  // Back up a bit.
  nxtDisplayString(0, "t = %d", total_ticks);
  if (total_ticks < IR_MIDPOINT) {
	  motor[motorD]=-IR_BACKUP_SPEED;
	  motor[motorE]=IR_BACKUP_SPEED;
	  wait1Msec(IR_SHORT_BACKUP);
	} else {
	  motor[motorD]=-IR_BACKUP_SPEED;
	  motor[motorE]=IR_BACKUP_SPEED;
	  wait1Msec(IR_LONG_BACKUP);
  }
  motor[motorD]=0;
  motor[motorE]=0;

  // Drop the block.
  servo[servo2]=AUTO_ARM_DUMP_POSITION;
  wait1Msec(AUTO_ARM_DUMP_DELAY);
  servo[servo2]=AUTO_ARM_STARTING_POSITION;
  wait1Msec(AUTO_ARM_DUMP_DELAY);

  // Bump into the wall.
  while (SensorValue[S2] == 0) {
     motor[motorD]=WALL_SEEKING_SPEED;
	   motor[motorE]=-WALL_SEEKING_SPEED;
  }

  // Back up a bit.
  motor[motorD]=0;
	motor[motorE]=0;
	motor[motorD]=-HALF_SPEED;
	motor[motorE]=HALF_SPEED;
  wait1Msec(DR_BACKUP_AMOUNT);

  // Turn to the left.
  motor[motorD]=FULL_SPEED;
  motor[motorE]=FULL_SPEED;
  wait1Msec(DR_TURN_AWAY_FROM_WALL_AMOUNT);

  // Go forward to line up with the ramp.
  motor[motorD]=HALF_SPEED;
  motor[motorE]=-HALF_SPEED;
  wait1Msec(DR_DRIVE_AWAY_FROM_WALL_AMOUNT);

  // Turn towards the ramp.
  motor[motorD]=FULL_SPEED;
  motor[motorE]=FULL_SPEED;
  wait1Msec(DR_TURN_TOWARDS_RAMP_AMOUNT);

  // Drive up onto the ramp.
  motor[motorD]=FULL_SPEED;
  motor[motorE]=-FULL_SPEED;
  wait1Msec(DR_DRIVE_ONTO_RAMP);

  // Stop.
  motor[motorD]=0;
  motor[motorE]=0;
}