// During every loop intervel of the teleop period void Robot::TeleopPeriodic() { // Tank drive, both left and right joystick control their respective motor along the // joystick's 'y' axis //myRobot.TankDrive(-rStick.GetRawAxis(RIGHT_STICK_Y), -lStick.GetRawAxis(LEFT_STICK_Y)); //myRobot.TankDrive(-gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_Y), -gamePad.GetRawAxis(GAMEPAD_RIGHT_STICK_Y)); // Choose the teleop drive option DriverControl(driveOption); // Edits the gyro data to account for drift /* editedGyroRate = gyro.GetRate(); if (gyro.GetRate() > GYRO_DRIFT_VALUE_MIN && gyro.GetRate() < GYRO_DRIFT_VALUE_MAX) { editedGyroRate = 0; } else { editedGyroRate += GYRO_DRIFT_VALUE_AVERAGE; } */ editedGyroAngle = gyro.GetAngle(); SmartDashboard::PutString("Operation Type:", "TeleOp"); // Print gyro data if (showGyro == true) { SmartDashboard::PutNumber("Gyro Angle (Raw)", gyro.GetAngle()*GYRO_SCALE_FACTOR); SmartDashboard::PutNumber("Gyro Angle (Edited)", editedGyroAngle*GYRO_SCALE_FACTOR); SmartDashboard::PutNumber("Gyro Rate (Raw)", gyro.GetRate()*GYRO_SCALE_FACTOR); SmartDashboard::PutNumber("Gyro Rate (Edited)", editedGyroRate*GYRO_SCALE_FACTOR); } // Print out the encoder data // Raw encoder data if (showEncoderRaw == true) { SmartDashboard::PutNumber("Encoder L get raw", encoder1.GetRaw()); SmartDashboard::PutNumber("Encoder R get raw", encoder2.GetRaw()); } // The delta encoder change if (showEncoderRate == true) { SmartDashboard::PutNumber("Encoder L get rate", encoder1.GetRate()); SmartDashboard::PutNumber("Encoder R (reversed) get rate", encoder2.GetRate()); } // Print the encoder index if (showEncoderIndex == true) { SmartDashboard::PutNumber("Encoder L index", encoder1.GetFPGAIndex()); SmartDashboard::PutNumber("Encoder R index", encoder2.GetFPGAIndex()); } }
// During every loop intervel of the teleop period void Robot::TeleopPeriodic() { // Tank drive, both left and right joystick control their respective motor along the // joystick's 'y' axis //myRobot.TankDrive(-rStick.GetRawAxis(RIGHT_STICK_Y), -lStick.GetRawAxis(LEFT_STICK_Y)); // Choose the teleop drive option for(int c=0;c<7;c++) { buttonDone[c] = false; } DriverControl(driveOption); if(fabs(gyro.GetRate()) > GYRO_DRIFT_VALUE) { editedGyroRate = gyro.GetAngle(); } else { editedGyroRate = 0; gyro.Reset(); } SmartDashboard::PutString("Operation Type:", "TeleOp"); // Print gyro data if (showGyro == true) { SmartDashboard::PutNumber("Gyro Angle", gyro.GetAngle()*GYRO_SCALE_FACTOR); SmartDashboard::PutNumber("Gyro Rate (Raw)", gyro.GetRate()*GYRO_SCALE_FACTOR); SmartDashboard::PutNumber("Gyro Rate (Edited)", editedGyroRate*GYRO_SCALE_FACTOR); } // Print out the encoder data // Raw encoder data if (showEncoderRaw == true) { SmartDashboard::PutNumber("Encoder L get raw", encoder1.GetRaw()); SmartDashboard::PutNumber("Encoder R get raw", encoder2.GetRaw()); } // The delta encoder change if (showEncoderRate == true) { SmartDashboard::PutNumber("Encoder L get rate", encoder1.GetRate()); SmartDashboard::PutNumber("Encoder R (reversed) get rate", encoder2.GetRate()); } // Print the encoder index if (showEncoderIndex == true) { SmartDashboard::PutNumber("Encoder L index", encoder1.GetFPGAIndex()); SmartDashboard::PutNumber("Encoder R index", encoder2.GetFPGAIndex()); } }
void OperatorControl(void) { NetTest(); return; myRobot.SetSafetyEnabled(true); digEncoder.Start(); const double ppsTOrpm = 60.0/250.0; //Convert from Pos per Second to Rotations per Minute by multiplication // (See the second number on the back of the encoder to replace 250 for different encoders) const float VoltsToIn = 41.0; // Convert from volts to cm by multiplication (volts from ultrasonic). // This value worked for distances between 1' and 10'. while (IsOperatorControl()) { if (stick.GetRawButton(4)) { myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), -1); } else if (stick.GetRawButton(5)) { myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), 1); } else { myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), 0); } myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), 0); SmartDashboard::PutNumber("Digital Encoder RPM", abs(digEncoder.GetRate()*ppsTOrpm)); SmartDashboard::PutNumber("Ultrasonic Distance inch", (double) ultra.GetAverageVoltage()*VoltsPerInch); SmartDashboard::PutNumber("Ultrasonic Voltage", (double) ultra.GetAverageVoltage()); Wait(0.1); } digEncoder.Stop(); }
/** * Get the current rate of the encoder. * Units are distance per second as scaled by the value from SetEncoderDistancePerPulse(). * * @return The current rate of the encoder. * * @param aSlot The digital module slot for the A Channel on the encoder * @param aChannel The channel on the digital module for the A Channel of the encoder * @param bSlot The digital module slot for the B Channel on the encoder * @param bChannel The channel on the digital module for the B Channel of the encoder */ double GetEncoderRate(UINT32 aSlot, UINT32 aChannel, UINT32 bSlot, UINT32 bChannel) { Encoder *encoder = AllocateEncoder(aSlot, aChannel, bSlot, bChannel); if (encoder != NULL) return encoder->GetRate(); else return 0.0; }
void OperatorControl(void) { digEncoder.Start(); const double ppsTOrpm = 60.0/250.0; //This converts from Pos per Second to Rotations per Minute (See second number on back of encoder to replace 250 if you need it) while (IsOperatorControl()) { SmartDashboard::PutNumber("Digital Encoder RPM", digEncoder.GetRate()*ppsTOrpm); Wait(0.1); } digEncoder.Stop(); }
/** * Runs the motors with arcade steering. */ void OperatorControl(void) { encoder.Start(); while (IsEnabled()) { float controlv = control.GetVoltage(); motor.Set(controlv/5); //get's voltage from control and divides it by 5 double rate = encoder.GetRate(); double distance = encoder.GetDistance(); printf ("%f %f \n", rate, distance); } }
// Called repeatedly when this Command is scheduled to run void DriveStraightTuning::Execute() { DriveSubsystem *driveSub = Robot::driveSubsystem; Encoder *leftEnc = Robot::driveSubsystem->leftEncoder; Encoder *rightEnc = Robot::driveSubsystem->rightEncoder; float leftRate = fabs(leftEnc->GetRate()); float rightRate = fabs(rightEnc->GetRate()); SmartDashboard::PutNumber("leftRate:", leftRate); SmartDashboard::PutNumber("rightRate:", rightRate); driveSub->theDriveTrain->Drive(0.5,0.0); // SmartDashboard::PutNumber("Gryo Angle", driveSub->driveGyro->GetAngle()); // _drive_p = SmartDashboard::GetNumber("Drive P"); // _drive_i = SmartDashboard::GetNumber("Drive i"); // _drive_d = SmartDashboard::GetNumber("Drive d"); // _straight_p = SmartDashboard::GetNumber("Straight P"); // _straight_i = SmartDashboard::GetNumber("Straight i"); // _straight_d = SmartDashboard::GetNumber("Straight d"); // _distance = SmartDashboard::GetNumber("Distance"); // _enabled = SmartDashboard::GetBoolean("Enable"); // static bool prevEnabled = false; // if ((_enabled == true) && (prevEnabled == false)) // { // PIDController *drive = driveSub->driveDistancePID; // PIDController *straight = driveSub->driveStraightPID; // drive->SetPID(_drive_p,_drive_i,_drive_d); // straight->SetPID(_straight_p,_straight_i,_straight_d); // driveSub->DriveStraight(_distance); // } // else if (_enabled == false) // { // driveSub->Cancel(); // prevEnabled = false; // } }
// During every loop intervel of the teleop period void Robot::TeleopPeriodic() { // Choose the teleop drive option DriverControl(driveOption); // A simple thing to test a motor if (gamePad.GetRawButton(GAMEPAD_BUTTON_A) == true) ballManipulator.GoUp(); else ballManipulator.StopAll(); // Edits the gyro rate to account for drift /* editedGyroRate = gyro.GetRate(); if (gyro.GetRate() > GYRO_DRIFT_VALUE_MIN && gyro.GetRate() < GYRO_DRIFT_VALUE_MAX) { editedGyroRate = 0; } else { editedGyroRate += GYRO_DRIFT_VALUE_AVERAGE; } */ // Edits the gyro angle to account for drift if (fabs(gyro.GetRate()) > GYRO_DRIFT_VALUE) editedGyroAngle = gyro.GetAngle(); else { gyro.Reset(); editedGyroAngle = 0; } // Reset the buttons for(int c=0; c<7; c++) buttonDone[c] = false; // To reset encoder data for the wheels if (gamePad.GetRawButton(GAMEPAD_BUTTON_Y) == true) { encoder1.Reset(); encoder2.Reset(); } // Print gyro data if (showGyro == true) { SmartDashboard::PutNumber("Gyro Angle (Raw)", gyro.GetAngle()*GYRO_SCALE_FACTOR); SmartDashboard::PutNumber("Gyro Angle (Edited)", editedGyroAngle*GYRO_SCALE_FACTOR); SmartDashboard::PutNumber("Gyro Rate (Raw)", gyro.GetRate()*GYRO_SCALE_FACTOR); SmartDashboard::PutNumber("Gyro Rate (Edited)", editedGyroRate*GYRO_SCALE_FACTOR); } // Print out the encoder data // Raw encoder data // if (showEncoderRaw == true) { SmartDashboard::PutNumber("Encoder L get raw", encoder1.GetRaw()); SmartDashboard::PutNumber("Encoder R get raw", encoder2.GetRaw()); // } // The delta encoder change if (showEncoderRate == true) { SmartDashboard::PutNumber("Encoder L get rate", encoder1.GetRate()); SmartDashboard::PutNumber("Encoder R (reversed) get rate", encoder2.GetRate()); } // Print the encoder index if (showEncoderIndex == true) { SmartDashboard::PutNumber("Encoder L index", encoder1.GetFPGAIndex()); SmartDashboard::PutNumber("Encoder R index", encoder2.GetFPGAIndex()); } }
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 write() { myfile<<GetTime()<<","<<theEncoder->GetDistance()<<","<<theEncoder->GetRate()<<",\n"; }
void OperatorControl(void) { //Open the file //setup watchdog GetWatchdog().SetEnabled(true); GetWatchdog().SetExpiration(0.3); theEncoder->Start(); //theLift->Enable(); #if RATE #endif float maxHeight = 0; float maxRate = 0; while (IsOperatorControl()) { printf("Distance %f rate %f maxHeight %f maxRate %f\n", theEncoder->GetDistance(), theEncoder->GetRate(), maxHeight, maxRate); if(writing) { write(); }else if (!closed){ printf("CLOSED!"); myfile.close(); closed = true; } Wait(.01); if(theEncoder->GetDistance() > maxHeight) maxHeight = theEncoder->GetDistance(); if(theEncoder->GetRate() > maxRate) maxRate = theEncoder->GetRate(); //check whether we should switch modes /*if (decelToStop(theEncoder->GetRate(),SETPOINT_METERS - theEncoder->GetDistance()) <= MAXDECEL && !decel) { decelRate = decelToStop(theEncoder->GetRate(),SETPOINT_METERS - theEncoder->GetDistance()); cout<<"DecelRate: "<<decelRate<<endl; decel = true; printf("BEGINNING DECELERATION\n"); initTime = GetTime(); }*/ /*if(theEncoder->GetDistance() > 1.0) { theJag->Set(0.0); writing = false; //theLift->SetPID(.3,0,0); //theLift->SetSetpoint(0.0); done = true; }else if(!done){ theJag->Set(1.0); //theLift->SetSetpoint(1.5); } */ if(theEncoder->GetDistance() >= SETPOINT_METERS) { done = true; theJag->Set(0.0); writing = false; }else if (!done){ theJag->Set(1.0); } rateSetpoint = vFinal + (decelRate * (GetTime() / pow(10,9))); #if 0 if(theEncoder->GetDistance() >= SETPOINT_METERS) { theJag->Set(0.0); writing = false; }else if(decel){ #if RATE //theLift->SetSetpoint(rateSetpoint); theJag->Set(0.0); #else theLift->SetSetpoint(SETPOINT_METERS); #endif }else{ theJag->Set(1.0); } #if RATE #else theLift->SetSetpoint(SETPOINT_METERS); #endif #endif GetWatchdog().Feed(); } }