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"

// Definitions here!

#define MAX_SPEED 127
// Set motor maximum speed, this allows for tweaking the speed of the robot with one change.

#define MAX_AUTO_SPEED 100
/* During the development of the autonomous portion of our code, we found that the robot 
   would have issues turning and driving at MAX_SPEED causing it to turn too much, and 
   not driving straight. After limiting the driving speed to 100, we found that the robot 
   was able to drive more consistently. 
*/

#define STOP 0
// Defines the value for when a motor is stopped.
  
#define DEADZONE 10
/* Defines the deadzone of the VEX controller. With our controllers, a value of 10 allowed 
   for the motors to completely lose power when the joystick is let go. 
*/  

#define DRIVE_OFFSET 10
/* Defines the offset used to correct and curves while the robot is driving straight during the 
   driveTiles(float numberOfTiles, bool direction) function.
*/

#define TILE 1206
/* Definition for Rotation points per tile. 

   Each tile is 23.4 inches wide.


   The exact radius of 4" omni wheels, using dial calipers: 2.075 inches

   2 * pi * r is circumference of the wheel - 13.0376 inches


   There are 627.2 points in a revolution with the vex direct motor encoders - according to robotc 
   developers: “The 2-wire 393 motor measures 627.2 counts per revolution of the output shaft in its 
   default high-torque configuration and 392 counts per revolution of the output shaft in its 
   modified high-speed configuration.”


   So if we do 1 revolution * distance / radius we get 627.2 * 23.4 / 13.0376 = 1206. 
   When the integrated motor encoder reports a movement of 1206, that means the robot has moved 1 tile. 
*/


// How much the wheels should spin in a 90 degree turn
#define POINTS_PER_TURN 320
/* Using trial and error, we found that our robot will make a 90 degree turn when the integrated motor 
   encoders report a distance of 320 while spinning in place.
*/


// definitions for driveTiles()
#define FORWARD true
#define REVERSE false
/* When the function driveTiles(float numberOfTiles, bool direction) is called, one of the explicit 
   parameters is a boolean for direction, where true is forward, and false is reverse. Using these 
   definitions in our code, it is clearer to us and readers as to what that parameter is for. 
*/

void stopDriving() {
  motor[driveLB] = STOP;
  motor[driveLF] = STOP;
  motor[driveRB] = STOP;
  motor[driveRF] = STOP;
}
// Explicit Parameters: None
// Output: All four driving motors will be stopped, stopping the robot’s movements immediately. 



void clearEnc() { // Reset driving motor encoder values to 0
    nMotorEncoder[driveRB] = 0;
    nMotorEncoder[driveLB] = 0;
}
// Explicit Parameters: None
// Output: Resets the driving encoders to 0, for use in other autonomous functions.



void shootBall() {
  motor[shoot] = MAX_SPEED;
  wait(1.25); // Shooting takes 1.25 seconds
  motor[shoot] = STOP;
}
// Explicit Parameters: None
// Output: The 2 motors connected to the shoot port will turn on for 1.25 seconds, which is 
// precisely the amount of time needed for the motors to pull back and release the launcher. 


void turntoRight(float turns) {
	clearEnc();
  while(turns * POINTS_PER_TURN > nMotorEncoder[driveLB]){
    motor[driveLB] = MAX_AUTO_SPEED;
    motor[driveLF] = MAX_AUTO_SPEED;
    motor[driveRB] = -MAX_AUTO_SPEED;
    motor[driveRF] = -MAX_AUTO_SPEED;
  }
  stopDriving();
}
// Explicit Parameters: None
// Output: These three functions manage the ball lift and, conveniently, the same motors in reverse 
// will flip a cap.  flipOn() will spin the motors in the direction needed to flip caps, ballIn() 
// will spin the motors in the direction needed to collect and pick up balls, and ballOff() will turn off the motors. 


void turntoLeft(float turns) {
  clearEnc();
	while(turns * POINTS_PER_TURN > nMotorEncoder[driveRB]){
    motor[driveLB] = -MAX_AUTO_SPEED;
    motor[driveLF] = -MAX_AUTO_SPEED;
    motor[driveRB] = MAX_AUTO_SPEED;
    motor[driveRF] = MAX_AUTO_SPEED;
  }
  stopDriving();
}


void flipOn() {
  motor[bintake] = -MAX_SPEED;
}
void ballOff() {
  motor[bintake] = STOP;
}
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) {
    ballIn();
  }
  else if (vexRT[Btn5D] == 1) {
  	flipOn();
  }
  else {
    ballOff();
  }
  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]) - DRIVE_OFFSET > nMotorEncoder[driveLB]) {
    		motor[driveLB] = MAX_AUTO_SPEED;
    		motor[driveLF] = MAX_AUTO_SPEED;
      	motor[driveRB] = MAX_AUTO_SPEED - DRIVE_OFFSET;
    		motor[driveRF] = MAX_AUTO_SPEED - DRIVE_OFFSET;
    	}
      if(abs(nMotorEncoder[driveLB]) - DRIVE_OFFSET > nMotorEncoder[driveRB]) {
    		motor[driveLB] = MAX_AUTO_SPEED - DRIVE_OFFSET;
    		motor[driveLF] = MAX_AUTO_SPEED - DRIVE_OFFSET;
      	motor[driveRB] = MAX_AUTO_SPEED;
    		motor[driveRF] = MAX_AUTO_SPEED;
    	} else {
      	motor[driveLB] = MAX_AUTO_SPEED;
    		motor[driveLF] = MAX_AUTO_SPEED;
      	motor[driveRB] = MAX_AUTO_SPEED;
    		motor[driveRF] = MAX_AUTO_SPEED;
    	}
	}
    while(direction == REVERSE && numberOfTiles * TILE - 200 > -nMotorEncoder[driveRB]) {
			if(abs(nMotorEncoder[driveRB]) - DRIVE_OFFSET > nMotorEncoder[driveLB]) {
    		motor[driveLB] = -MAX_AUTO_SPEED;
    		motor[driveLF] = -MAX_AUTO_SPEED;
      	motor[driveRB] = -MAX_AUTO_SPEED + DRIVE_OFFSET;
    		motor[driveRF] = -MAX_AUTO_SPEED + DRIVE_OFFSET;
    	}
      if(abs(nMotorEncoder[driveLB]) - DRIVE_OFFSET > nMotorEncoder[driveLB]) {
    		motor[driveLB] = -MAX_AUTO_SPEED + DRIVE_OFFSET;
    		motor[driveLF] = -MAX_AUTO_SPEED + DRIVE_OFFSET;
      	motor[driveRB] = -MAX_AUTO_SPEED;
    		motor[driveRF] = -MAX_AUTO_SPEED;
    	} else {
      	motor[driveLB] = -MAX_AUTO_SPEED;
    		motor[driveLF] = -MAX_AUTO_SPEED;
      	motor[driveRB] = -MAX_AUTO_SPEED;
    		motor[driveRF] = -MAX_AUTO_SPEED;
    	}
	}
	stopDriving();
}


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);
  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.
  }
}