task main() { int old_reading = shooter_motor_speed(); int old_time = nSysTime; int output = 82; //what the motor is set to int target = 100; //the value currently being read while(true) { displayLCDNumber(0,0,output,4); displayLCDNumber(0,1,target,6); //continuously retrieve motor speed of flywheel shooter_motor_speed(); int new_time = nSysTime; int new_reading = shooter_motor_speed(); int curr_speed = ((new_reading - old_reading)/(new_time - old_time)); int change = (target - curr_speed); //getting the change in speed //set motors to account for change in speed shooter_motor_set(output - (change*0.05)); //*0.05 is to convert into appropriate units old_time = new_time; old_reading = new_reading; } }
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 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 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); } }
void lcdDispPwr(bool doUseLRPwr) { if (doUseLRPwr) { displayLCDCenteredString(0, "Long Power:"); displayLCDNumber(1, 0, flyLRPwr); } else { displayLCDCenteredString(0, "Short Power:"); displayLCDNumber(1, 0, flySRPwr); } }
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)) {
//*!!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); } }
task jonflywheelControl () { float kP = 0.02; setFlywheel(20); flywheelRampUp (currentShot.lowSpeed); int flywheelSpeed; while(true) { flywheelSpeed = motor[flywheel4] + (currentShot.velocity-flywheelVelocity)*kP; clearLCDLine(0); displayLCDNumber(0,1,flywheelSpeed); displayLCDNumber(0,5,flywheelVelocity); displayLCDNumber(0,10,(currentShot.velocity-flywheelVelocity)*kP); flywheelSpeed = flywheelSpeed>100?100:flywheelSpeed; flywheelSpeed = flywheelSpeed<0?0:flywheelSpeed; setFlywheel(flywheelSpeed); } }
task flywheelControl() { setFlywheel(20); flywheelRampUp (currentShot.lowSpeed); int flywheelSpeedA, flywheelSpeedB; while(true) { flywheelSpeedA = currentShot.highSpeed + (currentShot.velocity-flywheelVelocity) * currentShot.kP; flywheelSpeedB = currentShot.lowSpeed + (currentShot.velocity-flywheelVelocity) * currentShot.kP; flywheelSpeedA = flywheelSpeedA>100?100:flywheelSpeedA; flywheelSpeedB = flywheelSpeedB>100?100:flywheelSpeedB; flywheelSpeedA = flywheelSpeedA<0?0:flywheelSpeedA; flywheelSpeedB = flywheelSpeedB<0?0:flywheelSpeedB; if(flywheelVelocity < currentShot.velocity+currentShot.ramp) { setFlywheel(flywheelSpeedA); } else { setFlywheel(flywheelSpeedB); } if(debugMode) { flywheelLCD(); flywheelLED(); clearLCDLine(1); displayLCDNumber(1,1,flywheelSpeedA); displayLCDNumber(1,5,flywheelSpeedB); } delay(flywheelControlUpdateFrequency); } }
void displayBattLevel(){ displayLCDString(0, 0, "Battery Level: "); displayLCDNumber(1, 0, (nImmediateBatteryLevel - 3000) / 72); displayNextLCDString("%"); }
////// 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); } }
void flywheelLCD () { clearLCDLine(0); displayLCDNumber(0,1,flywheelVelocity); displayLCDNumber(0,5,currentShot.velocity); displayLCDNumber(0,10,motor[flywheel4]); }
task lcd() { const float flyPwrIncrement = 5; const word battThresh = 7800; const long pwrBtnsDelayInterval = 750, pwrBtnsRepeatInterval = 100, dispPwrTimeout = 1000; bool dispPwr = false, doUseLRPwr = true, flash = false, forceBattWarning = true, pwrBtnsDown, pwrBtnsRepeating; float pwrBtns; long time = nSysTime, flashTs = time, dispPwrTs = time, pwrBtnTs = time; string str; DLatch dismissWarningLatch, pwrBtnLatch, pwrBtnLRModeLatch, pwrBtnSRModeLatch; while (true) { time = nSysTime; if (nImmediateBatteryLevel < battThresh) { if ((time - flashTs) >= 500) { flash = !flash; flashTs = time; } SensorValue[redLed] = SensorValue[yellowLed] = SensorValue[greenLed] = flash; } else { SensorValue[redLed] = SensorValue[yellowLed] = SensorValue[greenLed] = 0; } clearLCD(); if (nLCDButtons & kButtonCenter) { bLCDBacklight = true; displayLCDCenteredString(0, "Battery:"); sprintBatt(str); displayLCDString(1, 0, str); } else if (nImmediateBatteryLevel < battThresh && (rkBotDisabled || rkAutonMode || forceBattWarning)) { bLCDBacklight = flash; if (flash) displayLCDCenteredString(0, "BATTERY WARNING"); sprintBatt(str); displayLCDString(1, 0, str); if (fallingEdge(&dismissWarningLatch, nLCDButtons || (abs(vexRT[AccelY]) > 63))) forceBattWarning = false; } else if (rkBotDisabled) { bLCDBacklight = false; displayLCDCenteredString(0, "Moosebot Mk. II"); displayLCDCenteredString(1, "4800Buckets"); } else if (rkAutonMode) { bLCDBacklight = true; displayLCDCenteredString(0, "BUCKETS MODE"); displayLCDCenteredString(1, "ENGAGED"); } else { //User op mode bLCDBacklight = true; pwrBtnsDown = PWR_BTN_DOWN ^ PWR_BTN_UP; risingEdge(&pwrBtnLatch, pwrBtnsDown); if (pwrBtnsDown || (PWR_BTN_LRMODE ^ PWR_BTN_SRMODE)) dispPwrTs = time; if (risingEdge(&pwrBtnLRModeLatch, PWR_BTN_LRMODE)) doUseLRPwr = true; if (risingEdge(&pwrBtnSRModeLatch, PWR_BTN_SRMODE)) doUseLRPwr = false; if (pwrBtnsDown) { dispPwrTs = time; if (pwrBtnLatch.out || (time - pwrBtnTs >= (pwrBtnsRepeating ? pwrBtnsRepeatInterval : pwrBtnsDelayInterval))) { pwrBtnTs = time; if (!pwrBtnsRepeating) pwrBtnsRepeating = true; pwrBtns = twoWay((nLCDButtons & kButtonLeft) || vexRT[Btn7D], -flyPwrIncrement, (nLCDButtons & kButtonRight) || vexRT[Btn7U], flyPwrIncrement); if (doUseLRPwr) flyLRPwr += pwrBtns; else flySRPwr += pwrBtns; } } if (time - dispPwrTs <= dispPwrTimeout) { if (doUseLRPwr) { displayLCDCenteredString(0, "Long Power:"); displayLCDNumber(1, 0, flyLRPwr); } else { displayLCDCenteredString(0, "Short Power:"); displayLCDNumber(1, 0, flySRPwr); } } else if (flyTbh.doRun) { sprintf(str, "% 07.2f % 07.2f", fmaxf(-999.99, fminf(999.99, flyDiff.out)), fmaxf(-999.99, fminf(999.99, flyTbh.err))); displayLCDString(0, 0, str); sprintf(str, "% 07.2f % 07.2f", fmaxf(-999.99, fminf(999.99, fly2Flt.out)), fmaxf(-999.99, fminf(999.99, flyTbh.out))); displayLCDString(1, 0, str); SensorValue[redLed] = SensorValue[yellowLed] = SensorValue[greenLed] = 0; if (isTbhInThresh(&flyTbh, velThresh)) { if (isTbhDerivInThresh(&flyTbh, accelThresh)) SensorValue[greenLed] = 1; else SensorValue[yellowLed] = 1; } else SensorValue[redLed] = 1; } else { sprintf(str, "%-8s%8s", "Speed", "Error"); displayLCDString(0, 0, str); sprintf(str, "%-8s%8s", "Accel", "Out"); displayLCDString(1, 0, str); } } wait1Msec(50); } }
void lcdDisplayPot() { lcdClear(); displayLCDNumber(0, 0, SensorValue[Sel]/10, 2); displayLCDNumber(0, 8, SensorValue[Sel], 2); displayLCDString(1,0,"<Back"); }
/* Display shooter speeds on LCD */ void lcdDisplayShooter() { lcdClear(); displayLCDNumber(0, 0, motor[S_L], 2); displayLCDNumber(0, 8, motor[S_R], 2); displayLCDString(1,0,"<Back"); }