vexTurningPoint.c

vexTurningPoint.c

#pragma config(Sensor, in1,    gyro,           sensorGyro)
#pragma config(Sensor, dgtl1,  rightEncoder,   sensorQuadEncoder)
#pragma config(Sensor, dgtl6,  puncherLimit,   sensorTouch)
#pragma config(Sensor, dgtl11, leftEncoder,    sensorQuadEncoder)
#pragma config(Motor,  port1,           launcher1,     tmotorVex393_HBridge, openLoop, reversed)
#pragma config(Motor,  port2,           left1,         tmotorVex393HighSpeed_MC29, openLoop, driveLeft, encoderPort, dgtl11)
#pragma config(Motor,  port3,           left2,         tmotorVex393HighSpeed_MC29, openLoop, driveLeft)
#pragma config(Motor,  port4,           intake,        tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port5,           lift,          tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port6,           capIntake,     tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port8,           right2,        tmotorVex393HighSpeed_MC29, openLoop, reversed, driveRight)
#pragma config(Motor,  port9,           right1,        tmotorVex393HighSpeed_MC29, openLoop, reversed, driveRight, encoderPort, dgtl1)
#pragma config(Motor,  port10,          launcher2,     tmotorVex393_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

const float CIRC = 3.141592 * 4.155;

#pragma platform(VEX2)
#pragma competitionControl(Competition)
#include "Vex_Competition_Includes.c"
/*---------------------------------------------------------------------------*/
/*                          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, ...
}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              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.   */
/*---------------------------------------------------------------------------*/
#define abs(x) (x < 0 ? -x : x)

static const float kp = 10;

// AUTON STUFF
void move(int leftPower, int rightPower) {
	motor[left1] = leftPower;
	motor[left2] = leftPower;
	motor[right1] = rightPower;
	motor[right2] = rightPower;
}

void shoot() {
	while(SensorValue[puncherLimit] != 1) {
		motor[launcher1] = 70;
		motor[launcher2] = 70;
	}
	motor[launcher1] = 0;
	motor[launcher2] = 0;
}

float driveForward(int power, float distance) {

	SensorValue[leftEncoder] = 0;
  SensorValue[rightEncoder] = 0;

	int error = 0;
	int slavePower = power; // subtract based upon observations

	float ticksNeeded = (360 / CIRC) * distance;
	float totalTicks = 0.0;

	writeDebugStream("Need to move: %d", ticksNeeded);
	while(abs(totalTicks) < ticksNeeded) {
		totalTicks = (SensorValue[rightEncoder] + SensorValue[leftEncoder])/2;
		error = SensorValue[rightEncoder] - SensorValue[leftEncoder];
		slavePower += (int) (error / kp);
		move(slavePower, power);

		writeDebugStreamLine("Moved: %f", (totalTicks * CIRC)/360);
		wait1Msec(20);

 }
 	writeDebugStreamLine("Moved: %f", (totalTicks * CIRC)/360);
 	move(0, 0);
	return totalTicks;
}

task autonomous()
{
 	shoot();
	float distance = driveForward(70, 45);

}

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

// multiply by direction to go forwards or backwards
int direction = 1;

// DRIVER CONTROL
// Each of these "controls", controls a part of the robot
task driveControl ()
{
		while(true)
		{
			float distanceTraveledRight = (SensorValue[rightEncoder] * CIRC)/360;
	  	float distanceTraveledLeft = (SensorValue[leftEncoder] * CIRC)/360;
	  	//writeDebugStreamLine("traveled: %f", (distanceTraveledRight + distanceTraveledLeft)/2);
			motor[right1] = direction * vexRT[Ch3] - vexRT[Ch1];
			motor[right2] = direction * vexRT[Ch3] - vexRT[Ch1];
			motor[left1] = direction * vexRT[Ch3] + vexRT[Ch1];
			motor[left2] = direction * vexRT[Ch3]  + vexRT[Ch1];
		}
}


task puncherControl()
{
	while(true)
	{
		if(vexRT[Btn8U]) {
			motor[launcher1] = 85;
			motor[launcher2] = 85;
		}	else {
			motor[launcher1] = 0;
			motor[launcher2] = 0;
		}
	}
}

task ballIntakeControl()
{
	while(true) {
		if(vexRT[Btn8R]) {
			motor[intake] = 85;
		} else {
			motor[intake] = 0;
		}
	}
}

task liftControl()
{
	while(true) {
		if(vexRT[Btn6U]) {
			motor[lift] = 85;
		} else if(vexRT[Btn6D]) {
			motor[lift] = -85;
		} else {
			motor[lift] = 0;
		}
	}
}

task capIntakeControl()
{
	while(true) {
		if(vexRT[Btn5U]) {
			motor[capIntake] = 85;
		} else if (vexRT[Btn5D]) {
			motor[capIntake] = -85;	
		} else {
			motor[capIntake] = 0;
		}
	}
}

task usercontrol()
{
	SensorValue[rightEncoder] = 0;
	SensorValue[leftEncoder] = 0;
	startTask(driveControl);
	startTask(puncherControl);
	startTask(ballIntakeControl);
	startTask(liftControl);
	startTask(capIntakeControl);
  while (true)
  {
  	// Direction flip
 		 /*if(vexRT[Btn8U]) {
 		   //direction *= -1;
 		   wait1Msec(100);
  	}*/
  }

}