3796C FlowerPower STARSTRUCK#1.c

#pragma config(Sensor, dgtl1,  elevatorPosition, sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  elevatorDown,   sensorTouch)
#pragma config(Sensor, dgtl4,  scoopPosition,  sensorQuadEncoder)
#pragma config(Sensor, dgtl6,  scoopDown,      sensorTouch)
#pragma config(Motor,  port2,           rightMotor,    tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor,  port3,           leftMotor,     tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port6,           rightElevator, tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port7,           leftElevator,  tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port8,           rightScoop,    tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port9,           leftScoop,     tmotorServoContinuousRotation, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*----------------------------------------------------------------------------*/
/*         Description: "Starstruck" (2016-17)                                */
/*         Competition file for Team 3796C FLOWER POWER												*/
/*																																						*/
/*				This has four lines of code that need to be swapped for dual				*/
/*				contoller mode (reference functions for elevator and scoop					*/
/*				control.  Other features of this program include safety stops				*/
/*				in the down position for the elevator and scoop control. It is			*/
/*				designed to have two ranges for upward scoop movement based on			*/
/*				elevator position																										*/
/*																																						*/
/*----------------------------------------------------------------------------*/

// 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!
#include "Vex_Competition_Includes.c"

const int threshold = 25;				//joystick minimum movement to create motion
int Y, X = 0;										//"Y" holds forward/backward motor settings; "X" holds left and right motor settings
const int ElevTOP = 11000;						//Mechanical top of elevator travel
const int ElevMID = 6000;							//Elevator height which allows unrestricted scoop movement
const int ScoopTOP = 850;							//Mechanical top of scoop travel until ElevMID
const int ScoopLIFT = 1200;						//Mechanical top of scoop travel after ElevMID


	void drive_control ()					//control of two rear drive motors
	{
		if (abs(vexRT[Ch2]) > threshold)		// Ch2 is right joystick forward/backward motion
      Y = vexRT[Ch2];										//forward/backward only for sufficient joystick movement
    else
      Y = 0;
    if (abs(vexRT[Ch4]) > threshold)		// Ch4 is left joystick left/right motion
      X = vexRT[Ch4];										//left and right only for sufficient joystick movement
    else
      X = 0;

		motor[leftMotor]  = (Y + X)/2;
	  motor[rightMotor] = (Y - X)/2;
	}


	void elevator_control ()				//Elevator control
	{
		if (vexRT[Btn6UXmtr2] == 1 && abs(SensorValue(elevatorPosition)) < ElevTOP) //DUAL CONTROLLER COMMAND
		//if (vexRT[Btn6U] == 1 && abs(SensorValue(elevatorPosition)) < ElevTOP) // Elevator Up/Stop Up
	    {
	      motor[rightElevator] = -100;
	    	motor[leftElevator] = 100;
	    	wait1Msec(10);
	    }
	   else {
        motor[rightElevator] = 0;
	    	motor[leftElevator] = 0;
		}

   	if (vexRT[Btn6DXmtr2] == 1 && SensorValue (elevatorDown)!= 1) //DUAL CONTROLLER COMMAND
	  //if (vexRT[Btn6D] == 1 && SensorValue (elevatorDown)!= 1) // Elevator Down/Stop Down
	    {
	      motor[rightElevator] = 100;
	    	motor[leftElevator] = -100;
	    }
	   else {
        motor[rightElevator] = 0;
	    	motor[leftElevator] = 0;
			}
	}

	void scoop_control ()						//Control of robot arm scooping motion
	{
		if (abs(SensorValue(elevatorPosition))< ElevMID)
    {
      if (vexRT[Btn5UXmtr2] == 1 && abs(SensorValue(scoopPosition)) <ScoopTOP) //DUAL CONTROLLER COMMAND
		//if (vexRT[Btn5U] == 1 && abs(SensorValue(scoopPosition)) <ScoopTOP) //Scoop Up/Stop Up
			{
				motor[rightScoop] = 50;
				motor[leftScoop] = -50;
				wait1Msec(10);
			}
			else
			{
				motor[rightScoop] = 0;
				motor[leftScoop] = 0;
			}
    }
    else 		// This allows the scoop to travel upward beyond its normal range once the elevator is sufficiently high enough
    {
      if (vexRT[Btn5UXmtr2] == 1 && abs(SensorValue(scoopPosition)) <ScoopLIFT) //DUAL CONTROLLER COMMAND
		//if (vexRT[Btn5U] == 1 && abs(SensorValue(scoopPosition)) <ScoopLIFT) //Scoop Up/Stop Up
			{
				motor[rightScoop] = 50;
				motor[leftScoop] = -50;
				wait1Msec(10);
			}
			else
			{
				motor[rightScoop] = 0;
				motor[leftScoop] = 0;
			}
    }

			if (vexRT[Btn5DXmtr2] == 1 && SensorValue (scoopDown) != 1) //DUAL CONTROLLER COMMAND
			//if (vexRT[Btn5D] == 1 && SensorValue (scoopDown) != 1) // scoop down/stop down
	    	{
	    		motor[rightScoop] = -50;
	    		motor[leftScoop] = 50;
	    	}
	    	else
	    	{
					motor[rightScoop] = 0;
					motor[leftScoop] = 0;
				}
  	}


/*---------------------------------------------------------------------------*/
/*                          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.                               */
/*---------------------------------------------------------------------------*/

void pre_auton()
{
	while (SensorValue (elevatorDown)!= 1)	//Intialize elevator in down position and reset encoder for height accuracy
  {
    motor[rightElevator] = 75;
	  motor[leftElevator] = -75;
  }
  	motor[rightElevator] = 0;
	  motor[leftElevator] = -0;
  wait1Msec (500);
  SensorValue (elevatorPosition)= 0;  //reset of encoder in the down position


  while (SensorValue (scoopDown) != 1)	//Intialize scoop in down position and reset encoder for accuracy
  	{
  		motor[rightScoop] = -25;
  		motor[leftScoop] = 25;
  	}
  		motor[rightScoop] = -0;
  		motor[leftScoop] = 0;
  	wait1Msec (500);
  	SensorValue (scoopPosition) = 0;  //reset of scoop in the down position



  // 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;

	// Set bDisplayCompetitionStatusOnLcd to false if you don't want the LCD
	// used by the competition include file, for example, you might want
	// to display your team name on the LCD in this function.
	// bDisplayCompetitionStatusOnLcd = false;

  // All activities that occur before the competition starts
  // Example: clearing encoders, setting servo positions, ...
}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              Autonomous Task                              */
/*                                                                           */
/*  This task is used to control your robot during the autonomous phase of   */
/*  a VEX Competition.                                                       */
/*                                                                           */
/*  You must modify the code to add your own robot specific commands here.   */
/*---------------------------------------------------------------------------*/

task autonomous()
{
	  //for driving & elevator lift
		motor[leftMotor]  = -100;
	  motor[rightMotor] = -100;

	  motor[rightElevator] = -120;
  	motor[leftElevator] = 120;

	  wait1Msec(1500);
	  motor[leftMotor] = 0;
	  motor[rightMotor] = -0;
	  //for elevator

  	wait1Msec(3000);
  	motor[rightElevator] = 0;
  	motor[leftElevator] = -0;

  	//for scoop
  	motor[rightScoop] = 75;
		motor[leftScoop] = -75;
		wait1Msec(2000);
    motor[rightScoop] = 0;
		motor[leftScoop] = -0;

}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              User Control Task                            */
/*                                                                           */
/*  This task is used to control your robot during the user control phase of */
/*  a VEX Competition.                                                       */
/*                                                                           */
/*  You must modify the code to add your own robot specific commands here.   */
/*---------------------------------------------------------------------------*/

task usercontrol()
{
  // User control code here, inside the loop

  while (true)
  {
    drive_control();
    elevator_control();
    scoop_control();
  }
}