void OperatorControl(void) { autonomous = false; //myRobot.SetSafetyEnabled(false); //myRobot.SetInvertedMotor(kFrontLeftMotor, true); // myRobot.SetInvertedMotor(3, true); //variables for great pid double rightSpeed,leftSpeed; float rightSP, leftSP, liftSP, lastLiftSP, gripSP, tempLeftSP, tempRightSP; float stickY[2]; float stickYAbs[2]; bool lightOn = false; AxisCamera &camera = AxisCamera::GetInstance(); camera.WriteResolution(AxisCameraParams::kResolution_160x120); camera.WriteMaxFPS(5); camera.WriteBrightness(50); camera.WriteRotation(AxisCameraParams::kRotation_0); rightEncoder->Start(); leftEncoder->Start(); liftEncoder->Start(); rightEncoder->Reset(); leftEncoder->Reset(); liftEncoder->Reset(); bool fastest = false; //transmission mode float reduction = 1; //gear reduction from bool bDrivePID = false; //float maxSpeed = 500; float liftPower = 0; float liftPos = -10; bool disengageBrake = false; int count = 0; //int popCount = 0; double popStart = 0; double popTime = 0; int popStage = 0; int liftCount = 0; int liftCount2 = 0; const float LOG17 = log(17.61093344); float liftPowerAbs = 0; float gripError = 0; float gripErrorAbs = 0; float gripPropMod = 0; float gripIntMod = 0; bool shiftHigh = false; leftEncoder->SetDistancePerPulse((6 * PI / 1000)/reduction); //6-inch wheels, 1000 raw counts per revolution, rightEncoder->SetDistancePerPulse((6 * PI / 1000)/reduction); //about 1:1 gear ratio DriverStationEnhancedIO &myEIO = driverStation->GetEnhancedIO(); GetWatchdog().SetEnabled(true); GetWatchdog().SetExpiration(0.3); PIDDriveLeft->SetOutputRange(-1, 1); PIDDriveRight->SetOutputRange(-1, 1); //PIDDriveLeft->SetInputRange(-244,244); //PIDDriveRight->SetInputRange(-244,244); PIDDriveLeft->SetTolerance(5); PIDDriveRight->SetTolerance(5); PIDDriveLeft->SetContinuous(false); PIDDriveRight->SetContinuous(false); //PIDDriveLeft->Enable(); //PIDDriveRight->Enable(); PIDDriveRight->SetPID(DRIVEPROPGAIN, DRIVEINTGAIN, DRIVEDERIVGAIN); PIDDriveLeft->SetPID(DRIVEPROPGAIN, DRIVEINTGAIN, DRIVEDERIVGAIN); liftSP = 0; PIDLift->SetTolerance(10); PIDLift->SetContinuous(false); PIDLift->SetOutputRange(-0.75, 1.); //BUGBUG PIDLift->Enable(); gripSP = 0; float goalGripSP = 0; bool useGoalSP = true; bool gripPIDOn = true; float gripP[10]; float gripI[10]; float gripD[10]; PIDGrip->SetOutputRange(-0.5, 0.28); //Negative goes up PIDGrip->SetTolerance(5); PIDGrip->SetSetpoint(0); PIDGrip->Enable(); miniDep->Set(miniDep->kForward); int i = 0; while(i < 10) { gripP[i] = GRIPPROPGAIN; i++; } i = 0; while(i < 10) { gripI[i] = GRIPINTGAIN; i++; } i = 0; while(i < 10) { gripD[i] = GRIPDERIVGAIN; i++; } while (IsOperatorControl()) { GetWatchdog().Feed(); count++; #if !(SKELETON) sendVisionData(); #endif /* if(LIFTLOW1BUTTON && !(counts%10)) printf("LIFTLOW1BUTTON\n"); if(LIFTLOW2BUTTON && !(counts%10)) printf("LIFTLOW2BUTTON\n"); if(LIFTMID1BUTTON && !(counts%10)) printf("LIFTMID1BUTTON\n"); if(LIFTMID2BUTTON && !(counts%10)) printf("LIFTMID2BUTTON\n"); if(LIFTHIGH1BUTTON && !(counts%10)) printf("LIFTHIGH1BUTTON\n"); if(LIFTHIGH2BUTTON && !(counts%10)) printf("LIFTHIGH2BUTTON\n"); */ /* if(lsLeft->Get()) printf("LSLEFT\n"); if(lsMiddle->Get()) printf("LSMIDDLE\n"); if(lsRight->Get()) printf("LSRIGHT\n"); */ stickY[0] = stickL.GetY(); stickY[1] = stickR.GetY(); stickYAbs[0] = fabs(stickY[0]); stickYAbs[1] = fabs(stickY[1]); if(bDrivePID) { #if 0 frontLeftMotor->Set(stickY[0]); rearLeftMotor->Set(stickY[0]); frontRightMotor->Set(stickY[1]); rearRightMotor->Set(stickY[1]); if(!(count%5)) printf("Speeds: %4.2f %4.2f Outputs: %f %f \n", leftEncoder->GetRate(), rightEncoder->GetRate(), frontLeftMotor->Get(), frontRightMotor->Get()); #endif if(stickYAbs[0] <= 0.05 ) { leftSP = 0; if(!(count%3) && !BACKWARDBUTTON) { PIDDriveLeft->Reset(); PIDDriveLeft->Enable(); } } else leftSP = stickY[0] * stickY[0] * (stickY[0]/stickYAbs[0]); //set points for pid control if(stickYAbs[1] <= 0.05) { rightSP = 0; if(!(count%3) && !BACKWARDBUTTON) { PIDDriveRight->Reset(); PIDDriveRight->Enable(); } } else rightSP = stickY[1] * stickY[1] * (stickY[1]/stickYAbs[1]); if (BACKWARDBUTTON) { tempRightSP = rightSP; tempLeftSP = leftSP; rightSP = -tempLeftSP; leftSP = -tempRightSP; //This line and above line sets opposite values for the controller. ...Theoretically. } PIDDriveLeft->SetSetpoint(leftSP); PIDDriveRight->SetSetpoint(rightSP); leftSpeed = leftEncoder->GetRate(); rightSpeed = rightEncoder->GetRate(); if(!(count++ % 5)) { printf("rate L: %2.2f R: %2.2f SP %2.4f %2.4f ERR %2.2f %2.2f Pow: %1.2f %1.2f\n", leftPIDSource->PIDGet(), rightPIDSource->PIDGet(), leftSP, rightSP, PIDDriveLeft->GetError(), PIDDriveRight->GetError(), frontLeftMotor->Get(), frontRightMotor->Get()); //printf("Throttle value: %f", stickR.GetThrottle()); if(PIDDriveRight->OnTarget()) printf("Right on \n"); if(PIDDriveLeft->OnTarget()) printf("Left on \n"); } if(PIDRESETBUTTON) { //PIDDriveRight->SetPID(stickR.GetThrottle()+1,DRIVEINTGAIN, DRIVEDERIVGAIN); //PIDDriveLeft->SetPID(stickR.GetThrottle()+1,DRIVEINTGAIN, DRIVEDERIVGAIN); PIDDriveLeft->Reset(); PIDDriveRight->Reset(); PIDDriveLeft->Enable(); PIDDriveRight->Enable(); } } else { if(PIDDriveLeft->IsEnabled()) PIDDriveLeft->Reset(); if(PIDDriveRight->IsEnabled()) PIDDriveRight->Reset(); if(DEMOSWITCH) { stickY[0] = stickY[0]*(1 - lift->getPosition()); //reduces power based on lift height stickY[1] = stickY[0]*(1 - lift->getPosition()); } if(stickYAbs[0] > 0.05) { frontLeftMotor->Set(stickY[0]); rearLeftMotor->Set(stickY[0]); } else { frontLeftMotor->Set(0); rearLeftMotor->Set(0); } if(stickYAbs[1] > 0.05) { frontRightMotor->Set(-stickY[1]); rearRightMotor->Set(-stickY[1]); } else { frontRightMotor->Set(0); rearRightMotor->Set(0); } } if(stickL.GetRawButton(2) && stickL.GetRawButton(3) && stickR.GetRawButton(2) && stickR.GetRawButton(3) && BACKWARDBUTTON && !(count%5)) bDrivePID = !bDrivePID; if((SHIFTBUTTON && shiftHigh) || DEMOSWITCH) { shifter->Set(shifter->kReverse); shiftHigh = false; maxSpeed = 12; } else if(!SHIFTBUTTON && !shiftHigh && !DEMOSWITCH) { shifter->Set(shifter->kForward); shiftHigh = true; maxSpeed = 25; //last value 35 } sendIOPortData(); #if !(SKELETON) /* if(LIGHTBUTTON) lightOn = true; else lightOn = false; if(!lightOn) light->Set(light->kOff); if(lightOn) light->Set(light->kOn); */ if(!MODESWITCH) { lastLiftSP = liftSP; if(!PIDLift->IsEnabled()) PIDLift->Enable(); if(LIFTLOW1BUTTON) liftSP = LIFTLOW1; if(LIFTLOW2BUTTON) liftSP = LIFTLOW2; if(LIFTMID1BUTTON) liftSP = LIFTMID1; if(LIFTMID2BUTTON) liftSP = LIFTMID2; if(LIFTHIGH1BUTTON) liftSP = LIFTHIGH1; if(LIFTHIGH2BUTTON && !(DEMOSWITCH && (stickYAbs[0] > 0.05 || stickYAbs[1] > 0.05))) liftSP = LIFTHIGH2; if(!lift->isBraking() && !disengageBrake) { PIDLift->SetSetpoint(liftSP); if(liftSP == 0 && liftPIDSource->PIDGet() < 0.1) //BUGBUG { //PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, 3*LIFTDERIVGAIN); PIDLift->SetOutputRange(-liftPIDSource->PIDGet() - 0.1, 1.); //BUGBUG } else PIDLift->SetOutputRange(-0.75, 1.); //BUGBUG } if(lift->isBraking() && lastLiftSP != liftSP) { PIDLift->SetSetpoint(lastLiftSP + 0.06); PIDLift->SetPID(12.5 + 1.5*lift->getPosition(), LIFTINTGAIN + 0.6 + 3*lift->getPosition()/10, 0); lift->brakeOff(); disengageBrake = true; if(!liftCount) liftCount = count; } //set brake (because near) if(fabs(PIDLift->GetError()) < 0.01 && !lift->isBraking() && !disengageBrake) { if(liftCount == 0) liftCount = count; if(count - liftCount > 3) { PIDLift->Reset(); liftMotor1->Set(LIFTNEUTRALPOWER); liftMotor2->Set(LIFTNEUTRALPOWER); Wait(0.02); lift->brakeOn(); Wait(0.02); liftMotor1->Set(0.0); liftMotor2->Set(0.0); PIDLift->Enable(); PIDLift->SetSetpoint(lift->getPosition()); liftCount = 0; } //if(!(count%50)) printf("Braking/Not PID \n"); } if(lift->isBraking() && !(count%10)) PIDLift->SetSetpoint(lift->getPosition()); if(fabs(PIDLift->GetError()) < 0.01 && disengageBrake && count - liftCount > 3) { disengageBrake = false; if(liftEncoder->PIDGet() < liftSP) PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, LIFTDERIVGAIN - 0.015); else PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, LIFTDERIVGAIN + 0.015); liftCount = 0; } //pid /* else if(!(fabs(PIDLift->GetError()) < 0.04) && !lift->isBraking() && liftPos == -20) { PIDLift->Enable(); liftPos = -10; printf("PID GO PID GO PID GO PID GO PID GO \n"); } */ //when liftPos is positive, measures position //when liftPos = -10, is pidding //when liftPos = -20, has just released brake } else //(MODESWITCH) { if(PIDLift->IsEnabled()) PIDLift->Reset(); liftPower = .8*pow(2*((log(LIFTSLIDER + 0.003/.0208116511)/LOG17) + 0.116), 2)*(2*((log(LIFTSLIDER + 0.003/.0208116511)/LOG17) + 0.116)/fabs(2*((log(LIFTSLIDER + 0.003/.0208116511)/LOG17) + 0.116))); liftPowerAbs = fabs(liftPower); if(liftPowerAbs > 1) liftPower /= liftPowerAbs; //if(!(count%5)) printf("Slider: %f", liftPower); if(lift->isBraking() && liftPowerAbs > 0.05) lift->brakeOff(); else if(!lift->isBraking() && liftPowerAbs <= 0.05 && !(count%5)) lift->brakeOn(); if (liftPowerAbs > 0.05) { liftMotor1->Set(liftPower); liftMotor2->Set(liftPower); } else { liftMotor1->Set(0); liftMotor2->Set(0); } } if(MODESWITCH && LIFTLOW1BUTTON && LIFTMID1BUTTON && LIFTHIGH1BUTTON) liftEncoder->Reset(); /* if(!(count%5)) { printf("Lift pos: %f Lift error: %f Lift SP: %f \n", liftPIDSource->PIDGet(), PIDLift->GetError(), PIDLift->GetSetpoint()); } */ if(!(count%5) && MODESWITCH && GRIPPERPOSUP && GRIPPERPOSDOWN && GRIPPERPOP) { gripPIDOn = !gripPIDOn; printf("Switch'd\n"); } if(gripPIDOn) { if(!PIDGrip->IsEnabled()) PIDGrip->Enable(); if(GRIPPERPOSUP && !GRIPPERPOSDOWN) { goalGripSP = 0; } else if(GRIPPERPOSDOWN && !GRIPPERPOSUP && lift->getPosition() < 0.5) { goalGripSP = 1; } /* else if(!GRIPPERPOSDOWN && !GRIPPERPOSUP) { goalGripSP = 0.5; } */ gripError = PIDGrip->GetError(); gripErrorAbs = fabs(gripError); PIDGrip->SetSetpoint(goalGripSP); if(gripErrorAbs < 0.4) PIDGrip->SetOutputRange(-0.4, 0.6); //negative is up else PIDGrip->SetOutputRange(-0.9, 0.8); //negative is up if(gripErrorAbs > 0.0 && gripErrorAbs < 0.2) { PIDGrip->SetPID(GRIPPROPGAIN - 1.25*(1 - gripErrorAbs) + gripPropMod, GRIPINTGAIN + gripIntMod, 0.3 + 0.1*(1 - gripPIDSource->PIDGet())); } else { PIDGrip->SetPID(GRIPPROPGAIN - 1.*(1 - gripErrorAbs) + gripPropMod, 0, 0.02); } if(fabs(gripPIDSource->PIDGet()) < 0.03 && PIDGrip->GetSetpoint() == 0) { gripLatch1->Set(true); gripLatch2->Set(false); } else if(!(gripLatch1->Get() && PIDGrip->GetSetpoint() == 0) || gripPIDSource->PIDGet() < 0) { gripLatch1->Set(false); gripLatch2->Set(true); } if(gripLatch1->Get() && !(count%20)) { PIDGrip->Reset(); PIDGrip->Enable(); } /* if(stickL.GetRawButton(1) && !(count%5)){ gripIntMod -= 0.001; } if(stickR.GetRawButton(1) &&!(count%5)) { gripIntMod += 0.001; } if(stickL.GetRawButton(2) && !(count%5)) { gripPropMod -= 0.1; } if(stickL.GetRawButton(3) && !(count%5)) { gripPropMod += 0.1; } */ /* if(LIFTBOTTOMBUTTON) { PIDGrip->Reset(); PIDGrip->Enable(); } */ if(!(count%5)) { printf("Gripper pos: %f Gripper error: %f Grip power: %f \n", gripPIDSource->PIDGet(), PIDGrip->GetError(), gripMotor1->Get()); } } //Calibration routine else { if(gripLatch1->Get() == true) { gripLatch1->Set(false); gripLatch2->Set(true); } if(PIDGrip->IsEnabled()) PIDGrip->Reset(); if(GRIPPERPOSUP) { gripMotor1->Set(-0.5); //gripMotor2->Set(0.5); } else if(GRIPPERPOSDOWN) { gripMotor1->Set(0.5); //gripMotor2->Set(-0.5); } else { gripMotor1->Set(0); //gripMotor2->Set(0); } } //if(!(count%5)) printf("Grip volts: %f \n", gripPot->GetVoltage()); //if(!(count%5)) printf("Grip 1 voltage: %f \n", gripMotor1->Get()); if(GRIPPEROPEN && !popStage) { #if !(INNERGRIP) gripOpen1->Set(true); gripOpen2->Set(false); #else gripOpen1->Set(false); gripOpen2->Set(true); #endif } else if(!popStage) { #if !(INNERGRIP) gripOpen1->Set(false); gripOpen2->Set(true); #else gripOpen1->Set(true); gripOpen2->Set(false); #endif } if(GRIPPERPOP && !popStage && goalGripSP == 0 && !(GRIPPEROPEN && GRIPPERCLOSE)) popStage = 1; if(popStage) popTime = GetClock(); if(popStage == 1) { //popCount = count; popStart = GetClock(); popStage++; //printf("POP START POP START POP START \n"); } if(popStage == 2) { #if !(INNERGRIP) gripOpen1->Set(true); gripOpen2->Set(false); #else gripOpen1->Set(false); gripOpen2->Set(true); #endif popStage++; //printf("GRIP OPEN GRIP OPEN GRIP OPEN \n"); } if(popStage == 3 && popTime - popStart > 0.0) //used to be 0.15 { gripPop1->Set(true); gripPop2->Set(false); popStage++; //printf("POP OUT POP OUT POP OUT \n"); } if(popStage == 4 && popTime - popStart > .75) //time was 0.9 { gripPop1->Set(false); gripPop2->Set(true); popStage++; //printf("POP IN POP IN POP IN \n"); } if(popStage == 5 && popTime - popStart > 0.9) popStage = 0; //time was 1.05 if(MINIBOTSWITCH && !(MODESWITCH && stickR.GetRawButton(1) && stickL.GetRawButton(1))) miniDep->Set(miniDep->kReverse); else if(MINIBOTSWITCH && MODESWITCH && stickR.GetRawButton(1) && stickL.GetRawButton(1)) miniDep->Set(miniDep->kOn); #endif if(!compSwitch->Get()) compressor->Set(compressor->kReverse); else compressor->Set(compressor->kOff); /* if(stickR.GetRawButton(1)) compressor->Set(compressor->kReverse); else compressor->Set(compressor->kForward); */ Wait(0.02); // wait for a motor update time } }
void OperatorControl() { compressor.Start(); myRobot.SetSafetyEnabled(true); myRobot.SetInvertedMotor(myRobot.kRearLeftMotor, true);//Invert rear left Motor myRobot.SetInvertedMotor(myRobot.kRearRightMotor, true);//Invert rear right motor myRobot.SetInvertedMotor(myRobot.kFrontLeftMotor, true);//Invert rear right motor myRobot.SetInvertedMotor(myRobot.kFrontRightMotor, true); DriverStation *ds; DriverStationLCD *dsLCD; ds = DriverStation::GetInstance(); dsLCD = DriverStationLCD::GetInstance(); dsLCD->Printf(DriverStationLCD::kUser_Line1,1, "Starting Teleop"); dsLCD->UpdateLCD(); while (true) { if (!compressor.GetPressureSwitchValue()){ compressor.Start(); } myRobot.ArcadeDrive(stick); /*PNEUMATIC CODE*/ if (stick.GetRawButton(8)){ shooterSole.Set(shooterSole.kForward); } if (stick.GetRawButton(1)){ shooterSole.Set(shooterSole.kReverse); } /*SHOOTER CODE*/ if (stick.GetRawButton(2)){ shooter.SetSpeed(1); } if (stick.GetRawButton(4)){ shooter.SetSpeed(-1); dsLCD->Printf(DriverStationLCD::kUser_Line1,(int)forklift.Get(), "Shooter Should be negative"); dsLCD->UpdateLCD(); } if (!stick.GetRawButton(4) || !stick.GetRawButton(2)){ shooter.SetSpeed(0); } /* FORKLIFT CODE*/ if (!stick.GetRawButton(5) || !stick.GetRawButton(6)){ forklift.SetSpeed(0); } if (stick.GetRawButton(5)){ forklift.SetSpeed(1); } if (stick.GetRawButton(6)){ forklift.SetSpeed(-1); dsLCD->Printf(DriverStationLCD::kUser_Line1,(int)forklift.Get(), "Forklift Should be negative"); dsLCD->UpdateLCD(); } if (!shoot.Get()){ shooter.SetSpeed(0); shooterSole.Set(shooterSole.kForward); } if (stick.GetRawButton(11)){ //myRobot.m_rearLeftMotor.SetSpeed(1); //myRobot.m_rearRightMotor.SetSpeed(1); //myRobot.m_frontLeftMotor.SetSpeed(1); //myRobot.m_frontRightMotor.SetSpeed(1); } //Wait(0.005);// wait for a motor update time } }