lineTest.c

#pragma config(Sensor, in1,    gyro,           sensorGyro)
#pragma config(Sensor, in2,    lineLeft,       sensorLineFollower)
#pragma config(Sensor, in3,    lineCenter,     sensorLineFollower)
#pragma config(Sensor, in4,    lineRight,      sensorLineFollower)
#pragma config(Sensor, dgtl1,  sonarOut,       sensorSONAR_raw)
#pragma config(Motor,  port1,           rightWheel1,   tmotorVex393_HBridge, openLoop)
#pragma config(Motor,  port2,           rightWheel2,   tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port3,           rightWheel3,   tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port4,           leftWheel2,    tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port5,           leftWheel3,    tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port6,           topShooter,    tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port7,           bottomShooter, tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port8,           intake,        tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port10,          leftWheel1,    tmotorVex393_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)

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

void pre_auton() {
	bStopTasksBetweenModes = true;

}

void spin (int velocity) {
	motor[leftWheel1] = -velocity;
	motor[leftWheel2] = -velocity;
	motor[leftWheel3] = -velocity;
	motor[rightWheel1] = velocity;
	motor[rightWheel2] = velocity;
	motor[rightWheel3] = velocity;

}
int error, target = 0;
task calculateError() {
	while(true){
		error = (SensorValue[gyro]-target)%4000;
		wait1Msec(50);
	}
}

void orient() {
	while(abs(SensorValue[gyro])>10) {
		if(SensorValue[gyro] > 50)
			spin(-50);
		else if(SensorValue[gyro] < -50)
			spin(50);
		else
			spin(SensorValue[gyro]/1270*127+20);
	}
	spin(0);
	playSound(soundBeepBeep);
}

task autonomous () {
	orient();
}

task usercontrol() {
	startTask(calculateError);
	while (true) {

		//tank drive
		//left wheels
		if(abs(vexRT(Ch3))<10) {
			motor[leftWheel1] = 0;
			motor[leftWheel2] = 0;
			motor[leftWheel3] = 0;
		} else {
			motor[leftWheel1] = vexRT(Ch3);
			motor[leftWheel2] = vexRT(Ch3);
			motor[leftWheel3] = vexRT(Ch3);
		}
		//left wheels
		if(abs(vexRT(Ch2))<10) {
			motor[rightWheel1] = 0;
			motor[rightWheel2] = 0;
			motor[rightWheel3] = 0;
		} else {
			motor[rightWheel1] = vexRT(Ch2);
			motor[rightWheel2] = vexRT(Ch2);
			motor[rightWheel3] = vexRT(Ch2);
		}

		if(vexRT(Btn5U))
			orient();

		if(vexRT(Btn6U))
			SensorValue[gyro] = 0;

		//Anywhere from 25-50 Msec pause
		wait1Msec(30);
	}
}