mobilerobotics/main.c

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Motor,  port1,            ,             tmotorVex393_HBridge, openLoop)
#pragma config(Motor,  port2,           shoot,         tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port3,           driveLB,       tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_2)
#pragma config(Motor,  port4,           driveLF,       tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port5,           driveRB,       tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_1)
#pragma config(Motor,  port6,           driveRF,       tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port7,           bintake,       tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port8,            ,             tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port9,            ,             tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port10,           ,             tmotorVex393_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX2)
#pragma competitionControl(Competition)
#include "Vex_Competition_Includes.c"

// some variable definitions
#define MAX_SPEED 127 // Max speed of the motors
#define STOP 0
#define DEADZONE 10

/*
  23.4 / (2 * pi * 2.075) * 672.2
  23.4 = inches in a tile
  2.075 = exact radius of 4" omniwheels
  2 * pi * r is circumference of the wheel
  627.2 points in a revolution with the vex encoders
  final calculation is the amount of points of rotation per tile of movement
*/
#define TILE 1206

// How much the wheels should spin in a 90 degree turn
#define POINTS_PER_TURN 320

// definitions for driveTiles()
#define FORWARD true
#define REVERSE false

void clearEnc() { // Reset driving motor encoder values to 0
    nMotorEncoder[driveRB] = 0;
    nMotorEncoder[driveLB] = 0;
}

void shootBall() {
  motor[shoot] = MAX_SPEED;
  wait(1.25); // Shooting takes 1.25 seconds.
  // Any unwanted extra movement will be undone by the rubber bands.
  motor[shoot] = STOP;
}
void turntoRight(float turns) {
	clearEnc();
  while(turns * POINTS_PER_TURN > nMotorEncoder[driveLB]){
    motor[driveLB] = 100;
    motor[driveLF] = 100;
    motor[driveRB] = -100;
    motor[driveRF] = -100;
  }
  motor[driveLB] = 0;
  motor[driveLF] = 0;
  motor[driveRB] = 0;
  motor[driveRF] = 0;
}
void turntoLeft(float turns) {
  clearEnc();
	while(turns * POINTS_PER_TURN > nMotorEncoder[driveRB]){
    motor[driveLB] = -100;
    motor[driveLF] = -100;
    motor[driveRB] = 100;
    motor[driveRF] = 100;
  }
  motor[driveLB] = 0;
  motor[driveLF] = 0;
  motor[driveRB] = 0;
  motor[driveRF] = 0;
}


void flipOn() {
  motor[bintake] = -MAX_SPEED;
}
void ballOff() {
  motor[bintake] = 0;
}
void ballIn() {
  motor[bintake] = MAX_SPEED;
}

void joystickDrive() {
  if(abs(vexRT[Ch3]) > DEADZONE) {
    motor[driveLB] = vexRT[Ch3];
    motor[driveLF] = vexRT[Ch3];
  }
  else {
  	motor[driveLB] = STOP;
    motor[driveLF] = STOP;
	}
  if(abs(vexRT[Ch2]) > DEADZONE) {
    motor[driveRB] = vexRT[Ch2];
    motor[driveRF] = vexRT[Ch2];
  }
  else {
  	motor[driveRB] = STOP;
    motor[driveRF] = STOP;
  }
}

void buttonChecks() {
  if (vexRT[Btn5U] == 1) {
    motor[bintake] = MAX_SPEED;
  }
  else if (vexRT[Btn5D] == 1) {
  	motor[bintake] = -MAX_SPEED;
  }
  else {
    motor[bintake] = STOP;
  }
  if (vexRT[Btn8D] == 1) {
  	shootBall();
  } // No need for reverse on the ball launcher!
}



void pre_auton() {
  /* Set bStopTasksBetweenModes to false if you want to keep user created tasks
  running between Autonomous and Driver controlled modes. You will need to
  manage all user created tasks if set to false. */
  bStopTasksBetweenModes = true;
}

void driveTiles(float numberOfTiles, bool direction) {
    // when direction is true, move forward, otherwise go in reverse
	clearEnc();
	while(direction == FORWARD && numberOfTiles * TILE - 200 > nMotorEncoder[driveRB]) {
			if(abs(nMotorEncoder[driveRB]) - 10 > nMotorEncoder[driveLB]) {
    		motor[driveLB] = 100;
    		motor[driveLF] = 100;
      	motor[driveRB] = 90;
    		motor[driveRF] = 90;
    	}
      if(abs(nMotorEncoder[driveLB]) - 10 > nMotorEncoder[driveRB]) {
    		motor[driveLB] = 90;
    		motor[driveLF] = 90;
      	motor[driveRB] = 100;
    		motor[driveRF] = 100;
    	} else {
      	motor[driveLB] = 100;
    		motor[driveLF] = 100;
      	motor[driveRB] = 100;
    		motor[driveRF] = 100;
    	}
	}
    while(direction == REVERSE && numberOfTiles * TILE - 200 > -nMotorEncoder[driveRB]) {
			if(abs(nMotorEncoder[driveRB]) - 10 > nMotorEncoder[driveLB]) {
    		motor[driveLB] = -100;
    		motor[driveLF] = -100;
      	motor[driveRB] = -90;
    		motor[driveRF] = -90;
    	}
      if(abs(nMotorEncoder[driveLB]) - 10 > nMotorEncoder[driveLB]) {
    		motor[driveLB] = -90;
    		motor[driveLF] = -90;
      	motor[driveRB] = -100;
    		motor[driveRF] = -100;
    	} else {
      	motor[driveLB] = -100;
    		motor[driveLF] = -100;
      	motor[driveRB] = -100;
    		motor[driveRF] = -100;
    	}
	}
	motor[driveLB] = 0;
  motor[driveLF] = 0;
  motor[driveRB] = 0;
  motor[driveRF] = 0;
}
task autonomous() {
	turntoRight(0.03);
	shootBall();
	turntoLeft(0.03);
	driveTiles(2, FORWARD); // Move 2 forward to hit bottom flag
  driveTiles(1, REVERSE);
  turntoRight(1);
  driveTiles(0.5, REVERSE); // Drive 1/3 of a tile backwards to hit the wall and align ourselves!
  flipOn(); // Turn on the ball intake in reverse, which is what we can use to flip the caps
  driveTiles(1.6, FORWARD); // flip cap
  ballOff();
  driveTiles(1.35, REVERSE);
  turntoLeft(1);
  driveTiles(1, REVERSE);
  turntoRight(1);
  driveTiles(0.6, REVERSE);
  driveTiles(2.1, FORWARD); // Flip the other cap without turning on the spinner
  flipOn();
  driveTiles(0.5, FORWARD);
  ballIn();
  driveTiles(0.1, REVERSE);
   // So we can pick up the ball!
  wait(3);
  driveTiles(0.1, REVERSE);
  turntoLeft(1);
  driveTiles(0.2, REVERSE);
  wait(3);
  ballOff();
  shootBall();
  driveTiles(0.05, REVERSE);
  driveTiles(0.33, FORWARD); // Hit middle column bottom flag
	//driveTiles(2, REVERSE);
  wait(2);
	turntoRight(1);
	driveTiles(2.2, REVERSE);
	turntoLeft(1);
	driveTiles(1, REVERSE);
	turntoRight(1);
	driveTiles(0.25, REVERSE);
	driveTiles(3, FORWARD);

}
task usercontrol() { // In user control mode
  while (true) {
    joystickDrive(); // Joystick mapping function
    buttonChecks();  // Button mapping, for lift, ball launcher, etc.
  }
}