Skills.c

#pragma config(Motor,  port1,           Claw1,         tmotorVex393_HBridge, openLoop)
#pragma config(Motor,  port2,           DriveLeft1,    tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port3,           DriveLeft2,    tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port4,           DriveRight1,   tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port5,           DriveRight2,   tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port6,           Descorer1,     tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port7,           Descorer2,     tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port8,           Puncher1,      tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port9,           Puncher2,      tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port10,          Claw2,         tmotorVex393_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*---------------------------------------------------------------------------*/

/*                                                                        */

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

#include "Vex_Competition_Includes.c"
#define min(a, b) (a) < (b) ? (a) : (b)
#define max(a, b) (a) < (b) ? (b) : (a)

// Global variables

int con = -1; // Conventional direction


/*---------------------------------------------------------------------------*/

/*                        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 () {

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

  return;

}



// Global helper functions

// For stopping the drive
void StopDrive () {
  motor[DriveLeft1] = motor[DriveRight1] = motor[DriveRight2] = motor[DriveLeft2] = 0;
}

// For stopping the shooter
void StopShoot () {
  motor[Puncher1] = motor[Puncher2] = 0;
}

// For stopping the claw flipper
void StopDescorer () {
  motor[Descorer1] = motor[Descorer2] = 0;
}

// Drive forward
void DriveF (int amount, int time) {
  int tmp = amount;
  double backDev = 1;
  motor[DriveLeft1] = tmp * -con * backDev;
  motor[DriveRight1] = tmp * -con * backDev;
  motor[DriveRight2] = tmp * -con * backDev;
  motor[DriveLeft2] = tmp * -con * backDev;
  if (time != 0) wait1Msec(time);
}

// Drive backwards
void DriveB (int amount, int time) {
  int tmp = -amount;
  double backDev = 1;
  motor[DriveLeft1] = tmp * -con * backDev;
  motor[DriveRight1] = tmp * -con * backDev;
  motor[DriveRight2] = tmp * -con * backDev;
  motor[DriveLeft2] = tmp * -con * backDev;
  if (time != 0) wait1Msec(time);
}

// Turn right
void TurnR (int amount, int time) {
  motor[DriveLeft1] = amount * -con;
  motor[DriveRight1] = -amount * -con;
  motor[DriveRight2] = -amount * -con;
  motor[DriveLeft2] = amount * -con;
  if (time != 0) wait1Msec(time);
}

// Turn left
void TurnL (int amount, int time) {
  motor[DriveLeft1] = amount * con;
  motor[DriveRight1] = -amount * con;
  motor[DriveRight2] = -amount * con;
  motor[DriveLeft2] = amount * con;
  if (time != 0) wait1Msec(time);
}

void Turn90L () {
	int time = 314; // Change time as necessary
	// Can use sensors alternatively
  TurnL(127, time);
}

void Turn90R () {
	int time = 314; // Change time as necessary
	// Can use sensors alternatively
  TurnR(127, time);
}


void PuncherOn (int amount, int time) {
	motor[Puncher1] = motor[Puncher2] = amount * con;
	if (time != 0) wait1Msec(time);
}


void DescorerUp (int amount, int time) {
  motor[Descorer1] = motor[Descorer2] = amount * con;
  if (time != 0) wait1Msec(time);
}

void DescorerDown (int amount, int time) {
  motor[Descorer1] = motor[Descorer2] = amount * -con;
  if (time != 0) wait1Msec(time);

}

/*---------------------------------------------------------------------------*/

/*                                                                        */

/*                            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 () {
  // ..........................................................................
  // Insert user code here.
  // ..........................................................................
  // Planning on using sensors!
	
 
}

/*---------------------------------------------------------------------------*/

/*                                                                        */

/*                            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 (1) {
    // FORWARD AND BACKWARD
    int tmp = 0;
    double backDev = 1;
    if (vexRT[Ch2] > 20) {
      tmp = min(vexRT[Ch2], 127);
    }
    else if (vexRT[Ch2] < -20) {
      tmp = max(vexRT[Ch2], -127);
    }
    
    motor[DriveLeft1] = tmp * con*backDev;
    motor[DriveRight1] = tmp * con*backDev;
    motor[DriveRight2] = tmp * con*backDev;
    motor[DriveLeft2] = tmp * con*backDev;
    
    // Left axle turn
    if (vexRT[Btn5U]) {
      motor[DriveLeft1] = 127 * con;
      motor[DriveRight1] = -127 * con;
      motor[DriveRight2] = -127 * con;
      motor[DriveLeft2] = 127 * con;
    }
    // Right axle turn
    else if (vexRT[Btn6U]) {
      motor[DriveLeft1] = 127 * -con;
      motor[DriveRight1] = -127 * -con;
      motor[DriveRight2] = -127 * -con;
      motor[DriveLeft2] = 127 * -con;
    }

    // Turn left sensitive
    if (vexRT[Ch4] > 0) {
      motor[DriveLeft1] = 50 * con;
      motor[DriveRight1] = 0;
      motor[DriveRight2] = 0;
      motor[DriveLeft2] = 50 * con;
    }

    // Turn right sensitive
    else if (vexRT[Ch4] < 0) {
      motor[DriveLeft1] = 0;
      motor[DriveRight1] = -50 * -con;
      motor[DriveRight2] = -50 * -con;
      motor[DriveLeft2] = 0;
    }
  }
}