Autonomous.c

#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Sensor, S2,     irSeekSns,      sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S3,     light1Sns,      sensorLightActive)
#pragma config(Sensor, S4,     light2Sns,      sensorLightActive)
#pragma config(Motor,  mtr_S1_C1_1,     backRight,     tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     frontLeft,     tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     frontRight,    tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_2,     backLeft,      tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C4_1,     armMotor,      tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C4_2,     rampMotor,     tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    servo1,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_2,    sensorServo,          tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_6,    clawServo,            tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                           Autonomous Mode Code
//
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.
#include "Util/drivers/hitechnic-irseeker-v2.h"
#include "omniDrive.h"

// !!!!!!!!!!!!!! TODO: MOVE TO COMMON FILE FOR TELEOP TO USE !!!!!!!!!!

// constants
#define ARM_ZEROIZE_POWER -10  // hope this is low enough
#define ARM_POS_MAX 360
#define ARM_2ND_ROW_POS 90
#define ARM_1ST_ROW_POS 180
#define IR_STRENGTH_CLOSE 50 // TODO: choose a good value

// global variables
OmniMotors gMotors;


void initArm() {
  long lastArmPos = nMotorEncoder[armMotor];

	// Move arm to home
  motor[armMotor] = ARM_ZEROIZE_POWER;

  // go until we are not moving (apparently)
  while( nMotorEncoder[armMotor] - lastArmPos < -1 ) {
  	wait1Msec( 500 );
  	lastArmPos = nMotorEncoder[armMotor];
  }
  // done moving
  motor[armMotor] = 0;

  // tell the system this is the new zero position
  nMotorEncoder[armMotor] = 0;
}

void initDriveMotorStruct() {
  gMotors.frontLeft  = frontLeft;
  gMotors.frontRight = frontRight;
  gMotors.backLeft   = backLeft;
  gMotors.backRight  = backRight;
}

void zeroDriveMotors() {
	motor[gMotors.frontLeft]  = 0;
  motor[gMotors.backRight]  = 0;
  motor[gMotors.backLeft]   = 0;
  motor[gMotors.frontRight] = 0;
}

void initDriveMotors() {
	// ensure our struct is set up
	initDriveMotorStruct();
	// zeroize motors for good measure
	zeroDriveMotors();
}



void initializeRobot() {
	initDriveMotors();
	initArm();

  return;
}


/**
 * move the arm to a given position
 * @param pos   the target position of the arm
 * @param speed the motor power to get to pos
 */
void moveArmTo( long pos, int speed ) {
	pos = clamp( pos, 1, ARM_POS_MAX ); // 0 is special
	if( pos == nMotorEncoder[armMotor] )
		return;
	speed = sgn( pos - nMotorEncoder[armMotor] ) * abs( clamp( speed, -100, 100 ) );
	nMotorEncoderTarget[armMotor] = pos;
	motor[armMotor] = (pos - nMotorEncoder[armMotor]) > 0 ? speed : -speed;

	// while Motor is still running (hasn't reached target yet) just wait longer
	while( nMotorRunState[armMotor] != runStateIdle) {
    wait1Msec( 10 );
  }
  motor[armMotor] = 0;
  // prevent this from targeting other opterations
  nMotorEncoderTarget[armMotor] = 0;
}


/**
 * move the arm by a given amount
 * @param deltaPos the target change of position of the arm
 * @param speed    the motor power to get to pos
 */
void moveArmBy( long deltaPos, int speed ) {
	moveArmTo( nMotorEncoder[armMotor] + deltaPos, speed );
}


// !!!!!!!!!!!!!!!!!!!!!!!!



bool driveToBeacon() {
	int acS0 = 0, acS1 = 0, acS2 = 0, acS3 = 0, acS4 = 0;
	int irDir, strength = 0;

	// the default DSP mode is 1200 Hz.
  tHTIRS2DSPMode mode = DSP_1200;

  // wait for sensor
  while( ! HTIRS2setDSPMode( irSeekSns, mode ) ) {
    wait1Msec( 100 );
  }

  do {
  	// Read the individual signal strengths of the internal sensors
    if( !HTIRS2readAllACStrength( irSeekSns, acS0, acS1, acS2, acS3, acS4 ) )
      break; // I2C read error occurred

    // Read the Enhanced direction and strength
	  if( !HTIRS2readEnhanced( irSeekSns, irDir, strength ) )
      break; // I2C read error occurred

    // if we are not there yet drive toward the target
    if( strength < IR_STRENGTH_CLOSE ) {
      omniDrive( gMotors, 1, 0, 50, 0.333 * (acS2 + acS3 + acS4) - 0.5 * (acS0 + acS1) );
      wait1Msec( 100 );
    }
  } while( strength < IR_STRENGTH_CLOSE );

  return strength >= IR_STRENGTH_CLOSE;
}

task main() {
  initializeRobot();

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

	moveArmTo( ARM_2ND_ROW_POS, 50 );

	driveToBeacon();

	// placeRing();

	// rejoyce
}