task main() {
int speed;
int sonar_value;
int distance = 25;
while (true) {
sonar_value = SensorValue(sonarSensor);
displayLCDPos(0,0);
displayNextLCDString("Sonar: ");
displayNextLCDNumber(sonar_value);
if (sonar_value < 0) {
speed = 127;
} else {
speed = (sonar_value - distance)^2;
}
clearLCDLine(1);
displayLCDPos(1,0);
displayNextLCDString("Speed: ");
displayNextLCDNumber(speed);
motor[frontLeftMotor] = speed;
motor[frontRightMotor] = speed;
motor[backLeftMotor] = speed;
motor[backRightMotor] = speed;
wait1Msec(100);
}
}
void pre_auton()
{
bLCDBacklight = true; // Turn on LCD Backlight
bStopTasksBetweenModes = true;
bDisplayCompetitionStatusOnLcd = false;
clearLCDLine(0); // Clear line 1 (0) of the LCD
clearLCDLine(1); // Clear line 2 (1) of the LCD
string mainBattery, I2C_ErrorCode;
if(nImmediateBatteryLevel/1000.0>7 && getI2C == true)
{
displayLCDString(0, 0, "Systems: GREEN");
displayLCDString(1, 0, "Ready to Begin!");
}
else if(nImmediateBatteryLevel/1000.0>7 && getI2C == false)
{
displayLCDCenteredString(0, "I2C Fault!!!");
displayLCDCenteredString(1, "Error #: ");
sprintf(I2C_ErrorCode, "%1.2f%c", nI2CStatus);
displayNextLCDString(I2C_ErrorCode);
}
else if(getI2C == false && nImmediateBatteryLevel/1000.0<7)
{
displayLCDCenteredString(0, "I2C Fault!!!");
displayLCDCenteredString(1, "Battery Fault!!!");
}
else
{
displayLCDCenteredString(0, "REPLACE BATT!!!");
displayLCDString(1, 0, "Main V: ");
sprintf(mainBattery, "%1.2f%c", nImmediateBatteryLevel/1000.0,'V');
displayNextLCDString(mainBattery);
}
}
void pre_auton() {
bStopTasksBetweenModes = true;
bLCDBacklight = true;
displayLCDPos(0,0);
displayNextLCDString("program select");
redteam = true;
screenrefresh();
time1[T1] = 0;
while (programselecting == true) {
if (nLCDButtons & kButtonLeft) {
while (nLCDButtons & kButtonLeft) {
}
if (redteam == true) {
redteam = false;
}
else if (redteam == false) {
redteam = true;
}
screenrefresh();
}// end while
if (nLCDButtons & kButtonCenter) {
while (nLCDButtons & kButtonCenter)
{
}
programselect = programselect+1;
if (programselect > totalprogramnumber)
{
programselect = 1;
}
screenrefresh();
}
if (nLCDButtons & kButtonRight)
{
clearLCDLine(0);
clearLCDLine(1);
displayLCDPos(0,0);
displayNextLCDString("Robot ready");
wait1Msec(300);
displayNextLCDString(".");
wait1Msec(300);
displayNextLCDString(".");
wait1Msec(300);
displayNextLCDString(".");
wait1Msec(500);
bLCDBacklight = false;
programselecting = false;
}
}
}//end pre_auton
void LCD(){
{
clearLCDLine(0);
clearLCDLine(1); // Clear LCD Bottom Line
displayLCDPos(1,0); // Set the cursor to bottom line, first position
displayNextLCDString("Arm: "); // Print "Potentio: " starting at the cursor position
displayNextLCDNumber(SensorValue(ncoderarm)); // Display the reading of the Potentiometer to current cursor position
displayLCDPos(0,5);
displayNextLCDString("Touch: ");
displayNextLCDNumber(SensorValue(Touch));
}
}
void RobotReady() {
clearLCDLine(0);
clearLCDLine(1);
displayLCDPos(0,0);
displayNextLCDString("Robot ready");
wait1Msec(300);
displayNextLCDString(".");
wait1Msec(300);
displayNextLCDString(".");
wait1Msec(300);
displayNextLCDString(".");
wait1Msec(500);
bLCDBacklight = false;
}
task usercontrol()
{
bLCDBacklight = true;
startTask(speedControl);
while (true)
{
if(vexRT[Btn8U] == 1){
/*
autoDrive(-265, 265, 70, false);
wait1Msec(500);
autoDrive(1100, 1100, 70, true);
wait1Msec(500);
autoDrive(330, -330, 70, false);
wait1Msec(500);
autoDrive(-200, 200, 70, false);
wait1Msec(500);
autoDrive(1100, 1100, 70, true);
wait1Msec(500);
autoDrive(190, -190, 70, false);
*/
autoDrive(-210, 210, 70, false);
wait1Msec(500);
autoDrive(2100, 2120, 70, true);
wait1Msec(500);
autoDrive(260, -260, 70, false);
wait1Msec(500);
}
if(vexRT[Btn8D] == 1){
autoDrive(500, 500, 70, false);
autoDrive(-500, -500, 70, false);
}
//Display main battery
displayLCDString(0, 0, "Btry");
sprintf(mainBattery, "%1.2f%c", nImmediateBatteryLevel/1000.0,'V'); //Build the value to be displayed
displayNextLCDString(mainBattery);
//Display flywheel controls
clearLCDLine(1);
displayLCDString(1, 0, "F:");
sprintf(mainBattery2, "%d %d %d", target, SensorValue[leftEncoder], SensorValue[rightEncoder]); //Build the value to be displayed
displayNextLCDString(mainBattery2);
}
}
void screenrefresh()
{
clearLCDLine(1);
displayLCDPos(1,0);
if (redteam == true)
{
displayNextLCDString("RED ");
}
if (redteam == false)
{
displayNextLCDString("BlUE");
}
displayNextLCDString(" ");
displayNextLCDNumber(programselect);
displayNextLCDString(" OK");
}
task LCD() {
bLCDBacklight = true; // Turn on LCD Backlight
string mainBattery, backupBattery;
while(true) // An infinite loop to keep the program running until you terminate it
{
clearLCDLine(0); // Clear line 1 (0) of the LCD
clearLCDLine(1); // Clear line 2 (1) of the LCD
//Display the Primary Robot battery voltage
displayLCDString(0, 0, "Primary: ");
sprintf(mainBattery, "%1.2f%c", nImmediateBatteryLevel/1000.0,'V'); //Build the value to be displayed
displayNextLCDString(mainBattery);
//Display the Backup battery voltage
//displayLCDString(1, 0, "Backup: ");
//sprintf(backupBattery, "%1.2f%c", BackupBatteryLevel/1000.0, 'V'); //Build the value to be displayed
//displayNextLCDString(backupBattery);
//Sensor values
displayLCDNumber(1, 0, -SensorValue(TensionEncoder));
displayLCDNumber(1, 4, SensorValue(BowEncoder));
//Short delay for the LCD refresh rate
wait1Msec(100);
}
}
task main() {
clearLCDLine(0);
clearLCDLine(1);
displayLCDPos(0, 0);
displayNextLCDString("Titties");
nVolume = 4;
#ifndef NOSOUND
PlaySoundFile(SONGNAME);
#endif
resetVars(); // reset all variables
resetSensors(); // reset all sensors
#ifndef NOAUTON
AutoSelector();//run the RedFront autonomous
Autonomous();
#endif
AutoRedPost();
while (true) {
#ifndef NOSOUND
if (bSoundQueueAvailable) {
PlaySoundFile(SONGNAME);
}
#endif
RC(); // recieve inputs
calcMotorValues();
//writeStream();
//beltPower = 127;
RunRobot();
}
}
task main()
{
startTask(Scoop);
startTask(Shoot);
startTask(Drive);
clearLCDLine(0);
clearLCDLine(1);
while(true)
{
displayLCDPos(0, 0);
displayNextLCDString("Move Bitch");
displayLCDPos(1, 0);
displayNextLCDString("get out the way");
}
}
int initializeDisplay()
{
//clearLCDLine(0);
//clearLCDLine(1);
bLCDBacklight = true; //Turn Backlight on
displayLCDPos(1, 0); //Move "Cursor" to first spot
displayNextLCDString("Ready to PWN :-D"); //Display motivational message
return 0;
}
static void displayStatusAndTime()
{
displayLCDPos(1, 0);
if (bIfiRobotDisabled)
displayNextLCDString("Disable ");
else
{
if (bIfiAutonomousMode)
displayNextLCDString("Auton ");
else
displayNextLCDString("Driver ");
}
displayNextLCDNumber(nTimeXX / 600, 2);
displayNextLCDChar(':');
displayNextLCDNumber((nTimeXX / 10) % 60, -2);
displayNextLCDChar('.');
displayNextLCDNumber(nTimeXX % 10, 1);
}
void batteryVoltagle(int buttonPress) {
if(buttonPress == batteryButton) {
clearLCDLine(0); // Clear line 1 (0) of the LCD
clearLCDLine(1); // Clear line 2 (1) of the LCD
//Display the Primary Robot battery voltage
displayLCDString(0, 0, "Primary: ");
sprintf(mainBattery, "%1.2f%c", nImmediateBatteryLevel/1000.0,'V'); //Build the value to be displayed
displayNextLCDString(mainBattery);
if(nImmediateBatteryLevel < 6.2) {
backLightFlash();
}
//Display the Backup battery voltage
displayLCDString(1, 0, "Backup: ");
sprintf(backupBattery, "%1.2f%c", BackupBatteryLevel/1000.0, 'V'); //Build the value to be displayed
displayNextLCDString(backupBattery);
wait1Msec(100);
}
}
void driver(){
conInput();
chaDrive();
clearLCDLine(0);
clearLCDLine(1);
displayLCDPos(1,1);
displayNextLCDString("Cor");
displayLCDPos(1,5);
displayNextLCDString("Mid");
displayLCDPos(1,9);
displayNextLCDString("Cse");
SensorValue[red] = 1;
SensorValue[yellow] = 1;
SensorValue[green] = 1;
if(launchSpeedSet == corner){
displayLCDPos(1,0);
displayNextLCDString("*");
SensorValue[red] = 0;
}else
if(launchSpeedSet == middle){
displayLCDPos(1,4);
displayNextLCDString("*");
SensorValue[yellow] = 0;
}else
if(launchSpeedSet == close){
displayLCDPos(1,8);
displayNextLCDString("*");
SensorValue[green] = 0;
}else{
}
displayLCDPos(0,0);
displayNextLCDString("Motor Speed: ");
displayLCDPos(0,13);
string response = (int)launchSpeedSet;
displayNextLCDString(response);
displayLCDPos(1,13);
}
void batteryLCD(){//displays battery levels on LCD
if (time1[T1]%100 == 0){
switch(batteryLCDBool){
case true:
clearLCDLine(0);
displayLCDString(0, 0, "Primary: ");
if (nAvgBatteryLevel < 5500){
displayNextLCDString("Replace");
}
else if (nAvgBatteryLevel < 6500){
displayNextLCDString("Low");
}
else{
displayNextLCDString("Good");
}
break;
case false:
ExpanderBatteryLevel = SensorValue[ExpanderBattery] / 7;
clearLCDLine(0);
displayLCDString(0, 0, "Secondary: ");
if (ExpanderBatteryLevel < 550){
displayNextLCDString("Replace");
}
else if (ExpanderBatteryLevel < 650){
displayNextLCDString("Low");
}
else{
displayNextLCDString("Good");
}
break;
}
}
}
task main()
{
int nBiasValues[3];
int X_Accel;
int Y_Accel;
int Z_Accel;
clearLCDLine(0);
clearLCDLine(1);
bLCDBacklight = true;
displayLCDCenteredString(0, "Accel Test");
wait1Msec(500); // Bias values are being calculated
PlaySound(soundBeepBeep);
// Store the bias values in a variable so that they can be easily displayed in the ROBOTC debugger
// global variables window
nBiasValues[0] = SensorBias[xAxis];
nBiasValues[1] = SensorBias[yAxis];
nBiasValues[2] = SensorBias[zAxis];
while (true)
{
// Use the VEX LCD to display the accelerometer values
clearLCDLine(0);
clearLCDLine(1);
displayLCDPos(0, 0);
displayNextLCDString(".X.. ..Y.. ..Z..");
displayLCDPos(1, 0);
displayNextLCDNumber(SensorValue[xAxis], 4);
displayLCDPos(1, 5);
displayNextLCDNumber(SensorValue[yAxis], 5);
displayLCDPos(1, 12);
displayNextLCDNumber(SensorValue[zAxis], 4);
// Also store values in variables so that they can be easily displayed in "Globals" debugger window
X_Accel = SensorValue[xAxis];
Y_Accel = SensorValue[yAxis];
Z_Accel = SensorValue[zAxis];
wait1Msec(100);
}
}
task main()
{
// Master CPU will not let competition start until powered on for at least 2-seconds
clearLCDLine(0);
clearLCDLine(1);
displayLCDPos(0, 0);
displayNextLCDString("Startup");
wait1Msec(2000);
while (true)
{
clearLCDLine(0);
clearLCDLine(1);
displayLCDPos(0, 0);
while (bIfiRobotDisabled)
{
if (JustDisabled == true){
startTask(LCD);
SensorValue(BaseLock) = 0;
SensorType[ in2 ] = sensorNone;
wait1Msec(1000);
SensorType[ in2 ] = sensorGyro;
wait1Msec(2000);
}
else {
wait1Msec(25);
}
JustDisabled = false;
}
JustDisabled = true;
nTimeXX = 0;
clearLCDLine(0);
clearLCDLine(1);
displayLCDPos(0, 0);
if (bIfiAutonomousMode)
{
displayNextLCDString("Autonomous");
startTask(autonomous);
// Waiting for autonomous phase to end
while (bIfiAutonomousMode && !bIfiRobotDisabled)
{
if (!bVEXNETActive)
{
if (nVexRCReceiveState == vrNoXmiters) // the transmitters are powered off!!
allMotorsOff();
}
wait1Msec(25); // Waiting for autonomous phase to end
}
allMotorsOff();
if(bStopTasksBetweenModes)
{
allTasksStop();
}
}
else
{
displayNextLCDString("User Control");
startTask(usercontrol);
// Here we repeat loop waiting for user control to end and (optionally) start
// of a new competition run
while (!bIfiAutonomousMode && !bIfiRobotDisabled)
{
if (nVexRCReceiveState == vrNoXmiters) // the transmitters are powered off!!
allMotorsOff();
wait1Msec(25);
}
allMotorsOff();
if(bStopTasksBetweenModes)
{
allTasksStop();
}
}
}
}
task usercontrol()
{
while (true)
{
float x1 = vexRT[Ch4];
float y1 = vexRT[Ch3];
float x2 = vexRT[Ch1];
float y2 = vexRT[Ch2];
//Left Joystick Arcade Drive//
motor [left] = y1 + x1; //
motor [right] = y2 - x2; //
//////////////////////////////
//Arm Control/////////////////
if (vexRT[Btn6U] == 1) //
{ //
arm(127); //
} //
else if (vexRT[Btn6D] == 1 )//
{ //
arm(-127); //
} //
else //
{ //
arm(0); //
} //
//////////////////////////////
//Grabber Control/////////////
if (vexRT[Btn8U] == 1) //
{ //
grab(127); //
} //
else if (vexRT[Btn8D] == 1) //
{ //
grab(-127); //
} //
else //
{ //
grab(0); //
} //
//////////////////////////////
//LCD Display/////////////////
string mainBattery, PEBattery;
clearLCDLine(0); // Clear line 1 (0) of the LCD
clearLCDLine(1); // Clear line 2 (1) of the LCD
//Display the Primary Robot battery voltage
displayLCDString(0, 0, "Primary: ");
sprintf(mainBattery, "%1.2f%c", nImmediateBatteryLevel/1000.0,'V');
displayNextLCDString(mainBattery);
//Display the Backup battery voltage
displayLCDString(1, 0, "Count: ");
sprintf(PEBattery, "%1.2f%c", BackupBatteryLevel/1000.0, 'V');
displayNextLCDString(PEBattery);
//Deadband////////////////////
int deadBand = 10;
if(x1 > deadBand || x1 < -deadBand)
{
x1 = x1;
}
else
{
x1 = 0;
}
if(y1 > deadBand || y1 < -deadBand)
{
y1 = y1;
}
else
{
y1 = 0;
}
if(x2 > deadBand || x2 < -deadBand)
{
x2 = x2;
}
else
{
x2 = 0;
}
if(y2 > deadBand || y2 < -deadBand)
{
y2 = y2;
}
else
{
y2 = 0;
}
}
}
task usercontrol()
{
// User control code here, inside the loop
int potThreshold = 1616;
int flipperVal = 0;
//clearDebugStream;
//ClearTimer(T1);
wait1Msec(2000); // wait 2 seconds before exectuing following code
bMotorReflected[port2] = true;
while (true)
{
// This is the main execution loop for the user control program. Each time through the loop
// your program should update motor + servo values based on feedback from the joysticks.
// .....................................................................................
// Insert user code here. This is where you use the joystick values to update your motors, etc.
// .....................................................................................
int ch2 = vexRT[Ch2];
int ch3 = vexRT[Ch3];
int secondch2 = vexRT[Ch2Xmtr2];
int secondch3 = vexRT[Ch3Xmtr2];
int pot = SensorValue[potentiometer];
//motor[motor1] = motor[motor2] = motor[motor3] = motor[motor10] = ch2;
// Flipper on Controller 2
/*motor[flip1] = secondch2;
motor[flip2] = secondch2;
motor[flip3] = secondch2;
motor[flip4] = secondch2;*/
//Flipper button control
if(vexRT[Btn6UXmtr2])
{
motor[servo] = -125;
wait1Msec(500);
motor[servo] = 95;
}
motor[flip1] = flipperVal;
motor[flip2] = flipperVal;
motor[flip3] = flipperVal;
motor[flip4] = flipperVal;
if(vexRT[Btn8DXmtr2] && pot < potThreshold) {
flipperVal = -127;
} else if (vexRT[Btn8UXmtr2]) {
flipperVal = 127;
}else if (pot > potThreshold) {
flipperVal = -20;
} else {
flipperVal = 0;
}
// doesn't work yet
/*if(vexRT[Btn8DXmtr2] && pot < potThreshold) {
flipperVal = -127;
} else if (vexRT[Btn8DXmtr2] && pot > potThreshold) {
flipperVal = -20;
} else if (pot > potThreshold && !vexRT[Btn8DXmtr2]) {
if (pot > 750) {
flipperVal = 127;
} else {
flipperVal = 0;
}}*/
// Drive On Controller 1
motor[backLeft] = ch3;
motor[frontLeft] = ch3;
motor[backRight] = ch2;
motor[frontRight] = ch2;
if(vexRT[Btn7UXmtr2])
{
motor[intake] = -127;
}
if(vexRT(Btn7DXmtr2))
{
motor[intake] = 0;
}
if(vexRT[Btn5U]) {
motor[backLeft] = 127;
motor[frontLeft] = -127;
motor[backRight] = -127;
motor[frontRight] = 127;
}
if(vexRT[Btn6U]) {
motor[backLeft] = -127;
motor[frontLeft] = 127;
motor[backRight] = 127;
motor[frontRight] = -127;
}
//int pot = SensorValue[potentiometer];
//sfoo = time100[T1]/10;
writeDebugStreamLine("The potentiometer is reading %d", SensorValue[potentiometer]);
//int shoot vexRT[Btn8D];
clearLCDLine(0); // clear the top VEX LCD line
clearLCDLine(1); // clear the bottom VEX LCD line
setLCDPosition(0,0); // set the VEX LCD cursor the first line, first space
displayNextLCDString("Potentiometer:"); // display "Potentiometer:" on the top line
setLCDPosition(1,0); // set the VEX LCD cursor the second line, first space
displayNextLCDNumber(SensorValue(potentiometer));
}
}
void LCDER(string here) {
clearLCDLine(1);
bLCDBacklight = true;
displayLCDPos(1,11);
displayNextLCDString(here);
}
void displayBattLevel(){
displayLCDString(0, 0, "Battery Level: ");
displayLCDNumber(1, 0, (nImmediateBatteryLevel - 3000) / 72);
displayNextLCDString("%");
}
task usercontrol()
{
// User control code here, inside the loop
int intakingState = -1, feedState = -1, ballFeedTime = 20;
int oldL[10] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
int oldR[10] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
int sumR = 0, sumL = 0;
bool intaking = false, inc = false, dec = false, feeding = false, ballThere1 = false, ballThere2 = false;
bLCDBacklight = true;
//Mapping
int driveMap[128] = {0,0,0,0,0,0,0,0,0,0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
22,23,24,25,26,27,28,28,29,29,
30,30,31,31,32,32,33,33,34,34,
35,35,36,36,37,37,38,38,39,39,
40,40,41,41,42,42,43,43,44,44,
45,45,46,46,47,47,48,48,49,49,
50,50,51,51,52,52,53,53,54,54,
55,55,56,56,57,57,58,58,59,59,
60,60,61,62,63,64,65,66,67,68,
69,70,71,72,73,74,75,76,77,78,
79,80,81,82,83,84,85,86,87,88,
89,90,91,92,94,96,127,127};
//Start speed control task
startTask(speedControl);
while (true)
{
// This is the main execution loop for the user control program. Each time through the loop
// your program should update motor + servo values based on feedback from the joysticks.
// .....................................................................................
// Insert user code here. This is where you use the joystick values to update your motors, etc.
// .....................................................................................
//Intake state control
if(vexRT[Btn5U] == 1){
intaking = true;
}else if(vexRT[Btn5U] == 0 && intaking == true){
intaking = false;
intakingState *= -1;
}
//Intake control
if(vexRT[Btn5D] == 1)
motor[intake] = -127;
else if(intakingState == 1)
motor[intake] = 127;
else
motor[intake] = 0;
//Feed state control
if(vexRT[Btn8R] == 1){
feeding = true;
}else if(vexRT[Btn8R] == 0 && feeding == true){
feeding = false;
feedState *= -1;
}
//Feed control
if(feedState == -1){
if(vexRT[Btn6U] == 1)
motor[feed] = 127;
else if(vexRT[Btn6D] == 1)
motor[feed] = -127;
else{
//Limit controls
if(sensorvalue[bottomLimit] == 1 && sensorvalue[middleLimit] == 0 && sensorvalue[topLimit] == 0)
ballThere1 = true;
else if(ballThere1 && sensorvalue[middleLimit] == 1)
ballThere1 = false;
if(sensorvalue[bottomLimit] == 1 && sensorvalue[middleLimit] == 1 && sensorvalue[topLimit] == 0)
ballThere2 = true;
else if(ballThere2 && sensorvalue[topLimit] == 1)
ballThere2 = false;
if(ballThere2 || ballThere1)
motor[feed] = 80;
else
motor[feed] = 0;
}
}else if(feedState == 1){
if(speed >= speeds[target])
motor[feed] = 127;
else
motor[feed] = 0;
}
//Target speed control
if(vexRT[Btn8U] == 1){
inc = true;
}else if(vexRT[Btn8U] == 0 && inc == true){
inc = false;
if(target < 15)
target += 1;
}
if(vexRT[Btn8D] == 1){
dec = true;
}else if(vexRT[Btn8D] == 0 && dec == true){
dec = false;
if(target > 0)
target -= 1;
}
//Quick target control
if(vexRT[Btn7L] == 1)
target = 5;
else if(vexRT[Btn7U] == 1)
target = 8;
else if(vexRT[Btn7R] == 1)
target = 12;
else if(vexRT[Btn7D] == 1)
target = 4;
//Safety stop
if(vexRT[Btn8L] == 1){
target = 0;
signal = 0;
}
for(int i = 0; i < 9; i++){
oldL[i] = oldL[i+1];
oldR[i] = oldR[i+1];
}
oldL[9] = driveMap[abs(vexRT[Ch3])] * sgn(vexRT[Ch3]);
oldR[9] = driveMap[abs(vexRT[Ch2])] * sgn(vexRT[Ch2]);
sumL = oldL[0] + oldL[1] + oldL[2] + oldL[3] + oldL[4] + oldL[5] + oldL[6] + oldL[7] + oldL[8] + oldL[9];
sumR = oldR[0] + oldR[1] + oldR[2] + oldR[3] + oldR[4] + oldR[5] + oldR[6] + oldR[7] + oldR[8] + oldR[9];
//Drive motor controls
motor[driveL] = sumL / 10;
motor[driveLF] = sumL / 10;
motor[driveRF] = sumR / 10;
motor[driveR] = sumR / 10;
//Display main battery
displayLCDString(0, 0, "Btry");
sprintf(mainBattery, "%1.2f%c", nImmediateBatteryLevel/1000.0,'V'); //Build the value to be displayed
displayNextLCDString(mainBattery);
//Display flywheel controls
clearLCDLine(1);
displayLCDString(1, 0, "F:");
sprintf(mainBattery2, "%d %d %d", target, signal, speed); //Build the value to be displayed
displayNextLCDString(mainBattery2);
wait1Msec(10);
}
}
void pre_auton(){
bLCDBacklight = true; // Turn on LCD Backlight
clearLCDLine(0);
clearLCDLine(1);
//Display the Primary Robot battery voltage
displayLCDString(0, 0, "Primary: ");
sprintf(mainBattery, "%1.2f%c", nImmediateBatteryLevel/1000.0, 'V'); //Build the value to be displayed
displayNextLCDString(mainBattery);
//Display the Backup battery voltage
displayLCDString(1, 0, "Backup: ");
sprintf(backupBattery, "%1.2f%c", BackupBatteryLevel/1000.0, 'V'); //Build the value to be displayed
displayNextLCDString(backupBattery);
//Short delay for the LCD refresh rate
waitForPress();
// Clear LCD
clearLCDLine(0);
clearLCDLine(1);
wait1Msec(500);
//Declare count variable to keep track of our choice
int count = 0;
while(nLCDButtons != centerButton)
{
//Switch case that allows the user to choose from 5 different options
switch(count){
case 0: //Display first choice
displayLCDCenteredString(0, "Goal Auto red");
displayLCDCenteredString(1, "< Enter >");
waitForPress();
//Increment or decrement "count" based on button press
if(nLCDButtons == leftButton)
{
waitForRelease();
count--;
}
else if(nLCDButtons == rightButton)
{
waitForRelease();
count++;
}
break;
case 1: //display 2nd choice
displayLCDCenteredString(0, "Goal Auto Blue");
displayLCDCenteredString(1, "< Enter >");
if(nLCDButtons == leftButton) {
waitForRelease();
count--;
}
else if(nLCDButtons == rightButton) {
waitForRelease();
count++;
}
break;
default:
count = 0;
break;
}
switch(count){
case 2: //Display 3rd choice
displayLCDCenteredString(0, " Blue Hang auto");
displayLCDCenteredString(1, "< Enter >");
waitForPress();
//Increment or decrement "count" based on button press
if(nLCDButtons == leftButton) {
waitForRelease();
count--;
}
else if(nLCDButtons == rightButton) {
waitForRelease();
count++;
}
break;
case 3: //display 4th choice
displayLCDCenteredString(0, "Red Hang auto");
displayLCDCenteredString(1, "< Enter >");
if(nLCDButtons == leftButton) {
waitForRelease();
count--;
}
else if(nLCDButtons == rightButton) {
waitForRelease();
count++;
}
break;
default:
count = 0;
break;
case 4: //display 5th choice
displayLCDCenteredString(0, "Skills Challenge");
displayLCDCenteredString(1, "< Enter >");
if(nLCDButtons == leftButton) {
waitForRelease();
count--;
}
else if(nLCDButtons == rightButton) {
waitForRelease();
count++;
}
break;
default:
count = 0;
break;
}
//Clear LCD
clearLCDLine(0);
clearLCDLine(1);
switch(count) { //pick auto, display
case 0: //Choice 1 from LCD
displayLCDCenteredString(0, "Red Goal Auto");
displayLCDCenteredString(1, "is running!");
wait1Msec(1000);
programChoice = 1;
break;
case 1: //Choice 2 from LCD
displayLCDCenteredString(0, "Blue Goal Auto");
displayLCDCenteredString(1, "is running!");
wait1Msec(1000);
programChoice = 2;
break;
case 2: //Choice 2 from LCD
displayLCDCenteredString(0, "Blue Hang Auto");
displayLCDCenteredString(1, "is running!");
wait1Msec(1000);
programChoice = 3;
break;
case 3: //Choice 3 from LCD
displayLCDCenteredString(0, "Red Hang Auto");
displayLCDCenteredString(1, "is running!");
wait1Msec(1000);
programChoice = 4;
break;
case 4: //Choice 3 from LCD
displayLCDCenteredString(0, "Skills Challenge");
displayLCDCenteredString(1, "is running!");
wait1Msec(1000);
programChoice = 5;
break;
default:
displayLCDCenteredString(0, "No valid choice");
displayLCDCenteredString(1, "was made!");
wait1Msec(1000);
break;
}
}
}
////// Display stuff on the LCD //////
task LCD() {
bool ButtonPressed = false;
string AutoName = "Nothing";
bLCDBacklight = true; // Turn on LCD Backlight
string mainBattery, backupBattery;
while(true) // An infinite loop to keep the program running until you terminate it
{
clearLCDLine(0); // Clear line 1 (0) of the LCD
clearLCDLine(1); // Clear line 2 (1) of the LCD
displayLCDPos(0, 0);
//Display the Primary Robot battery voltage
displayLCDString(0, 0, "Primary: ");
sprintf(mainBattery, "%1.2f%c", nImmediateBatteryLevel/1000.0,'V'); //Build the value to be displayed
displayNextLCDString(mainBattery);
if (bIfiAutonomousMode){
displayLCDString(1, 0, "AUTO:");
displayLCDString(1, 6, AutoName);
}
else {
//displayLCDNumber(1, 0, Heading);
}
if (nLCDButtons == 2){
if (AutoSelect > 0 && ButtonPressed == false){
AutoSelect--;
}
ButtonPressed = true;
}
else if (nLCDButtons == 4){
if (AutoSelect < 9 && ButtonPressed == false){
AutoSelect++;
}
ButtonPressed = true;
}
else {
ButtonPressed = false;
}
switch(AutoSelect)
{
// SKILLS
case 1:
AutoName = "SKILLS";
break;
// AUTO'S
case 2:
AutoName = "BALL AUTO";
break;
//TEST CODE
case 9:
AutoName = "TEST";
break;
default:
AutoName = "NOTHING";
}
}
//Short delay for the LCD refresh rate
wait1Msec(100);
}
task main()
{
// Master CPU will not let competition start until powered on for at least 2-seconds
clearLCDLine(0);
clearLCDLine(1);
displayLCDPos(0, 0);
displayNextLCDString("Startup");
wait1Msec(2000);
pre_auton();
//wait1Msec(500);
while (true)
{
clearLCDLine(0);
clearLCDLine(1);
displayLCDPos(0, 0);
while (bSimulatedRobotDisabled)
{
displayLCDPos(0, 0);
displayNextLCDString("Disabled");
nTimeXX = 0;
while (true)
{
displayStatusAndTime();
if (!bSimulatedRobotDisabled)
break;
wait1Msec(25);
displayStatusAndTime();
if (!bSimulatedRobotDisabled)
break;
wait1Msec(25);
displayStatusAndTime();
if (!bSimulatedRobotDisabled)
break;
wait1Msec(25);
displayStatusAndTime();
if (!bSimulatedRobotDisabled)
break;
wait1Msec(25);
++nTimeXX;
}
}
nTimeXX = 0;
clearLCDLine(0);
clearLCDLine(1);
displayLCDPos(0, 0);
if (bSimulatedAutonomousMode)
{
displayNextLCDString("Autonomous");
StartTask(autonomous);
// Waiting for autonomous phase to end
while (bSimulatedAutonomousMode && !bSimulatedRobotDisabled)
{
if (!bVEXNETActive)
{
if (nSimulatedRecieveState == vrNoXmiters) // the transmitters are powered off!!
allMotorsOff();
}
wait1Msec(25); // Waiting for autonomous phase to end
}
allMotorsOff();
allTasksStop();
}
else
{
displayNextLCDString("User Control");
StartTask(usercontrol);
// Here we repeat loop waiting for user control to end and (optionally) start
// of a new competition run
while (!bSimulatedAutonomousMode && !bSimulatedRobotDisabled)
{
if (nSimulatedRecieveState == vrNoXmiters) // the transmitters are powered off!!
allMotorsOff();
wait1Msec(25);
}
allMotorsOff();
allTasksStop();
}
}
}
////// Display stuff on the LCD //////
task LCD() {
bool ButtonPressed = false;
string AutoName = "Nothing";
bLCDBacklight = true; // Turn on LCD Backlight
string mainBattery, backupBattery;
while(true) // An infinite loop to keep the program running until you terminate it
{
clearLCDLine(0); // Clear line 1 (0) of the LCD
clearLCDLine(1); // Clear line 2 (1) of the LCD
//Display the Primary Robot battery voltage
displayLCDString(0, 0, "Primary: ");
sprintf(mainBattery, "%1.2f%c", nImmediateBatteryLevel/1000.0,'V'); //Build the value to be displayed
displayNextLCDString(mainBattery);
if (bIfiAutonomousMode){
displayLCDString(1, 0, "AUTO:");
displayLCDString(1, 6, AutoName);
}
else{
//displayLCDNumber(1, 0, SensorValue(TurnGyro));
//displayLCDNumber(1, 0, SensorValue(BowEncoder));
//displayLCDNumber(1, 0, SensorValue(RightEncoder));
//displayLCDNumber(1, 0, SensorValue(BowPot));
displayLCDNumber(1, 0, abs(SensorValue(TensionEncoder)));
displayLCDNumber(1, 4, AutoSelect);
displayLCDString(1, 6, AutoName);
if (nLCDButtons == 1){
wait1Msec(2000);
if (nLCDButtons == 1){
SensorValue(TensionEncoder) = 0;
}
}
else if (nLCDButtons == 2){
if (AutoSelect > 0 && ButtonPressed == false){
AutoSelect--;
}
ButtonPressed = true;
}
else if (nLCDButtons == 4){
if (AutoSelect < 20 && ButtonPressed == false){
AutoSelect++;
}
ButtonPressed = true;
}
else {
ButtonPressed = false;
}
switch(AutoSelect)
{
// SKILLS
case 1:
AutoName = "SKILLS";
break;
// AUTO'S
case 2:
AutoName = "BALL AUTO";
break;
case 3:
AutoName = "Park AUTO";
break;
case 4:
AutoName = "WalloB";
break;
case 5:
AutoName = "WalloR";
break;
//TEST CODE
case 17:
AutoName = "PID TURN";
break;
case 18:
AutoName = "PID DRIVE";
break;
case 19:
AutoName = "LEFT Test";
break;
case 20:
AutoName = "RIGHT TEST";
break;
default:
AutoName = "NOTHING";
}
}
//Short delay for the LCD refresh rate
wait1Msec(100);
}
}
//Auton
task autonomous()
{
if(SensorValue[autonSelector] > 1000){
bLCDBacklight = true;
//First shots
startTask(speedControl);
target = 8;
clearTimer(T1);
motor[intake] = 127;
while(time1[T1] < 22000){
if(speed >= speeds[target]-2){
motor[feed] = 127;
}
else{
motor[feed] = 0;
}
}
motor[feed] = 0;
motor[intake] = 0;
target = 6;
wait1Msec(250);
target = 5;
wait1Msec(250);
target = 4;
wait1Msec(250);
target = 3;
wait1Msec(250);
target = 2;
wait1Msec(250);
target = 1;
wait1Msec(250);
target = 0;
wait1Msec(250);
stopTask(speedControl);
//Drive across
autoDrive(-210, 210, 70, false);
wait1Msec(300);
autoDrive(2100, 2100, 70, true);
wait1Msec(300);
autoDrive(250, -250, 70, false);
wait1Msec(300);
autoDrive(100, 100, 70, false);
wait1Msec(100);
//Second shots
startTask(speedControl);
target = 8;
clearTimer(T1);
motor[intake] = 127;
while(time1[T1] < 22000){
if(speed >= speeds[target]-2){
motor[feed] = 127;
}
else{
motor[feed] = 0;
}
}
motor[feed] = 0;
motor[intake] = 0;
target = 6;
wait1Msec(250);
target = 5;
wait1Msec(250);
target = 4;
wait1Msec(250);
target = 3;
wait1Msec(250);
target = 2;
wait1Msec(250);
target = 1;
wait1Msec(250);
target = 0;
wait1Msec(250);
stopTask(speedControl);
//Display flywheel controls
clearLCDLine(0);
displayLCDString(0, 0, "EL:");
sprintf(mainBattery, "%d RL:%d", 0, SensorValue[leftEncoder]); //Build the value to be displayed
displayNextLCDString(mainBattery);
//Display flywheel controls
clearLCDLine(1);
displayLCDString(1, 0, "ER:");
sprintf(mainBattery2, "%d RR:%d", 0, SensorValue[rightEncoder]); //Build the value to be displayed
displayNextLCDString(mainBattery2);
//Wait
wait1Msec(2000);
}
else{
//First shots
startTask(speedControl);
target = 12;
clearTimer(T1);
motor[intake] = 127;
while(time1[T1] < 9000){
if(speed >= speeds[target]){
motor[feed] = 127;
}
else{
motor[feed] = 0;
}
}
motor[feed] = 0;
motor[intake] = 0;
target = 6;
wait1Msec(250);
target = 5;
wait1Msec(250);
target = 4;
wait1Msec(250);
target = 3;
wait1Msec(250);
target = 2;
wait1Msec(250);
target = 1;
wait1Msec(250);
target = 0;
wait1Msec(250);
stopTask(speedControl);
}
}
task main()
{
int i;
const int kNumbOfMainSamples = 4000;
bLCDBacklight = true;
displayLCDPos(0, 0);
displayNextLCDString("Gyro Noise Test ");
memset(nSampleHisogram[0], 0, sizeof(nSampleHisogram));
wait1Msec(1000);
// Calculate a rough mezsure of 'bias' sum that fits in 16-bit signed.
nSampleSum = 0;
const int kRoughBiasCounts = 30;
for (i = 0; i < kRoughBiasCounts; ++i)
{
nSampleSum += SensorValue[gyro];
wait1Msec(1);
}
nBiasRough = nSampleSum / kRoughBiasCounts;
nSampleSum = 0;
for (i = 0; i < kNumbOfMainSamples; ++i)
{
nOffset = SensorValue[gyro] - nBiasRough;
nSampleSum += nOffset;
wait1Msec(1);
}
nBias = nBiasRough + nSampleSum / kNumbOfMainSamples;
nBiasSmall = nSampleSum / (kNumbOfMainSamples / 100) - (nSampleSum / kNumbOfMainSamples) * 100;
nBiasSmaller = nSampleSum / (kNumbOfMainSamples / 1000) - nBiasSmall * 10;
StartTask(testMotorNoise);
nLastCycles = -1;
bool bDisplayIt;
bool bOverflow = false;
for (nCycles = 0; true; ++nCycles)
{
//ASSERT(nCycles == (nLastCycles + 1));
nLastCycles = nCycles;
nTimeStamp = nPgmTime;
nOffset = SensorValue[gyro] - nBias;
nDrift += nOffset;
if (nCycles >= 0)
{
if ((nCycles % 100) == 50)
nDrift -= nBiasSmall;
if ((nCycles % 1000) == 500)
{
nDrift -= nBiasSmaller;
bDisplayIt = true;
}
else
bDisplayIt = false;
}
else
{
if ((nCycles % 1000) == -50)
nDrift -= nBiasSmall;
if ((nCycles % 1000) == -500)
{
nDrift -= nBiasSmaller;
bDisplayIt = true;
}
else
bDisplayIt = false;
}
if (nOffset < -32)
nOffset = -32;
else if (nOffset > 32)
nOffset = 32;
nCount = nSampleHisogram[nOffset + 32];
if (nCount >= 32767)
bOverflow = true;
if (!bOverflow)
++nSampleHisogram[nOffset + 32];
if (true)
{
nPlusMinusWeighted += nOffset;
if (nOffset < 0)
--nPlusMinus;
else if (nOffset > 0)
++nPlusMinus;
}
if (bDisplayIt)
{
++nSeconds;
displayLCDPos(1, 0);
displayNextLCDNumber(nSeconds, 3);
displayNextLCDString(" : ");
if (nDrift > 0)
{
displayNextLCDNumber(nDrift / 1000);
displayNextLCDChar('.');
displayNextLCDNumber(nDrift % 1000, -3);
}
else
{
displayNextLCDChar('-');
displayNextLCDNumber(- nDrift / 1000);
displayNextLCDChar('.');
displayNextLCDNumber(- nDrift % 1000, -3);
}
displayNextLCDChar(' ');
displayNextLCDChar(' ');
}
while (nTimeStamp == nPgmTime)
{}
}
StopTask(testMotorNoise);
}
task usercontrol()
{
while (true)
{
motor[rightfrontdrive]=vexRT[Ch2];
motor[rightmiddledrive]=vexRT[Ch2];
motor[rightreardrive]=vexRT[Ch2];
motor[leftfrontdrive]=vexRT[Ch3];
motor[leftmiddledrive]=vexRT[Ch3];
motor[leftreardrive]=vexRT[Ch3];
if (1 == 1)
{
clearLCDLine(0); // Clear line 1 (0) of the LCD
clearLCDLine(1); // Clear line 2 (1) of the LCD
//Display the Primary Robot battery voltage
displayLCDString(0, 0, "Primary: ");
sprintf(mainBattery, "%1.2f%c", nImmediateBatteryLevel/1000.0,'V'); //Build the value to be displayed
displayNextLCDString(mainBattery);
displayLCDPos(1,0);
displayNextLCDNumber(SensorValue(pot));
//Short delay for the LCD refresh rate
wait1Msec(100);
}
if(vexRT[Btn5U]==1)
{
if(SensorValue(pot) < 1700){
motor[leftlift] = 127;
motor[rightlift] = 127;
}
else if(SensorValue(pot) > 1700){
motor[leftlift] = 70;
motor[rightlift] = 70;
}
}
else if(vexRT[Btn5D]==1)
{
if(SensorValue(pot) < 1700){
motor[leftlift] = -127;
motor[rightlift] = -127;
}
else if(SensorValue(pot) > 1700){
motor[leftlift] = -70;
motor[rightlift] = -70;
}
}
else
{
motor[leftlift] = 10;
motor[rightlift] = 10;
}
if (vexRT[Btn6U]==1)
{
motor[leftintake]=-127;
motor[rightintake]=-127;
}
else if (vexRT[Btn6D]==1)
{
motor[leftintake]=127;
motor[rightintake]=127;
}
else
{
motor[leftintake]=0;
motor[rightintake]=0;
}
if (vexRT[Btn7U]==1)
{
SensorValue[cat1]=0;
SensorValue[cat2]=0;
}
else if (vexRT[Btn7D]==1)
{
SensorValue[cat2]=1;
SensorValue[cat1]=1;
}
if (vexRT[Btn8U]==1)
{
SensorValue[hang2]=1;
SensorValue[hang1]=1;
}
else if (vexRT[Btn8D]==1)
{
SensorValue[hang1]=0;
SensorValue[hang2]=0;
}
while(vexRT[Btn7L]==1)
{
if(SensorValue(pot) < 1450){
motor[leftlift] = 127;
motor[rightlift] = 127;
}
else if(SensorValue(pot) > 1500){
motor[leftlift] = -127;
motor[rightlift] = -127;
}
else
{
motor[leftlift] = 10;
motor[rightlift] = 10;
}
}
if(vexRT[Btn7R]==1)
{
if(SensorValue(pot) < 1900){
motor[leftlift] = 127;
motor[rightlift] = 127;
}
else if(SensorValue(pot) > 2025){
motor[leftlift] = -127;
motor[rightlift] = -127;
}
else
{
motor[leftlift] = 10;
motor[rightlift] = 10;
}
}
if(vexRT[Btn8L]==1)
{
if(SensorValue(pot) < 700){
motor[leftlift] = 50;
motor[rightlift] = 50;
}
else if(SensorValue(pot) > 710){
motor[leftlift] = -50;
motor[rightlift] = -50;
}
else if(SensorValue(pot) < 703){
motor[leftlift] = 35;
motor[rightlift] = 35;
}
else if(SensorValue(pot) > 707){
motor[leftlift] = -35;
motor[rightlift] = -35;
}
else
{
motor[leftlift] = 10;
motor[rightlift] = 10;
}
}
}
}
