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 360 

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

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 clearEnc();
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(int numberOfTiles, bool direction) { 
    // when direction is true, move forward, otherwise go in reverse
	clearEnc();
	while(direction == FORWARD && numberOfTiles * TILE > nMotorEncoder[1]) {
			if(abs(nMotorEncoder[1]) - 10 > nMotorEncoder[2]) {
    		motor[driveLB] = 100;
    		motor[driveLF] = 100; 
      	motor[driveRB] = 90;
    		motor[driveRF] = 90;
    	}
      if(abs(nMotorEncoder[2]) - 10 > nMotorEncoder[1]) {
    		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 > nMotorEncoder[1]) {
			if(abs(nMotorEncoder[1]) - 10 > nMotorEncoder[2]) {
    		motor[driveLB] = -100;
    		motor[driveLF] = -100; 
      	motor[driveRB] = -90;
    		motor[driveRF] = -90;
    	}
      if(abs(nMotorEncoder[2]) - 10 > nMotorEncoder[1]) {
    		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() {
	shootBall();
	driveTiles(2, true); // Move 2 forward to hit bottom flag
    //driveTiles(1, false);
}
void turnRight(int turns) {
  while(turns * POINTS_PER_TURN < nMotorEncoder[1]){
    	
  }
}
void turnLeft(int turns) {
  while(turns * POINTS_PER_TURN < nMotorEncoder[1]){
    	
  }  
}
      
void clearEnc() { // Reset driving motor encoder values to 0
    nMotorEncoder[1] = 0;
    nMotorEncoder[2] = 0;    
}
task usercontrol() { // In user control mode
  while (true) {
    joystickDrive(); // Joystick mapping function
    buttonChecks();  // Button mapping, for lift, ball launcher, etc.
  }
}