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");
}
