gyroTest.c

#pragma config(Sensor, in1,    gyro,           sensorGyro)
#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() {
	double Ki;
	double Kp;
	double Kd;

	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);
}
*/

float	Kp = 0.1;
float	Ki = 0.18;
float	Kd = 0.16;
task orient () {
	float error, integral, derivative, lastError;
	int speed;

	while (true) {
			//P
			error = 0 - SensorValue[gyro];

			//I
			if(error == 0)
				integral = 0;
			else if (abs(error)>10)
				integral = 0;
			else
				integral += error;

			//D
			derivative = error - lastError;

			lastError = error;


			speed = Kp*error+Ki*integral+Kd*derivative;
			if(speed>-5 && speed<5)
				speed = 0;
			else if(speed>127)
				speed = 127;
			else if(speed<-127)
				speed = -127;
			else if(speed<19 && speed>0)
				speed = 19;
			else if(speed>-19 && speed<0)
				speed = -19;
			spin(speed);
	}
}

task autonomous () {
	startTask(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))
			startTask(orient);
		else
			stopTask(orient);

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

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