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 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));
}
}
task changeBallCount() {
bool button8DPressed = false,
button8LPressed = false;
while (1) {
if (vexRT[Btn8D] && !button8DPressed) {
ballsInIntake--;
button8DPressed = true;
} else if (vexRT[Btn8L] && !button8LPressed) {
ballsInIntake++;
button8LPressed = true;
}
if (!vexRT[Btn8D]) {
button8DPressed = false;
}
if (!vexRT[Btn8L]) {
button8LPressed = false;
}
if (ballsInIntake > 4) {
ballsInIntake = 4;
} else if (ballsInIntake < 0) {
ballsInIntake = 0;
}
clearLCDLine(0);
displayLCDPos(0,0);
displayNextLCDNumber(ballsInIntake);
wait1Msec(100);
}
}
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()
{
do
{
derivitive;
displayNextLCDNumber(DPS);
}while(1<2)
}
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 screenrefresh()
{
clearLCDLine(1);
displayLCDPos(1,0);
if (redteam == true)
{
displayNextLCDString("RED ");
}
if (redteam == false)
{
displayNextLCDString("BlUE");
}
displayNextLCDString(" ");
displayNextLCDNumber(programselect);
displayNextLCDString(" OK");
}
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));
}
}
task countBallsInIntake() {
int numConsecZeros = 0, //number of consecutive 0s return by the limit switch
numConsecOnes = 0; //number of consecutive 1s return by the limit switch
while(1) {
if (ballsInIntake < 3 && rollerState == 1) { //if there are 2 or fewer balls in the intake, and intaking balls in
while(!SensorValue[intakeLimit]) { //limit switch indicates that a ball is not at the bottom of the dangle
wait1Msec(25); //wait until a ball approaches the dangle
}
while(numConsecZeros < 2) { //condition: SensorValue[intakeLimit]; wait until the ball has triggered the limit switch
if (!SensorValue[intakeLimit]) {
numConsecZeros++;
} else {
numConsecZeros = 0;
}
wait1Msec(25);
}
} else if (ballsInIntake <= 3 && rollerState == -1) { //if there are 3 or fewer balls in the intake and we are outtaking via the roller
while(!SensorValue[intakeLimit]) { //limit switch indicates that a ball is not at the bottom of the dangle
wait1Msec(25); //wait until a ball approaches the dangle
}
while(numConsecZeros < 2) { //condition: SensorValue[intakeLimit]; wait until the ball has triggered the limit switch
if (!SensorValue[intakeLimit]) {
numConsecZeros++;
} else {
numConsecZeros = 0;
}
wait1Msec(25);
}
} else if (ballsInIntake >= 3 && rollerState == 1) { //limit switch is pressed when there are 4 balls in the intake - do this when the third ball is in the intake, so ballsInIntake = 3
while(numConsecOnes < 2) { //condition: !SensorValue[intakeLimit]; only do this when going forward - this means we are intaking
if (SensorValue[intakeLimit]) {
numConsecOnes++;
} else {
numConsecOnes = 0;
}
wait1Msec(25);
}
} else if (ballsInIntake > 3 && rollerState == -1) { //limit switch is pressed when there are 4 balls in the intake - do this when the fourth ball is in the intake, so ballsInIntake = 3
while(numConsecZeros < 2) { //condition: SensorValue[intakeLimit]; only do this when going backward - this means we are releasing balls from the lower end of the intake; the 4th ball keeps the limt switch pressed
if (!SensorValue[intakeLimit]) {
numConsecZeros++;
} else {
numConsecZeros = 0;
}
wait1Msec(25);
}
}
numConsecZeros = 0; //reset to zero once limit we move on
numConsecOnes = 0;
//NOTE: this doesn't account for balls leaving the intake via the flywheel
//reach this point once the intake limit switch has been pressed and then released (so balls are counted after they are done passing the dangle)
if (rollerState == 1) { //if the roller is moving forward
ballsInIntake++; //increment the number of balls in the intake
} else if (rollerState == -1) { //if the roller is moving backwards (indicated by a rollerState value of -1)
ballsInIntake--; //decrement the number of balls in the intake
}
//limit the ballsinIntake variable to 0-4 (inclusive) only
if (ballsInIntake > 4) {
ballsInIntake = 4;
} else if (ballsInIntake < 0) {
ballsInIntake = 0;
}
/*
if (ballsInIntake < 4) {
overrideAutoOuttakeOnly = false;
}
if (flywheelMode == 3 || flywheelMode == 4) {
outtakeOnly = false;
} else {
if (ballsInIntake == 4) {
if (vexRT[Btn6U] && !overrideAutoOuttakeOnly) {
outtakeOnly = true;
} else {
outtakeOnly = false;
overrideAutoOuttakeOnly = true;
}
} else if (ballsInIntake < 4 && outtakeOnly) { //check if we can disable outttake only mode
outtakeOnly = false;
}
}*/
clearLCDLine(0);
displayLCDPos(0,0);
displayNextLCDNumber(ballsInIntake);
}
}
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;
}
}
}
}
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);
}