robot.c

#pragma config(Sensor, in1,    posPotent,      sensorPotentiometer)
#pragma config(Sensor, in2,    progPotent,     sensorPotentiometer)
#pragma config(Sensor, dgtl1,  rightEncoder,   sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  leftEncoder,    sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  leftSonar,      sensorSONAR_mm)
#pragma config(Motor,  port1,           intake1,       tmotorVex393TurboSpeed_HBridge, openLoop)
#pragma config(Motor,  port2,           left1,         tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port3,           left2,         tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port4,           left3,         tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port5,           puncher,       tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port6,           flipper,       tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port7,           right3,        tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port8,           right2,        tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port9,           right1,        tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port10,          intake2,       tmotorVex393TurboSpeed_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*********************************************************************/
/*********************************************************************/
/*********************************************************************/

/** --------------------- 72832S competition ctrl -------------------*/

/*********************************************************************/
/*********************************************************************/
/*********************************************************************/

// Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(15)
#pragma userControlDuration(105)

#pragma systemFile

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*        Description: Competition template for VEX EDR                      */
/*                                                                           */
/*---------------------------------------------------------------------------*/

// This code is for the VEX cortex platform
#pragma platform(VEX2)

// Select Download method as "competition"
#pragma competitionControl(Competition)

// Main competition background code...do not modify!

/*---------------------------------------------------------------------------*/
/*                          Pre-Autonomous Functions                         */
/*                                                                           */
/*  You may want to perform some actions before the competition starts.      */
/*  Do them in the following function.  You must return from this function   */
/*  or the autonomous and usercontrol tasks will not be started.  This       */
/*  function is only called once after the cortex has been powered on and    */
/*  not every time that the robot is disabled.                               */
/*---------------------------------------------------------------------------*/

/*********************************************************************/
/*********************************************************************/
/*********************************************************************/

/** ---------------------- 72832S include files ---------------------*/

/*********************************************************************/
/*********************************************************************/
/*********************************************************************/

#include "include/jpearman/SmartMotorLib.c"

#include "BNSLibrary/BNSLib.h"

#include "Vex_Competition_Includes.c"  // Main competition background code...do not modify!

#include "include/API.h"

/*********************************************************************/
/*********************************************************************/
/*********************************************************************/

/** ------------------------ 72832S pre auton -----------------------*/

/*********************************************************************/
/*********************************************************************/
/********************************************\*************************/

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

task autonomous(){
	/*
	pidReqVal=1050;
	startTask(pidPos);
	delayFunc(10000);
*/
    resetEncoders();

    clearLCD();

    delay(4);

	if(autonRun){
		//auton menu
	  setLCDPosition(0,0);
	  displayNextLCDString("Match Auton Run");
	  setLCDPosition(1,0);
	  if (val1 >= cutoffs[0] && val1 < cutoffs[1]) {
			displayNextLCDString("Red, Back");
			delay();
			startTask(intakeOnTask);
			driveForward(1.75);
			delay();
			driveBackward(0.2);
			delay();
			driveTurn90(true);
			delay();
			driveForward(1.2);
			delay();
			intakeOff();
	  } else if (val1 >= cutoffs[1] && val1 < cutoffs[2]) {
	    displayNextLCDString("Red, Front");
			punch();
			delay();
			driveBackward(0.1);
			delay();
			driveTurn90(false);
			delay();
			driveBackward(0.1);
			delay();
			driveForward(1.5);
			delay();
			startTask(intakeOnTask);
			delay();
			driveBackward(0.2);
			delay();
			driveTurn90(true);
			delay();
			driveForward(1.3);
			delay();
			intakeOff();
		}else if(val1 >= cutoffs[2] && val1 < cutoffs[3]){
	    displayNextLCDString("Blue, Front");
	    startTask(intakeOnTask);
			punch();
			delay();
			driveBackward(0.1);
			delay();
			driveTurn90(true);
			delay();
			driveBackward(0.1);
			delay();
			driveForward(1.5);
			delay();
			startTask(intakeOnTask);
			delay();
			driveBackward(0.2);
			delay();
			driveTurn90(true);
			delay();
			driveForward(1.3);
			delay();
			intakeOff();

		}else if (val1 >= cutoffs[3] && val1 < cutoffs[4]) {
	    delay();
	    displayNextLCDString("Blue, Back");
	    startTask(intakeOnTask);
	    delay();
			driveForward(1.75);
			delay();
			driveBackward(0.2);
			delay();
			driveTurn90(false);
			delay();
			driveForward(1.2);
			delay();
			intakeOff();
	  }
	}else if(skillsRun){
		setLCDPosition(0,0);
	  displayNextLCDString("Skills Auton Run");
	  setLCDPosition(1,0);

	  //SKILLS MVP(most valuable player) TEST #1

	  //SKILLS MVP(minimum viable product)
			punch();
			delay();
			delay();
			delay();
			delay();
			driveBackward(1.0);
			delay();
			delay();
			delay();
			delay();
			delay();
			driveTurn90(true);
			delay();
			delay();
			delay();
			delay();
			delay();
			driveBackward(0.2);
			delay();
			delay();
			delay();
			delay();
			delay();
			driveForward(3.2);

			//intake Off
			intakeOff();
	}else if(skillsRun==false && autonRun==false){
		setLCDPosition(0,0);
	  displayNextLCDString("Running Nothing");
	  setLCDPosition(1,0);
		delay(6);
	}
//	*/
}

/*********************************************************************/
/*********************************************************************/
/*********************************************************************/

/** ------------------- 72832S opcontrol functions ------------------*/

/*********************************************************************/
/*********************************************************************/
/*********************************************************************/

task usercontrol() {

	stopTask(autonomous);

	intakeOff();

	    // Initialize the Smart Motor Library
    SmartMotorsInit();

    // Define which motors are linked

    // Declare that you want the Library to keep a lid on the
    // motors by measuring the temperature of the PTC components
    SmartMotorPtcMonitorEnable();

    // Run smart motors
    SmartMotorRun();
    SmartMotorsInit();
    SmartMotorRun();

	string mainBattery, backupBattery;

    //display battery voltage
    lcdBattery();

    bLCDBacklight = true;                                    // Turn on LCD Backlight

    pidRunning=false;

    driveReverse=false;

    while (true) {
      opcontrol();
    }
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////