task leftRushOne(){
clearLCDLine(0);
clearLCDLine(1);
//clear LCD
//SensorValue[leftIEM] = 0;
//SensorValue[rightIEM] = 0;
driveTime(350, FORWARD, 127);
driveTime(200, BACKWARD, 127);
armTime(100,RAISE,127);
swivel(330,LEFT,60);
SensorValue[leftIEM] = 0;
SensorValue[rightIEM] = 0;
drive(.10 * clickspermeters,FORWARD,110); //10 to .15
swivel(350,RIGHT,60); //360 to 340
drive(.35 * clickspermeters,FORWARD,110); //.35 to .33
armTime(100,LOWER,127);
drive(.35 * clickspermeters,FORWARD,110);
drive(.14 * clickspermeters, FORWARD,40);
wait1Msec(275);
driveTime(700, BACKWARD,100);
armTime(1500, RAISE, 127);
wait1Msec(10);
drive(.10 * clickspermeters, FORWARD, 40);
drive(.46 * clickspermeters, FORWARD, 60);
autoIntake(2000, -127);
driveTime(400, BACKWARD, 100);
armTime(1100, RAISE, 120);
driveTime(400, BACKWARD, 100);
//auton = false;
if (auton == false){
armTime(1000,LOWER,100);
driveTime(1000,BACKWARD,100);
} //autoIntake(2000, 127);
}
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 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);
}
}
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;
bool getI2C = testI2C();
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, "Check Wires!!!");
}
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);
}
}
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 reverseFlywheel () {
while(true) {
if(vexRT(Btn7L) && flywheelVelocity > flywheelReverseStartThreshold) {
stopFlywheel();
clearLCDLine(1);
while(flywheelVelocity>0) {
setFlywheel(flywheelVelocity>flywheelSlowDownVelocity?0:-pow(abs((flywheelVelocity/1000)-flywheelSlowDownVelocity/1000),1.3));
clearLCDLine(0);
displayLCDNumber(0,0,flywheelVelocity);
displayLCDNumber(0,10,motor[flywheel1]);
delay(25);
}
}
else {
while(vexRT(Btn7L)) {
stopFlywheel();
setFlywheel(-127);
delay(25);
}
if(flywheelVelocity < 0 && !debugFlywheelActive) {
setFlywheel(0);
}
}
delay(25);
}
}
//This function updates the backlight, applies screen strings if they have
//changed, and applies menu hints. Up to now, variables topLCDLine and
//bottomLCDLine have been set to the desired strings. This keeps the LCD from
//being cleared and set excessively (which causes ugly blinking).
void outputLCD()
{
updateBacklight();
if (changedString(topLCDLine))
{
clearLCDLine(0);
displayLCDCenteredString(0,topLCDLine.curr);
}
if (changedString(bottomLCDLine))
{
clearLCDLine(1);
displayLCDCenteredString(1,bottomLCDLine.curr);
}
#ifdef MENU_WRAP //Wrap
if (sysState.curr != AUTONOMOUS && sysError==ERR_NONE)
{
displayLCDString(0,0, "<");
displayLCDString(0,15,">");
}
#else //No wrap
if (sysState.curr != AUTONOMOUS && sysError==ERR_NONE)
{
if (menuItemIndex>0) displayLCDString(0,0, "<"); //If not at the first item, show prev arrow
if (menuItemIndex<(int)M_NO_ITEMS-1) displayLCDString(0,15,">"); //If not at the last item, show next arrow
}
#endif
setLastString(&topLCDLine);
setLastString(&bottomLCDLine);
}
task usercontrol(){
bLCDBacklight = true;
clearLCDLine(0);
clearLCDLine(1);
while (true)
{
Variables();
Control();
}
}
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 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 pre_auton()
{
//Turn on LCD Backlight
clearLCDLine(0);
clearLCDLine(1);
bLCDBacklight = true;
// Task Manager
bStopTasksBetweenModes = true;
//Allows for LCD Selector to Run
startTask(Menu);
//Makes Sure Lift is Zeroed
//startTask(liftZero);
}
void calJoy(){//recalibrates joystick on-the-fly by saving values while joystick is released
halt();
clearLCDLine(1);
displayLCDString(1, 0, "calJoy: Waiting");
delay(2000);
if (abs(vexRT[Ch4]) < 30 && abs(vexRT[Ch3]) < 30){
joy1X = vexRT[Ch4];
joy1Y = vexRT[Ch3];
}
if (abs(vexRT[Ch1]) < 30 && abs(vexRT[Ch2]) < 30){
joy2X = vexRT[Ch1];
joy2Y = vexRT[Ch2];
}
clearLCDLine(1);
}//Function-variable creation end
task reverseFlywheel () {
while(true) {
if(vexRT(Btn7L) && motor[flywheel4]>0) {
stopFlywheel();
clearLCDLine(1);
while(flywheelVelocity>0) {
setFlywheel(flywheelVelocity<200?-pow(abs(x/40-5),1.5):0;
clearLCDLine(0);
displayLCDNumber(0,0,flywheelVelocity);
displayLCDNumber(0,10,motor[flywheel1]);
delay(25);
}
}
else {
while(vexRT(Btn7L)) {
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 leftRightOne(){
clearLCDLine(0);
clearLCDLine(1);
//SensorValue[leftIEM] = 0;
//SensorValue[rightIEM] = 0;
//Clear LCD
//jerk to get intake down
//turn(830,RIGHT,40);
// turn right 90 degrees
//drive(.43* clickspermeters, FORWARD, 127);
//forward to the line that the bonus sack is on999/8
//turn(550,LEFT,40);
//swivel turn to pick up yellow sack
//driveTime(500,FORWARD,100);
//driveTime(500,BACKWARD,100);
driveTime(500,FORWARD,100);
driveTime(500,BACKWARD,100);
drive(.35 * clickspermeters, FORWARD,100);
wait1msec(100);
drive(.15 * clickspermeters, FORWARD, 100);
wait1Msec(500);
drive(.45 * clickspermeters, FORWARD, 100); //.50 to .45
//forward to yellow sack
//turn to trough
wait1Msec(300);
//armTime(1150,RAISE,127);
//raise arm
swivel(310, RIGHT, 80);
drive(.2 * clickspermeters, FORWARD, 127);
//drive forward to goal
drive(.28 * clickspermeters, FORWARD, 127); //.20 to .23 to .26 to .28
wait1Msec(900);
driveTime(700, BACKWARD, 67);
//Let sacks out5
turn(650, LEFT, 80); //550 to 650
armTime(1800,RAISE,127);
drive(.51 * clickspermeters, FORWARD, 60); //127 to 80 //.53 to .58 to .52 to .48
autoIntake(2050, -127);
wait1Msec(200);
driveTime(700, BACKWARD, 95);
//auton=false;
/*
if (auton == false){
turn(200,LEFT,60);
driveTime(2000, BACKWARD, 100);
}
*/
}
task autonomous(){//Autonomous block begin
clearLCDLine(1);
switch(auton)
{
case 1://red 1
//code here
break;
case 2://red 2
//code here
break;
case 3://blue 1
//code here
break;
case 4://blue 2
//code here
break;
case 5://programming skills
//code here
break;
case 6://emergency fallback
//code here
break;
}
while(bIfiAutonomousMode){//"catch" program while autonomous mode is active to stop auton code from looping
halt(2);
}
}//Autonomous block end
task abi() {
float kP = 0.63;//.07; for mid/pipe
motor[flywheel4] = 25;
while(motor[flywheel4] < speedB+11) {
motor[flywheel4]+=3;
delay(40);
}
int motorSpeedA, motorSpeedB;
while(true) {
kP = 0.1;
veloA = currentGoalVelocity;
currVelo = getFlywheelVelocity();
motorSpeedA = speedA + (veloA-currVelo) * kP;
motorSpeedB = speedB + (veloA-currVelo) * kP;
motorSpeedA = motorSpeedA>100?100:motorSpeedA;
writeDebugStreamLine("%d, %d, %d",motorSpeedA, motorSpeedB, currVelo*kP);
if(currVelo < (veloA==VELOCITY_LONG?veloA+58:veloA+45)) {
motor[flywheel4] = motorSpeedA;
} else {
motor[flywheel4] = motorSpeedB;
}
clearLCDLine(0);
displayLCDNumber(0,1,currVelo);
displayLCDNumber(0,5,veloA);
displayLCDNumber(0,10,motor[flywheel4]);
delay(30);
}
}
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);
}
}
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 Display_LCD(){
bLCDBacklight = true;
while(true){
// Converting autonN to a string.
//string autonNstring = autonN;
// Battery level indicator
string display = "R:";
string regularBat, backupBat;
StringFormat(regularBat, "%1.2f", (float)nImmediateBatteryLevel/1000);
//StringFormat(expand_bat, "%1.2f", (float)SensorValue[]/275);
StringFormat(backupBat, "%1.2f", (float)BackupBatteryLevel/1000);
StringConcatenate(display, regularBat);
StringConcatenate(display, "V B:");
StringConcatenate(display, backupBat);
StringConcatenate(display, "V ");
clearLCDLine(0);
clearLCDLine(1);
if(vexRT[Btn7L] == 1)
{
displayLCDCenteredString(0, "Battery Power:");
displayLCDCenteredString(1, display);
}
else
{
if((float)nImmediateBatteryLevel/1000 < 6.45)
{
displayLCDCenteredString(0, "***WARNING***");
displayLCDCenteredString(1, "*LOW BATTERY*");
}
else
{
// Default screen:
string left = SensorValue[LeftLine];
string right = SensorValue[RightLine];
string arm = armRotation;
string lolbeav = isBeingIntaken;
displayLCDCenteredString(0, left);
displayLCDCenteredString(1, lolbeav);
}
}
wait1Msec(100);
}
}
void
LcdAutonomousDisplay( vexLcdMenus menu )
{
// Cleat the lcd
clearLCDLine(0);
clearLCDLine(1);
// Display the selection arrows
displayLCDString(1, 0, l_arr_str);
displayLCDString(1, 13, r_arr_str);
displayLCDString(1, 5, "CHANGE");
// Show the autonomous names
switch( menu ) {
case kMenuAlliance:
if( vAlliance == kAllianceBlue )
displayLCDString(0, 0, "Alliance - BLUE");
else
displayLCDString(0, 0, "Alliance - RED");
break;
case kMenuStartpos:
if( vPosition == kStartHanging )
displayLCDString(0, 0, "Start - Hanging");
else
displayLCDString(0, 0, "Start - Middle");
break;
case kMenuAutonSelect:
switch( vAuton ) {
case 0:
displayLCDString(0, 0, "Default");
break;
case 1:
displayLCDString(0, 0, "Special 1");
break;
default:
char str[20];
sprintf(str,"Undefined %d", vAuton );
displayLCDString(0, 0, str);
break;
}
break;
default:
displayLCDString(0, 0, "Unknown");
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);
}
}
void pre_auton()
{
// Set bStopTasksBetweenModes to false if you want to keep user created tasks running between
// Autonomous and Tele-Op modes. You will need to manage all user created tasks if set to false.
bStopTasksBetweenModes = false;
nMotorEncoder[blFly] = nMotorEncoder[brFly] = 0;
for(int i; i < 9; i++) {
lVel[i] = rVel[i] = 0;
}
bLCDBacklight = true;
clearLCDLine(0);
clearLCDLine(1);
batt = nAvgBatteryLevel / 1000.0; //The decimal is necessary because operators return values as simple as the most complex number
sprintf(battStr, "%1.2f", batt);
displayLCDCenteredString(0, "Battery:");
displayLCDCenteredString(1, battStr);
}
task leftLeftOne(){
clearLCDLine(0);
clearLCDLine(1);
//clear LCD
SensorValue[leftIEM] = 0;
SensorValue[rightIEM] = 0;
driveTime(350, FORWARD, 127);
driveTime(200, BACKWARD, 127); //200 to 250 to 200
//quick jerk to bring down intake
swivel(880,LEFT,65); //630 to 600 to 610 to 600 to 630 to 650 to 670 to 690 to 750 to 770 to 800 to 780 to 750
//swivel left to wall
drive( .35 * clickspermeters, FORWARD, 70); //60 70 to .36
wait1Msec(800);
//drive to wall and pick up green sack
driveTime(450, BACKWARD, 127);
//drive backward
turn(800, RIGHT, 55);//600 to 400 to 350 to 370 to 350 to 600 to 670 to 690 to 680 to 690 to 700 to 720 to 650 to 750 to 740
armTime(200,RAISE,127); //raise arm to avoid tape //50 to 125 to 325 to 125 to 200
//wait1Msec(50); replace waits with armTime
// turn to trough
drive(.65 * clickspermeters, FORWARD, 110);
wait1Msec(50);
armTime(300,LOWER,127);//lower the arm to contact ground // 125 5o 345 to 125 to 250 to 200 to 250
//forward to yellow sack
drive(.32 * clickspermeters, FORWARD, 70); // .33 to .31
wait1Msec(1000);
//drive forward to yellow sack and wait for 1 second
driveTime(700, BACKWARD, 100);
wait1Msec(50);
//go backward
armTime(1500, RAISE, 127);//950 to 1100 to 1300
wait1Msec(10);
//raise arm
drive(.10 * clickspermeters, FORWARD, 40);
drive(.46 * clickspermeters, FORWARD, 60); //.49 to .40 to .43
//Forward to goal
autoIntake(2000, -127); //why did this reverse? negative to positive to negative
//Let sacks out
driveTime(400, BACKWARD, 100);
//back off
//auton=false;
if (auton == false){
turn(1800,LEFT,60);
driveTime(2000, BACKWARD, 100);
}
}
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");
}
}
task usercontrol()
{
//startTask(accelerate); //controls flywheel acceleration so that the flywheel can accelerate concurrently with drivetrain and intake motors
clearLCDLine(0);
clearLCDLine(1);
bLCDBacklight = true;
char str[32];
bLCDBacklight = true;
// Start the flywheel control task
startTask(flywheelController);
while(true)
{
//drivetrain
motor[leftDrive] = vexRT[Ch3];
motor[rightDrive] = vexRT[Ch2];
sprintf( str, "%4d %4d %5.2f", l_target_velocity, l_motor_velocity, nImmediateBatteryLevel/1000.0 );
displayLCDString(0, 0, str );
sprintf( str, "%4.2f %4.2f ", l_drive, l_drive_at_zero );
displayLCDString(1, 0, str );
//intake
if(vexRT[Btn6U] == 1)
{
motor[intake1] = 125;
motor[intake2] = 125;
}
else if(vexRT[Btn6D] == 1)
{
motor[intake1] = -125;
motor[intake2] = -125;
}
else
{
motor[intake1] = 0;
motor[intake2] = 0;
}
wait1Msec(10); //so we don't overload the CPU
}
}
//Measures shooter speed, calculates power, and updates LCD
void calculateShooter() {
wait1Msec(50);
lastEncA = currentDistA; //Get the prevoius speed of the shooter
currentDistA = SensorValue[encShooterRight2]; //Get the current speed of the shooter SensorValue[encShooterRight2]
int currSysTime;
//Calculate the motor speed based on the system timer and the motor distance. Average the results, weighing
//heavily on the previous value to smooth out fluctuations
float speed = ((currentDistA - lastEncA) * 50.0 / ((currSysTime = nSysTime) - lastSysTime+1));
speedAverages = speedAverages*0.9+ speed*0.1;
//Save the previous states for next iteration
lastSpeedA = speed;
lastSysTime = currSysTime;
if (speed > 80) { speed = 80; } //Clamp the aspeed to make sure it doesn't go over 80/s
else if (speed < -80) { speed = -80; } // (prevents it from generating erronously high values)
//Calculate error and add in a reversal gain if we are approaching the speed
//(prevents overshooting due to the flywheel behavior of the shooter wheels)
float error = targetSpeed - speedAverages;
if (abs(error) < 0.25) {error = 0;}
else if (abs(error) < 0.5) {error = error*-0.35;}
//Make sure the motors will spin down on their own and not generate a negative power
if (targetSpeed == 0) {error=0;}
//Calculate power based on the error. Clamp the motor output to 127
shooterMotorRaw = manualSetSpeed+ error*(2.9*6.0);
if (shooterMotorRaw <= 0) { shooterMotorRaw = 0; }
//Update the LCD
clearLCDLine(1);
ready = (speedAverages > optimalSpeed - 1 && speedAverages < optimalSpeed + 1);
bLCDBacklight = ready;
string str;
stringFormat(str, "M %-2i/%-3i",manualSetSpeed,shooterMotorRaw);
displayLCDCenteredString(1, str);
clearLCDLine(0);
string str2;
stringFormat(str2, "%2.2f/%2.2f",speedAverages,targetSpeed);
displayLCDCenteredString(0, str2);
}
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);
}
}
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
void displayEncoders()
{
//Setup the VEX LCD for displaying encoder values
clearLCDLine(0);
clearLCDLine(1);
displayLCDString(0, 0, "R: ");
displayLCDString(1, 0, "L: ");
//Clear the encoders associated with the left and right motors
nMotorEncoder[RFfly] = 0;
nMotorEncoder[LFfly] = 0;
while(1 == 1)
{
//Display the right and left motor encoder values
displayLCDNumber(0, 3, nMotorEncoder[RFfly], 6);
displayLCDNumber(1, 3, nMotorEncoder[LFfly], 6);
}
}
