/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void)
{
HSLImage *Himage;
Threshold targetThreshold(247, 255, 60, 140, 10, 50);
BinaryImage *matchingPixels;
vector<ParticleAnalysisReport> *pReport;
//myRobot->SetSafetyEnabled(true);
Saftey->SetEnabled(false);
AxisCamera &mycam = AxisCamera::GetInstance("10.15.10.11");
mycam.WriteResolution(AxisCamera::kResolution_640x480);
mycam.WriteCompression(20);
mycam.WriteBrightness(25);
Wait(3.0);
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Clear();
float X[2];
float Y[2];
float Z[2];
while(IsOperatorControl())
{
X[1] = Stick1->GetX();
X[2] = Stick2->GetX();
Y[1] = Stick1->GetY();
Y[2] = Stick2->GetY();
Z[1] = Stick1->GetZ();
Z[2] = Stick2->GetZ();
Jaguar1->Set(Y[1]);
Jaguar2->Set(Y[2]);
Wait(0.005);
if (mycam.IsFreshImage())
{
Himage = mycam.GetImage();
matchingPixels = Himage->ThresholdHSL(targetThreshold);
pReport = matchingPixels->GetOrderedParticleAnalysisReports();
for (unsigned int i = 0; i < pReport->size(); i++)
{
printf("Index: %d X Center: %d Y Center: %d \n", i, (*pReport)[i].center_mass_x, (*pReport)[i].center_mass_y);
}
delete Himage;
delete matchingPixels;
delete pReport;
}
}
//myRobot->ArcadeDrive(stick); // drive with arcade style (use right stick)
//Wait(0.005); // wait for a motor update time
}
/**
* Get the PWM value directly from the hardware.
*
* Read a raw value from a PWM channel.
*
* @param slot The slot the digital module is plugged into
* @param channel The PWM channel connected to this speed controller
* @return Raw PWM control value. Range: 0 - 255.
*/
UINT8 GetJaguarRaw(UINT32 slot, UINT32 channel)
{
Jaguar *jaguar = (Jaguar *) AllocatePWM(slot, channel, CreateJaguarStatic);
if (jaguar)
return jaguar->GetRaw();
else
return 0;
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void)
{
left1.Set(leftStick.GetY());
left2.Set(leftStick.GetY());
right1.Set(rightStick.GetY());
right2.Set(rightStick.GetY());
}
/**
* Get the PWM value directly from the hardware.
*
* Read a raw value from a PWM channel.
*
* @param channel The PWM channel connected to this speed controller
* @return Raw PWM control value. Range: 0 - 255.
*/
UINT8 GetJaguarRaw(UINT32 channel)
{
Jaguar *jaguar = (Jaguar *) AllocatePWM(SensorBase::GetDefaultDigitalModule(), channel, CreateJaguarStatic);
if (jaguar)
return jaguar->GetRaw();
else
return 0;
}
void bridgeboot (bool onoff)
{
if (onoff == true)
{
BridgeBootMotor10->SetSpeed(-1.0);
}
else
{
BridgeBootMotor10->SetSpeed(0.5);
}
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl()
{
myRobot.SetSafetyEnabled(true);
while (IsOperatorControl() && IsEnabled())
{
leftIntake.Set(0.8);
rightIntake.Set(0.8);
leftLift.Set(operatorStick.GetRawAxis(1));
rightLift.Set(operatorStick.GetRawAxis(1));
myRobot.TankDrive(driverStick.GetRawAxis(5), driverStick.GetRawAxis(1), true); // drive with arcade style (use right stick)
Wait(0.005); // wait for a motor update time
}
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void)
{
GetWatchdog().SetEnabled(true);
compressor->Start();
GetWatchdog().SetExpiration(0.5);
bool valve_state = false;
while (IsOperatorControl())
{
motor->Set(stick->GetY());
if (stick->GetRawButton(1) && !valve_state)
{
valve->Set(true);
valve_state = true;
}
if (!stick->GetRawButton(1) && valve_state)
{
valve->Set(false);
valve_state = false;
}
// Update driver station
//dds->sendIOPortData(valve);
GetWatchdog().Feed();
}
}
void Reset() {
m_talonCounter->Reset();
m_victorCounter->Reset();
m_jaguarCounter->Reset();
m_talon->Set(0.0f);
m_victor->Set(0.0f);
m_jaguar->Set(0.0f);
}
void ballgatherer (bool onoff, bool reverse)
{
if (kicker_in_motion == true){
BallGathererMotor9->SetSpeed(0.0);
return;
}
if (onoff == true){
if (reverse == true) {
BallGathererMotor9->SetSpeed(0.8);
}
else {
BallGathererMotor9->SetSpeed(-1.0);
}
}
else{
BallGathererMotor9->SetSpeed(0.0);
}
//printf ("%d %f ballgatherer onoff:\n");
}
void AutonomousInit(void) {
m_autoPeriodicLoops = 0; // Reset the loop counter for autonomous mode
// ClearSolenoidLEDsKITT();
m_feeding = true;
m_feedCount = 50; // (loops)
m_feedSpeed = 0.5; // (unitless)
m_feedCounter = 0;
m_loading = true;
m_loadCount = 60; // (loops)
m_loadPauseCount = 2; // (loops)
m_loadSpeed = 0.2; // (unitless)
m_loadCounter = 0;
m_frisbeeCounter = 0; // (loops) frisbee counter for autonomous
m_frisbeeCountMax = 3; // (loops) maximum number of frisbees we're going to feed in autonomous
m_breachFrisbee = true;
m_robot->SetSafetyEnabled(false);
// m_robotFrontDrive->SetSafetyEnabled(false);
// m_robotRearDrive->SetSafetyEnabled(false);
m_launchMotor->SetSafetyEnabled(false);
m_feedMotor->SetSafetyEnabled(false);
}
/**
* 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);
}
}
void TeleopInit(void) {
m_telePeriodicLoops = 0; // Reset the loop counter for teleop mode
m_dsPacketsReceivedInCurrentSecond = 0; // Reset the number of dsPackets in current second
// m_driveMode = UNINITIALIZED_DRIVE; // Set drive mode to uninitialized
// ClearSolenoidLEDsKITT();
m_feeding = false;
m_feedCount = 50; // (loops)
m_feedSpeed = 0.5; // (unitless)
m_feedCounter = 0;
m_loading = false;
m_loadCount = 58; // (loops)
m_loadPauseCount = 2; // (loops)
m_loadSpeed = 0.2; // (unitless)
m_loadCounter = 0;
m_robot->SetSafetyEnabled(false); // motor safety timers disabled as workaround for timeout problems on cRIO 2
// m_robotFrontDrive->SetSafetyEnabled(false);
// m_robotRearDrive->SetSafetyEnabled(false);
m_launchMotor->SetSafetyEnabled(false);
m_feedMotor->SetSafetyEnabled(false);
m_loadMotor->SetSafetyEnabled(false);
m_elevatorMotor->SetSafetyEnabled(false);
}
void ShooterControl (bool onoff,double power) //function for the shooter
{
if (onoff = true)
{
shooter1->SetSpeed(-power);
shooter2->SetSpeed(-power);
shooter3->SetSpeed(power);
shooter4->SetSpeed(power);
}
else
{
shooter1->SetSpeed(0.0);
shooter2->SetSpeed(0.0);
shooter3->SetSpeed(0.0);
shooter4->SetSpeed(0.0);
}
}
void DriveAutonomously(float Speed, float Curve) {
if (Curve < 0) //Right Turn
{
LeftDriveJaguar->Set(Speed);
RightDriveJaguar->Set(Speed - (Curve * Speed));
}
if (Curve < 0) //Left Turn
{
LeftDriveJaguar->Set(Speed - (Curve * Speed));
RightDriveJaguar->Set(Speed);
}
if (Curve == 0) //Straight
{
LeftDriveJaguar->Set(Speed);
RightDriveJaguar->Set(Speed);
}
}
void Disabled(void)
{
bool stopped = false;
while(!IsOperatorControl() && !IsAutonomous())
{
if(!stopped)
{
frontLeftMotor->Set(0.0);
rearLeftMotor->Set(0.0);
frontRightMotor->Set(0.0);
rearRightMotor->Set(0.0);
liftMotor1->Set(0.0);
liftMotor2->Set(0.0);
gripMotor1->Set(0.0);
//gripMotor2->Set(0.0);
PIDLift->Reset();
PIDDriveLeft->Reset();
PIDDriveRight->Reset();
PIDGrip->Reset();
stopped = true;
}
}
}
/**
* Set the PWM value directly to the hardware.
*
* Write a raw value to a PWM channel.
*
* @param slot The slot the digital module is plugged into
* @param channel The PWM channel connected to this speed controller
* @param value Raw PWM value. Range 0 - 255.
*/
void SetJaguarRaw(UINT32 slot, UINT32 channel, UINT8 value)
{
Jaguar *jaguar = (Jaguar *) AllocatePWM(slot, channel, CreateJaguarStatic);
if (jaguar) jaguar->SetRaw(value);
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void)
{
myRobot->SetSafetyEnabled(true);
while (IsOperatorControl())
{
bool setLimit;
double cimValue1= -scaleThrottle(-(stick->GetZ())); //Set desired speed from the Throttle, assuming from -1 to 1, also invert the cim, since we want it to rotate coutnerclockwise/clockwise
double cimValue2= scaleThrottle(-(stick->GetZ())); //Set desired speed from the Throttle, assuming from -1 to 1
//For shooter
/*if (stick.GetRawButton(1) == true) {
//back of the robot is moved forward by pushing forward on the joystick
myRobot.ArcadeDrive((stick.GetY()),
(stick.GetX()), false); // inverted drive control
} else {
//front of the robot is moved forward by pushing forward on the joystick
myRobot.ArcadeDrive(-(stick.GetY()),
(stick.GetX()), false); // normal drive control
}
*/
//For manual button speed control, this sets the speed
if (stick->GetRawButton(4) == true) {
cim1->Set(cimValue1); //use the value from the throttle to set cim speed
cim2->Set(cimValue2);//Get speed from throttle, and then scale it
setLimit = true;
//Open Ball Stopper
}
else {
cim1->Set(0.0);
cim2->Set(0.0);
setLimit = false;
//Close Ball Stopper
}
//For precisebelt pickup
if (stick->GetRawButton(1) == true) {
//back of the robot is moved forward by pushing forward on the joystick
if (setLimit == true) {
JagBelt->ArcadeDrive(0.1,
(stick->GetX()), false); // inverted drive control
} else {
JagBelt->ArcadeDrive((stick->GetY()),
(stick->GetX()), false); // inverted drive control
}
} else {
//front of the robot is moved forward by pushing forward on the joystick
if (setLimit == true) {
JagBelt->ArcadeDrive(-0.1,
(stick->GetX()), false); // inverted drive control
} else {
JagBelt->ArcadeDrive(-(stick->GetY()),
(stick->GetX()), false); // inverted drive control
}
}
//For normal belt pickup
/*if (stick->GetRawButton(6) == true) {
JagBelt->Drive(1.0, 0); //opens gripper
} else {
JagBelt->Drive(0.0, 0); //closes gripper
}
*/
//For drive
if (rightstick->GetRawButton(1) == true) {
//back of the robot is moved forward by pushing forward on the joystick
if (rightstick->GetRawButton(10) == true) {
precisionMode= true;
}
else {
precisionMode= false;
}
myRobot->ArcadeDrive((rightstick->GetY()), (rightstick->GetX()), precisionMode); // inverted drive control
} else
{
if (rightstick->GetRawButton(10) == true) {
precisionMode= true;
}
else {
precisionMode= false;
}
//front of the robot is moved forward by pushing forward on the joystick
myRobot->ArcadeDrive(-(rightstick->GetY()), (rightstick->GetX()), precisionMode); // normal drive control
}
/*if (stick->GetRawButton(8) == true) {
cim1->Set(-0.37); //run cim 1 at 50% speed counterclockwise??
cim2->Set(0.37); // run cim at 50% speed clockwise
check = true; //indicate if motors are running
} else if (check == true){ // if motors are running a
Wait(2.0);
belt->Set(1); // run the belt
Wait(2.0); // one sec delay
belt->Set(0.0); // turn belt off
check = false; // put new check
}
else if (check == false){ //if false
// 2 sec delay to wait for the first ball to shoot
cim1->Set(0.0); //stop cims
cim2->Set(0.0);
}
else { // Stop everything
cim1->Set(0.0); //stop cims
cim2->Set(0.0);
belt->Set(0.0);
}
*/
//Code for using window motor
if (rightstick->GetRawButton(4)) {
window->Set(Relay::kOn);
window->Set(Relay::kForward); // tell window motor to go forward
} else if (rightstick->GetRawButton(5) == true){
window->Set(Relay::kOn);
window->Set(Relay::kReverse); //tell window motor to go backward
}
else {
//Wait(1.0);
window->Set(Relay::kOff); //turn it off, if the relays aint being used
}
//Code for Banebot Motor for stopping ballz
if (stick->GetRawButton(2) == true) { // press button TWO to close
pickStop->Set(-0.5); //closes ball stopper
Wait(1);
pickStop->Set(0.0);
} else if (stick->GetRawButton(3) == true){ //press button three to open
pickStop->Set(0.5); //opens ball stopper
Wait(1.2); // too slow, so needs more time
pickStop->Set(0.0);
}
else if (stick->GetRawButton(5)== true){ //press 5 to stop imediately, useful for adjusting angles...
//Wait(1.0);
pickStop->Set(0.0);
}
//Code for ... servoooo
if (stick->GetRawButton(10) == true) { //press 10 on the left stick...
bar->SetAngle(60); // set the angles to 60...clockwise?
bar2->SetAngle(60);
} else if (stick->GetRawButton(11) == true) // press 11 on the left stick
{
bar->SetAngle(-60); //set the angles to -60...counterclockwise?
bar2->SetAngle(-60);
}
// Initialize functions...
// RelayServo();
//PreciseBelt();
//UltrasonicRange();
// Accelerometer();
//dash->GetPacketNumber();
// send data back to dashboard
dash->GetPacketNumber(); //not sure why this is here 0_0
//int limitValue= limitSwitch->Get() ? 1 : 0; // retrieve 1 and 0 only.../ look for 0 and 1
float servoAngle = bar->GetAngle();
//dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Limit State: %d", limitValue); //send data back to driver station...
// dsLCD->Printf(DriverStationLCD::kUser_Line2, 2, "Servo Angle: %f", servoAngle); //send data back to driver station...
float gyroVal = gyro -> GetVoltage();//Gets voltage from gyro
float ultraVal = rangeFinder -> GetVoltage(); //Get voltage from ultrasonic sensor
float tempVal = Temperature -> GetVoltage();//Gets temperature
//Do the math to convert data received from the ultrasonic volts->miliVolts->milivolts per inch->inches
float Vm = (ultraVal*1000);
float Ri = (Vm/9.765625);
// Print data back to dashboard
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Ultrasonic Range: %f",Ri);
dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, "Gyro: %f", gyroVal);
dsLCD->Printf(DriverStationLCD::kUser_Line3, 1, "Temperature: %f", tempVal);
dsLCD->Printf(DriverStationLCD::kUser_Line4, 1, "Cim1 Speed: %f%%", (cimValue1*100)); //display speed that the mototrs are reunning at different percentages...
dsLCD->Printf(DriverStationLCD::kUser_Line5, 1, "Cim2 Speed: %f%%", (cimValue2*100));
//Update dashboard
dsLCD->UpdateLCD();
}
}
/**
* Set the PWM value directly to the hardware.
*
* Write a raw value to a PWM channel.
*
* @param channel The PWM channel connected to this speed controller
* @param value Raw PWM value. Range 0 - 255.
*/
void SetJaguarRaw(UINT32 channel, UINT8 value)
{
Jaguar *jaguar = (Jaguar *) AllocatePWM(SensorBase::GetDefaultDigitalModule(), channel, CreateJaguarStatic);
if (jaguar) jaguar->SetRaw(value);
}
/**
* Set the PWM value.
*
* The PWM value is set using a range of -1.0 to 1.0, appropriately
* scaling the value for the FPGA.
*
* @param channel The PWM channel connected to this speed controller
* @param speed The speed value between -1.0 and 1.0 to set.
*/
void SetJaguarSpeed(UINT32 channel, float speed)
{
Jaguar *jaguar = (Jaguar *) AllocatePWM(SensorBase::GetDefaultDigitalModule(), channel, CreateJaguarStatic);
if (jaguar) jaguar->Set(speed);
}
/**
* Set the PWM value.
*
* The PWM value is set using a range of -1.0 to 1.0, appropriately
* scaling the value for the FPGA.
*
* @param slot The slot the digital module is plugged into
* @param channel The PWM channel connected to this speed controller
* @param speed The speed value between -1.0 and 1.0 to set.
*/
void SetJaguarSpeed(UINT32 slot, UINT32 channel, float speed)
{
Jaguar *jaguar = (Jaguar *) AllocatePWM(slot, channel, CreateJaguarStatic);
if (jaguar) jaguar->Set(speed);
}
/******************************************************
* Drive left & right motors for 2 seconds then stop *
******************************************************/
void Autonomous(void)
{
//increase 2nd RPM
compressor.Start(); // starts the compressor;
bool BRIDGE = bridge.Get();
bool HIGH = high.Get();
bool MIDDLE = middle.Get();
bool TWOSEC = twosec.Get();
bool FIVESEC = fivesec.Get();
int highRPM = 1800; // 1st 1800 short about 5 ft
int secondhighRPM = 1800; //1st 1400 (did not fire)
int rpmForShooter;
DriverStationLCD *dslcd = DriverStationLCD::GetInstance(); // don't press SHIFT 5 times; this line starts up driver station messages (in theory
char debugout [100];
// baddog.SetExpiration(30.0);
// baddog.Feed();
dslcd->Clear();
sprintf(debugout,"Bridge=%u",BRIDGE);
dslcd->Printf(DriverStationLCD::kUser_Line1,1,debugout);
sprintf(debugout,"High=%u",HIGH);
dslcd->Printf(DriverStationLCD::kUser_Line2,2,debugout);
sprintf(debugout,"Middle=%u",MIDDLE);
dslcd->Printf(DriverStationLCD::kUser_Line3,3,debugout);
sprintf(debugout,"TwoSec=%u",TWOSEC);
dslcd->Printf(DriverStationLCD::kUser_Line4,4,debugout);
sprintf(debugout,"FiveSec=%u",FIVESEC);
dslcd->Printf(DriverStationLCD::kUser_Line5,5,debugout);
dslcd->UpdateLCD(); // update the Driver Station with the information in the code */
if (BRIDGE == 0 && HIGH == 0 && MIDDLE == 0 && TWOSEC == 0 && FIVESEC == 0)
{
myRobot.Drive(0.0, 0.0);
Wait(10.0);
}
if (BRIDGE == 1 && HIGH == 0 && MIDDLE == 0 && TWOSEC == 0 && FIVESEC == 0) //1700 RPM
{
/*myRobot.Drive(-0.5, 0.0);
Wait(3.0);
*/
flashring.Set(Relay::kForward);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<7)
{
shooter.setTargetRPM(1700);
//wait-0.01
Wait(0.005);
}
lynx.Set(-1.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<1)
{
shooter.setTargetRPM(1700);
Wait(0.005);
}
turret.Set(-0.05);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<0.2)
{
shooter.setTargetRPM(1700);
Wait(0.005);
}
turret.Set(0.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(1700);
Wait(0.005);
}
// lynx.Set(0.0);
shepard.Set(true);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(1700);
Wait(0.005);
}
shepard.Set(false);
shooter.setTargetRPM((int)0);
flashring.Set(Relay::kOff);
lynx.Set(0.0);
}
if (BRIDGE == 1 && HIGH == 1 && MIDDLE == 0 && TWOSEC == 0 && FIVESEC == 0) //main autonomous code-default
{
flashring.Set(Relay::kForward);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<7)
{
shooter.setTargetRPM((int)highRPM);
//wait-0.01
Wait(0.005);
}
lynx.Set(-1.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<1)
{
shooter.setTargetRPM((int)highRPM);
Wait(0.005);
}
turret.Set(-0.05);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<0.2)
{
shooter.setTargetRPM((int)secondhighRPM);
Wait(0.005);
}
turret.Set(0.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM((int)secondhighRPM);
Wait(0.005);
}
// lynx.Set(0.0);
shepard.Set(true);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM((int)secondhighRPM);
Wait(0.005);
}
shepard.Set(false);
shooter.setTargetRPM((int)0);
flashring.Set(Relay::kOff);
lynx.Set(0.0);
// baddog.Feed();
}
if (BRIDGE == 1 && HIGH == 1 && MIDDLE == 0 && TWOSEC == 1 && FIVESEC == 0)
{
//Wait(2.0);
//Robot aims
//Robot shoots
//myRobot.Drive(-0.5, 0.0);
//Wait(3.0);
flashring.Set(Relay::kForward);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<7)
{
shooter.setTargetRPM(1900);
//wait-0.01
Wait(0.005);
}
lynx.Set(-1.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<1)
{
shooter.setTargetRPM(1900);
Wait(0.005);
}
turret.Set(-0.05);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<0.2)
{
shooter.setTargetRPM(1800);
Wait(0.005);
}
turret.Set(0.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(1800);
Wait(0.005);
}
// lynx.Set(0.0);
shepard.Set(true);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(1800);
Wait(0.005);
}
shepard.Set(false);
shooter.setTargetRPM((int)0);
flashring.Set(Relay::kOff);
lynx.Set(0.0);
}
if (BRIDGE == 1 && HIGH == 1 && MIDDLE == 0 && TWOSEC == 0 && FIVESEC == 1)
{
Wait(5.0);
//Robot aims
//Robot shoots
//myRobot.Drive(-0.5, 0.0);
myRobot.Drive(0.0,0.0);
Wait(3.0);
}
if (BRIDGE == 1 && HIGH == 0 && MIDDLE == 1 && TWOSEC == 0 && FIVESEC == 0)
{
//Robot aims
//Robot shoots
myRobot.Drive(-0.5, 0.0);
Wait(3.0);
}
if (BRIDGE == 1 && HIGH == 0 && MIDDLE == 1 && TWOSEC == 1 && FIVESEC == 0)
{
Wait(2.0);
//Robot aims
//Robot shoots
myRobot.Drive(-0.5, 0.0);
Wait(3.0);
}
if (BRIDGE == 1 && HIGH == 0 && MIDDLE == 1 && TWOSEC == 0 && FIVESEC == 5)
{
Wait(5.0);
//Robot aims
//Robot shoots
myRobot.Drive(-0.5, 0.0);
Wait(3.0);
}
if (BRIDGE == 0 && HIGH == 1 && MIDDLE == 0 && TWOSEC == 0 && FIVESEC == 0) //position robot-front of key-low RPMs
{
//Robot aims
//Robot shoots
//myRobot.Drive(-0.5, 0.0);
//Wait(3.0);
flashring.Set(Relay::kForward);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<7)
{
shooter.setTargetRPM(1600);
//wait-0.01
Wait(0.005);
}
lynx.Set(-1.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<1)
{
shooter.setTargetRPM(1600);
Wait(0.005);
}
turret.Set(-0.05);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<0.2)
{
shooter.setTargetRPM(1600);
Wait(0.005);
}
turret.Set(0.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(1600);
Wait(0.005);
}
// lynx.Set(0.0);
shepard.Set(true);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(1600);
Wait(0.005);
}
shepard.Set(false);
shooter.setTargetRPM((int)0);
flashring.Set(Relay::kOff);
lynx.Set(0.0);
}
if (BRIDGE == 0 && HIGH == 1 && MIDDLE == 0 && TWOSEC == 1 && FIVESEC == 0)
{
Wait(2.0);
//Robot aims
//Robot shoots
myRobot.Drive(-0.5, 0.0);
Wait(3.0);
}
if (BRIDGE == 0 && HIGH == 1 && MIDDLE == 0 && TWOSEC == 0 && FIVESEC == 1)
{
Wait(5.0);
//Robot aims
//Robot shoots
myRobot.Drive(-0.5, 0.0);
Wait(3.0);
}
if (BRIDGE == 0 && HIGH == 0 && MIDDLE == 1 && TWOSEC == 0 && FIVESEC == 0)
{
//Robot aims
//Robot shoots
myRobot.Drive(-0.5, 0.0);
Wait(3.0);
}
if (BRIDGE == 0 && HIGH == 0 && MIDDLE == 1 && TWOSEC == 1 && FIVESEC == 0)
{
Wait(2.0);
//Robot aims
//Robot shoots
myRobot.Drive(-0.5, 0.0);
Wait(3.0);
}
if (BRIDGE == 0 && HIGH == 0 && MIDDLE == 1 && TWOSEC == 0 && FIVESEC == 5)
{
Wait(5.0);
//Robot aims
//Robot shoots
myRobot.Drive(-0.5, 0.0);
Wait(3.0);
}
if (BRIDGE == 0 && HIGH == 0 && MIDDLE == 1 && TWOSEC == 1 && FIVESEC == 1)
{
flashring.Set(Relay::kForward);
ShooterTimer.Reset();
ShooterTimer.Start();
targeting.SetLMHTarget(BOGEY_H);
while(ShooterTimer.Get()<2)
{
if (targeting.ProcessOneImage())
{
targeting.ChooseBogey();
targeting.MoveTurret();
rpmForShooter = targeting.GetCalculatedRPM();
shooter.setTargetRPM(rpmForShooter);
Wait(0.01);
}
}
targeting.StopPID();
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get() < 7)
{
shooter.setTargetRPM(rpmForShooter);
Wait(0.005);
}
lynx.Set(-1.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<1)
{
shooter.setTargetRPM(rpmForShooter);
Wait(0.005);
}
turret.Set(-0.05);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<0.2)
{
shooter.setTargetRPM(rpmForShooter);
Wait(0.005);
}
turret.Set(0.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(rpmForShooter);
Wait(0.005);
}
// lynx.Set(0.0);
shepard.Set(true);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(rpmForShooter);
Wait(0.005);
}
shepard.Set(false);
shooter.setTargetRPM((int)0);
flashring.Set(Relay::kOff);
lynx.Set(0.0);
}
if (BRIDGE == 1 && HIGH == 1 && MIDDLE == 1 && TWOSEC == 0 && FIVESEC == 0)
{
flashring.Set(Relay::kForward);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<7)
{
shooter.setTargetRPM(2000); //high RPM
//wait-0.01
Wait(0.005);
}
lynx.Set(-1.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<1)
{
shooter.setTargetRPM(2000); //high RPM
Wait(0.005);
}
turret.Set(-0.05);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<0.2)
{
shooter.setTargetRPM(1800); //low RPM
Wait(0.005);
}
turret.Set(0.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(1800); //low RPM
Wait(0.005);
}
// lynx.Set(0.0);
shepard.Set(true);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(1800); //low RPM
Wait(0.005);
}
shepard.Set(false);
shooter.setTargetRPM((int)0);
flashring.Set(Relay::kOff);
lynx.Set(0.0);
}
//baddog.Feed();
myRobot.SetSafetyEnabled(false);
}
/****************************************
* Runs the motors with arcade steering.*
****************************************/
void OperatorControl(void)
{
//TODO put in servo for lower camera--look in WPI to set
// Watchdog baddog;
// baddog.Feed();
myRobot.SetSafetyEnabled(true);
//SL Earth.Start(); // turns on Earth
// SmartDashboard *smarty = SmartDashboard::GetInstance();
//DriverStationLCD *dslcd = DriverStationLCD::GetInstance(); // don't press SHIFT 5 times; this line starts up driver station messages (in theory)
//char debugout [100];
compressor.Start();
gyro.Reset(); // resets gyro angle
int rpmForShooter;
while (IsOperatorControl()) // while is the while loop for stuff; this while loop is for "while it is in Teleop"
{
// baddog.Feed();
//myRobot.SetSafetyEnabled(true);
//myRobot.SetExpiration(0.1);
float leftYaxis = driver.GetY();
float rightYaxis = driver.GetTwist();//RawAxis(5);
myRobot.TankDrive(leftYaxis,rightYaxis); // drive with arcade style (use right stick)for joystick 1
float random = gamecomponent.GetY();
float lazysusan = gamecomponent.GetZ();
//bool elevator = Frodo.Get();
float angle = gyro.GetAngle();
bool balance = Smeagol.Get();
SmartDashboard::PutNumber("Gyro Value",angle);
int NumFail = -1;
//bool light = Pippin.Get();
//SL float speed = Earth.GetRate();
//float number = shooter.Get();
//bool highspeed = button1.Get()
//bool mediumspeed = button2.Get();
//bool slowspeed = button3.Get();
bool finder = autotarget.Get();
//bool targetandspin = autodistanceandspin.Get();
SmartDashboard::PutString("Targeting Activation","");
//dslcd->Clear();
//sprintf(debugout,"Number=%f",angle);
//dslcd->Printf(DriverStationLCD::kUser_Line2,2,debugout);
//SL sprintf(debugout,"Number=%f",speed);
//SL dslcd->Printf(DriverStationLCD::kUser_Line4,4,debugout);
//sprintf(debugout,"Number=%f",number);
//dslcd->Printf(DriverStationLCD::kUser_Line1,1,debugout);
//sprintf(debugout,"Finder=%u",finder);
//dslcd->Printf(DriverStationLCD::kUser_Line5,5,debugout);
//dslcd->UpdateLCD(); // update the Driver Station with the information in the code
// sprintf(debugout,"Number=%u",maxi);
// dslcd->Printf(DriverstationLCD::kUser_Line6,5,debugout)
bool basketballpusher = julesverne.Get();
bool bridgetipper = joystickbutton.Get();
if (bridgetipper) // if joystick button 7 is pressed (is true)
{
solenoid.Set(true); // then the first solenoid is on
}
else
{
//Wait(0.5); // and then the first solenoid waits for 0.5 seconds
solenoid.Set(false); //and then the first solenoid turns off
}
if (basketballpusher) // if joystick button 6 is pressed (is true)
{
shepard.Set(true); // then shepard is on the run
//Wait(0.5); // and shepard waits for 0.5 seconds
}
else
{
shepard.Set(false); // and then shepard turns off
} //10.19.67.9 IP address of computer;255.0.0.0 subnet mask ALL FOR WIRELESS CONNECTION #2
//}
//cheetah.Set(0.3*lazysusan);
// smarty->PutDouble("pre-elevator",lynx.Get());
lynx.Set(random);
// smarty->PutDouble("elevator",lynx.Get());
// smarty->PutDouble("joystick elevator",random);
if (balance) // this is the start of the balancing code
{
angle = gyro.GetAngle();
myRobot.Drive(-0.03*angle, 0.0);
Wait(0.005);
}
/*if (light) //button 5 turns light on oand off on game controller
flashring.Set(Relay::kForward);
else
flashring.Set(Relay::kOff);
*/
if (finder)
{
flashring.Set(Relay::kForward);
if (button_H.Get()==true)
{
targeting.SetLMHTarget(BOGEY_H);
SmartDashboard::PutString("Targeting","High Button Pressed");
}
if (button_M.Get()==true)
{
targeting.SetLMHTarget(BOGEY_M);
SmartDashboard::PutString("Targeting","Medium Button Pressed");
}
if (button_L.Get()==true)
{
targeting.SetLMHTarget(BOGEY_L);
SmartDashboard::PutString("Targeting","Low Button Pressed");
}
if (button_H.Get()==true || button_M.Get()==true || button_L.Get()==true)
{
if (targeting.ProcessOneImage())
{
NumFail = 0;
SmartDashboard::PutString("Targeting Activation","YES");
targeting.ChooseBogey();
targeting.MoveTurret();
#ifdef USE_HARDWIRED_RPM
shooter.setTargetRPM(HARDWIRED_RPM);
#else
rpmForShooter = targeting.GetCalculatedRPM();
shooter.setTargetRPM(rpmForShooter);
#endif
targeting.InteractivePIDSetup();
}
else
{
NumFail++;
if (NumFail > 10)
targeting.StopPID();
}
SmartDashboard::PutNumber("Numfail", NumFail);
shooter.setTargetRPM(rpmForShooter);
}
else
{
SmartDashboard::PutString("Targeting Activation","NO");
shooter.setTargetRPM(0);
targeting.StopPID();
}
}
else
{
flashring.Set(Relay::kOff);
targeting.StopPID();
turret.Set(lazysusan); // the lazy susan would turn right & left based on how far the person moves the right joystick#2 side to side
//targeting.StopPID();
//if (elevator) //shooter would move at full speed if button is pressed
//TODO Change RPM values
//TODO Disable calculation of RPM values
SmartDashboard::PutNumber("CurrentRPM",shooter.GetCurrentRPM());
if (button_H.Get() == true)
shooter.setTargetRPM((int)2100);
//From front of free throw line, should hit the backboard and go in
//used to be 2700 RPMs
else if (button_M.Get() == true)
shooter.setTargetRPM((int)1900);
//From front of free throw line, should go in the net--can shoot the next ball on the overshoot?
//Used to be 2250 RPMs
else if (button_L.Get() == true)
shooter.setTargetRPM((int)1350);
//From fender, should hit the backboard
//Used to be 2000 RPMs
//shooter.Set(0.5);
else
shooter.setTargetRPM(0);
// else if (mediumspeed)
//shooter.setTargetRPM((int)0);
//else if (slowspeed)
//shooter.setTargetRPM((int)0);
/*if (targetandspin) //code for autotargeting and speed will go here
{
shooter.setTargetRPM((int)1800);
}
else
{*/
//}
myRobot.TankDrive(leftYaxis,rightYaxis);
}
//Wait(0.005);
}
}
void AutonomousPeriodic(void) {
GetWatchdog().Feed();
m_autoPeriodicLoops++;
if (m_frisbeeCounter < m_frisbeeCountMax){
m_launchMotor->Set(0.8);
}
if ((m_breachFrisbee)&& (m_autoPeriodicLoops >= 100)){ //delay for launcher to get speed
if ((m_loading) && (m_loadCounter <= m_loadCount)){ // load breech
m_loadCounter++;
m_loadMotor->Set(m_loadSpeed);
}
else if ((m_loading) && (m_loadCounter > m_loadCount) && \
(m_loadCounter <= ((m_loadCount+m_loadPauseCount)))) { // pause loader
m_loadCounter++;
m_loadMotor->Set(0.); // pause loader
}
else if ((m_loading) && \
(m_loadCounter >= (m_loadCount+m_loadPauseCount)) && \
(m_loaderLimit->Get())) { // loader arm is still retracting
// loader limit switch reads true when not depressed
m_loadMotor->Set(-m_loadSpeed);; // reset loader
}
else {
// not loading, not feeding, finished retracting, ready to feed first frisbee
m_loading = false;
m_loadCounter = 0;
m_loadMotor->Set(0.);; // stop loader
m_breachFrisbee = false;
}
}
else if ((m_frisbeeCounter < m_frisbeeCountMax) && (!m_breachFrisbee)){ //launch all frisbees in the magazine and breach
if (m_feeding && (!m_loaderLimit->Get()) && \
(m_feedCounter <= m_feedCount)){
//* loader limit switch reads true when not depressed *//
//feed a frisbee from magazine
m_feedCounter++;
m_feedMotor->Set(m_feedSpeed);
}
else if (m_feeding && (!m_loaderLimit->Get()) && \
(m_feedCounter > m_feedCount)){ //done feeding a frisbee
m_feeding = false;
m_loading = true;
m_feedCounter = 0;
m_feedMotor->Set(0.);
}
else if ((m_loading && (!m_feeding)) && (m_loadCounter <= m_loadCount)){ // load breech
m_loadCounter++;
m_loadMotor->Set(m_loadSpeed);
}
else if ((m_loading && (!m_feeding)) && (m_loadCounter > m_loadCount) && \
(m_loadCounter <= ((m_loadCount+m_loadPauseCount)))) { // pause loader
m_loadCounter++;
m_loadMotor->Set(0.); // pause loader
}
else if ((m_loading && (!m_feeding)) && \
(m_loadCounter >= (m_loadCount+m_loadPauseCount)) && \
(m_loaderLimit->Get())) { // loader arm is still retracting
// loader limit switch reads true when not depressed
m_loadMotor->Set(-m_loadSpeed);; // reset loader
}
else {
// not loading, not feeding, finished retracting, ready for next frisbee
m_loading = false;
m_loadCounter = 0;
m_loadMotor->Set(0.);; // stop loader
m_frisbeeCounter++;
m_feeding = true;
}
}
else if (m_frisbeeCounter >= m_frisbeeCountMax){ //finished with autonomous
m_launchMotor->Set(0.0);
}
//
// generate KITT-style LED display on the solenoids
// SolenoidLEDsKITT( m_autoPeriodicLoops );
/* the below code (if uncommented) would drive the robot forward at half speed
* for two seconds. This code is provided as an example of how to drive the
* robot in autonomous mode, but is not enabled in the default code in order
* to prevent an unsuspecting team from having their robot drive autonomously!
*/
//* below code commented out for safety
// if (m_autoPeriodicLoops <= (10 * (UINT32)GetLoopsPerSec())) {
/*if (m_autoPeriodicLoops <= 100) {
// When on the first periodic loop in autonomous mode, start driving forwards at half speed
m_robot->Drive(0.25, 0.); // drive forwards at quarter speed
// m_robotFrontDrive->Drive(0.25, 0.25); // drive forwards at quarter speed
// m_robotRearDrive->Drive(0., 0.0); // drive forwards at quarter speed
}
else {
m_robot->Drive(0., 0.); // drive forwards at quarter speed
// m_robotFrontDrive->Drive(0.0, 0.0); // stop robot
// m_robotRearDrive->Drive(0.0, 0.0); // stop robot
}
*/
/* Driver station display output. */
char msg[256];
static DriverStationLCD *dsLCD = DriverStationLCD::GetInstance();
sprintf(msg, "Frisbee Counter = %d", m_frisbeeCounter);
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, msg);
sprintf(msg, "Feeder %s", m_feeding? "ON " : "OFF");
dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, msg);
sprintf(msg, "Feed Counter = %d", m_feedCounter);
dsLCD->Printf(DriverStationLCD::kUser_Line3, 1, msg);
sprintf(msg, "Load Counter = %d", m_loadCounter);
dsLCD->Printf(DriverStationLCD::kUser_Line4, 1, msg);
// line number (enum), starting col, format string, args for format string ...
dsLCD->UpdateLCD();
}
void RobotDemo::RPS_control_code(float desired_RPS)
// THE 360 COUNT PER REVOLUTION ENCODERS ARE GOOD FOR 10K RPM, BUT THE CIM motor MAXIMUM SPEED IS APPROXIMATELY 5K.
{
#if 1
/* If the difference between the old and new RPS (typ ~35) by greater than 1 either way,
* reset the ingegral terms to prevent stale values from affecting our PID calculations
*/
if (desired_RPS != prev_desired_RPS)
{
if (fabs(prev_desired_RPS - desired_RPS) >= 1.0)
{
integral_back = 0;
integral_front = 0;
}
cout << desired_RPS << " " << prev_desired_RPS << endl;
prev_desired_RPS = desired_RPS;
}
#endif
//motor = new Victor(motor_pwm);
//test_encoder = new Encoder(encoder_pwm1, encoder_pwm2, true);
if ((pid_code_timer->Get()) >= pidtime)
{
float actual_time = pid_code_timer ->Get();
prev_error_back = error_back;
prev_error_front = error_front;
RPS_back = (back_shooter_encoder->Get() / 360.0) / actual_time;
RPS_front = (front_shooter_encoder->Get() / 360.0) / actual_time;
error_back = desired_RPS - RPS_back;
error_front = desired_RPS - RPS_front;
integral_back = integral_back + (error_back * actual_time);
integral_front = integral_front + (error_front * actual_time);
derivitive_back = (error_back - prev_error_back) / actual_time;
derivitive_front = (error_front - prev_error_front) / actual_time;
//take error and set it to motors
{
RPS_speed_back = (error_back * first_pterm) + (integral_back
* iterm) + (derivitive_back * dterm);
RPS_speed_front = (error_front * first_pterm) + (integral_front
* iterm) + (derivitive_front * dterm);
}
if (RPS_speed_back > 1)
{
RPS_speed_back = 1;
}
if (RPS_speed_front > 1)
{
RPS_speed_front = 1;
}
if (RPS_speed_back < 0)
{
RPS_speed_back = 0;
}
if (RPS_speed_front < 0)
{
RPS_speed_front = 0;
}
//cout << RPS_speed << endl;
//cout << autonomous_timer->Get() << " RPS_back: " << RPS_back
//<< " RPS_front: " << RPS_front << " Desired RPS:"
//<< autonomous_desired_RPS << endl;
shooter_motor_back ->Set(RPS_speed_back);
shooter_motor_front ->Set(RPS_speed_front);
counter++;
#if 0
if (counter == 5)
{
counter = 1;
printf(
" %i)%f %f %f %f\n",
number, RPS, RPS_speed, error,
shooter_motor_back->Get());
number++;
}
else
{
printf("%f %f %f %f\n", RPS, RPS_speed, error,
shooter_motor_back->Get());
}
#endif
pid_code_timer->Reset();
back_shooter_encoder ->Reset();
front_shooter_encoder ->Reset();
}
}
RobotDemo()
{
#if YEAR_2013
drive = new RobotDrive(left_drive_motor_A_PWM, left_drive_motor_B_PWM,
right_drive_motor_A_PWM, right_drive_motor_B_PWM);
drive->SetExpiration(15);
drive->SetSafetyEnabled(false);
#endif
drive->SetInvertedMotor(RobotDrive::kFrontRightMotor, true);
drive->SetInvertedMotor(RobotDrive::kFrontLeftMotor, true);
drive->SetInvertedMotor(RobotDrive::kRearRightMotor, true);
drive->SetInvertedMotor(RobotDrive::kRearLeftMotor, true);
//Joystick
//ds = new DriverStation();
drive_stick_sec = new Joystick(right_stick);
drive_stick_prim = new Joystick(left_stick);
operator_stick = new Joystick(operator_joystick);
//Motors
#if JAGUAR_SWITCH
shooter_motor_front = new Jaguar(shooter_front_motor);
shooter_motor_back = new Jaguar(shooter_back_motor);
#else
shooter_motor_front = new Talon(shooter_front_motor);
shooter_motor_back = new Talon(shooter_back_motor);
#endif
#if DUMB_DRIVE_CODE
left_drive_motor_A = new Victor(left_drive_motor_A_PWM);
left_drive_motor_B = new Victor(left_drive_motor_B_PWM);
right_drive_motor_A = new Victor(right_drive_motor_A_PWM);
right_drive_motor_B = new Victor(right_drive_motor_B_PWM);
#endif
#if YEAR_2013
climbing_motor = new Victor(climbing_motor_PWM);
#endif
//limit switches
top_claw_limit_switch = new DigitalInput(top_claw_limit_switch_port);
bottom_claw_limit_switch = new DigitalInput(
bottom_claw_limit_switch_port);
//Encoders
front_shooter_encoder = new Encoder(shooter_motor_front_encoder_A_port,
shooter_motor_front_encoder_B_port, false);
back_shooter_encoder = new Encoder(shooter_motor_back_encoder_A_port,
shooter_motor_back_encoder_B_port, false);
//solenoids
shooter_angle_1 = new Solenoid(SHOOTER_ANGLE_SOLENOID_1);
shooter_angle_2 = new Solenoid(SHOOTER_ANGLE_SOLENOID_2);
shooter_fire_piston_A = new Solenoid(shooter_fire_piston_solenoid_A);
shooter_fire_piston_B = new Solenoid(shooter_fire_piston_solenoid_B);
//Timers
shooter_piston_timer = new Timer();
VC_timer = new Timer();
loop_time_measure_timer = new Timer();
shooter_reset = new Timer();
pid_code_timer = new Timer();
autonomous_timer = new Timer();
error_timer = new Timer();
retraction_timer = new Timer();
override_timer = new Timer();
stabilizing_timer = new Timer();
shooter_stop_timer = new Timer();
//Compressor
compressor1 = new Compressor(PRESSURE_SWITCH, compressor_enable);
#if CAMERA
//Camera
camera = &(AxisCamera::GetInstance(AxisCameraIpAddr));
camera->WriteResolution(AxisCamera::kResolution_640x480);
AxisCamera::GetInstance();
#endif
//Function starter
compressor1 ->Start();
//float RPS;
//PIDController pid1 (0.1 , 0.001 ,0.0 , &RPS , test_motor );
loop_time_measure_timer ->Start();
VC_timer ->Start();
//Variable Initialization
arcadedrive = true;
test_encoder_value = 0;
total_test_encoder_value = 0;
old_RPS = 0;
new_RPS = 0;
second_count = 0;
set_speed = 0.5;
cycle_counter = 0;
//test_motor ->Set(set_speed);
// float RPS;
//int old_encoder_value;
//double old_timer;
speed2 = 0.0;
test_speed = 0.3;
button = false;
pickup_on = false;
switch_on = false;
kicker_piston_on = false;
kicker_button_on = false;
average_counter = 0;
counter = 1;
number = 1;
additive_error = 0;
prev_error_front = 0;
prev_desired_RPS = 0;
ROC = 0;
claw_ = false;
claw_go_down = false;
constant_ = false;
constant_desired_RPS = false;
divided = 0;
//RPS_encoder_1 = new Encoder(9, 10);// for 2012 robot
shooter_motor_back_RPS = shooter_motor_back->Get();
//RPS_encoder_1 -> SetDistancePerPulse(1 / 250);//TODO figure out how this works
first_press = true;
test_RPS = false;
desired_RPS_control = 0.0;
slow_control = 0;
//RPS_encoder_1 ->Start();
pid_code_timer ->Start();
front_shooter_encoder->Start();
back_shooter_encoder->Start();
autonomous_timer ->Start();
error_timer ->Start();
retraction_timer->Start();
stabilizing_timer->Start();
override_timer->Start();
}
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 Autonomous(void)
{
#if 1
/*int autoMode = 0;
autoMode |= bcd1->Get();
autoMode <<= 1;
autoMode |= bcd2->Get();
autoMode <<= 1;
autoMode |= bcd3->Get()
;*/
//double ignoreLineStart = 0;
GetWatchdog().SetEnabled(true);
GetWatchdog().SetExpiration(0.2);
float liftSP = 0;
PIDLift->SetTolerance(10);
PIDLift->SetContinuous(false);
PIDLift->SetOutputRange(-0.75, 1); //BUGBUG
PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, LIFTDERIVGAIN);
PIDLift->Enable();
PIDGrip->SetSetpoint(0);
PIDGrip->Enable();
stopCount = 0;
float reduction;
int counts = 0;
leftEncoder->Start();
rightEncoder->Start();
leftEncoder->Reset();
rightEncoder->Reset();
liftEncoder->Start();
liftEncoder->Reset();
leftEncoder->SetDistancePerPulse((6 * PI / 500)/reduction);
rightEncoder->SetDistancePerPulse((6 * PI / 500)/reduction);
double avgEncoderCount;
float leftSpeed = .2, rightSpeed = .2;
short int lsL,lsM,lsR;
lineFollowDone = false;
counts = 0;
//int fancyWaiter = 0;
int popstage = 0;
goPop = false;
double backStart = 0;
double backTime = 0;
double popStart = 0;
double popTime = 0;
bool backDone = false;
miniDep->Set(miniDep->kForward);
int liftCount = 0;
bool disengageBrake = false;
float lastLiftSP = 0;
gripOpen1->Set(true);
gripOpen2->Set(false);
gripLatch1->Set(true);
gripLatch2->Set(false);
while(IsAutonomous())
{
if(!(counts % 100))printf("%2.2f \n",getDistance());
if(backStart) backTime = GetClock();
if(popStart) popTime = GetClock();
//if(!ignoreLineStart)ignoreLineStart = GetClock();
if(!compSwitch->Get()) compressor->Set(compressor->kReverse);
else compressor->Set(compressor->kOff);
if(counts%3 == 0)
{
leftValue = lsLeft->Get();
middleValue = lsMiddle->Get();
rightValue = lsRight->Get();
}
counts++;
GetWatchdog().Feed();
//avgEncoderCount = (leftEncoder->Get() + rightEncoder->Get())/2;
//myRobot.SetLeftRightMotorOutputs(.2,.2);
//All three/five autonomous programs will do the same thing up until 87 inches from the wall
if (counts % 100 == 0){//TESTING
if(lsLeft->Get()){
lsL = 1;
}else{
lsL = 0;
}
if(lsRight->Get()){
lsR = 1;
}else{
lsR = 0;
}
if(lsMiddle->Get()){
lsM = 1;
}else{
lsM = 0;
}
//printf("Encoder: %2.2f \n", (float)avgEncoderCount);
//printf("Distance: %2.2f SensorL:%1d SensorM:%1d SensorR:%1d\n",getDistance(),lsL,lsM,lsR);//TESTING
}
#if FOLLOWLINE
/*if(GetClock() - ignoreLineStart <= 0.5)
{
frontLeftMotor->Set(-.4);
rearLeftMotor->Set(-.4);
frontRightMotor->Set(.4);
rearRightMotor->Set(.4);
}
else */if (false){//(avgEncoderCount <= SECONDBREAKDISTANCE){
followLine();
}
#else
if (getDistance() > 24){
frontLeftMotor->Set(-leftSpeed);
rearLeftMotor->Set(-leftSpeed);
frontRightMotor->Set(rightSpeed);
rearRightMotor->Set(rightSpeed);
if(leftEncoder->Get() > rightEncoder->Get() && leftSpeed == .2){
rightSpeed += .03;
}else if(leftEncoder->Get() >rightEncoder->Get() && leftSpeed > .2){
leftSpeed -= .03;
}else if(leftEncoder->Get() < rightEncoder->Get() && rightSpeed == .2){
leftSpeed += .03;
}else if(leftEncoder->Get() < rightEncoder->Get() && rightSpeed > .2){
rightSpeed -= .03;
}
}
#endif
else{
if(counts % 100 == 0) {printf("DISTANCE: %2.2f\n",getDistance());}
switch(FOLLOWLINE)
{
case STRAIGHTLINEMODE: //Straight line. Scores on middle column of left or right grid.
//if(lineFollowDone && !(counts %50)) printf("Lift error: %f \n", PIDLift->GetError());
lastLiftSP = liftSP;
if(!lineFollowDone)
{
followLine();
}
else if(!PIDLift->GetSetpoint() && !popstage && !backStart)
{
//if(counts % 50 == 0)printf("Go backward\n");
frontLeftMotor->Set(.3);
rearLeftMotor->Set(.3);
frontRightMotor->Set(-.3);
rearRightMotor->Set(-.3);
//PIDLift->SetSetpoint(LIFTMID2);
liftSP = LIFTHIGH2 + 0.025;
//fancyWaiter = counts;
backStart = GetClock();
printf("Setpoint set setpoint set setpoint set \n");
/*
if(leftValue && middleValue && rightValue)
{
printf("Stopped 2nd time\n");
goPop = true;
frontLeftMotor->Set(0);
rearLeftMotor->Set(0);
frontRightMotor->Set(0);
rearRightMotor->Set(0);
PIDLift->SetSetpoint(LIFTHIGH2);
}
*/
}
#if 1 //if we've backed up for half a second and we're not popping
else if((backTime - backStart) > 0.65 && !backDone)
{
printf("Stopping!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!ONE\n");
frontLeftMotor->Set(0);
rearLeftMotor->Set(0);
frontRightMotor->Set(0);
rearRightMotor->Set(0);
//PIDLift->SetSetpoint(LIFTMID2);
liftSP = LIFTHIGH2;
backDone = true;
//Wait(.01);
//lift->brakeOff();
//fancyWaiter = counts;
//printf("Fancy waiter set Fancy waiter set Fancy waiter set");
}
/*
else if(lift->getPosition() < LIFTHIGH2)
{
//Move teh lifts
//if(counts % 100 == 0) printf("Stopping because lineFollowDone == true\n");
PIDLift->SetSetpoint(LIFTHIGH2);
}
*/
//if the lift is at the top and we're done backing up
else if(PIDLift->GetSetpoint() && fabs(liftSP - lift->getPosition()) < 0.015 && backDone)
{
if(!popStart) popStart = GetClock();
if(popstage == 0)
{
//lift->brakeOn();
//PIDLift->SetSetpoint(lift->getPosition());
popstage++;
printf("BRAKE BRAKE BRAKE BRAKE BRAKE \n");
}
else if(popstage == 1 && popTime - popStart > 0.1)
{
#if !(INNERGRIP)
gripOpen1->Set(true);
gripOpen2->Set(false);
#else
gripOpen1->Set(false);
gripOpen2->Set(true);
#endif
popstage++;
printf("OPEN OPEN OPEN OPEN OPEN \n");
}
else if(popstage == 2 && popTime - popStart > 0.35)
{
gripPop1->Set(true);
gripPop2->Set(false);
popstage++;
printf("POP POP POP POP POP POP POP \n");
}
else if(popstage == 3 && popTime - popStart > 1.35)
{
gripPop1->Set(false);
gripPop2->Set(true);
frontLeftMotor->Set(.2);
rearLeftMotor->Set(.2);
frontRightMotor->Set(-.2);
rearRightMotor->Set(-.2);
popstage++;
printf("UNPOP UNPOP UNPOP UNPOP UNPOP \n");
}
else if(popstage == 4 && popTime - popStart > 1.85)
{
printf("DOWN DOWN DOWN DOWN DOWN DOWN \n");
frontLeftMotor->Set(0);
rearLeftMotor->Set(0);
frontRightMotor->Set(0);
rearRightMotor->Set(0);
//PIDLift->SetSetpoint(0);
liftSP = 0;
}
/*
else if(popstage == 4 && popTime - popStart > 1.85)
{
printf("DOWN DOWN DOWN DOWN DOWN DOWN \n");
frontLeftMotor->Set(min(0.3, 0.25*(popTime - popStart - 1.85)));
rearLeftMotor->Set(min(0.3, 0.25*(popTime - popStart - 1.85)));
frontRightMotor->Set(max(-0.3, -0.25*(popTime - popStart - 1.85)));
rearRightMotor->Set(max(-0.3, -0.25*(popTime - popStart - 1.85)));
//PIDLift->SetSetpoint(0);
liftSP = 0;
popstage++;
}
else if(popstage == 5 && popTime - popStart > 4.85)
{
frontLeftMotor->Set(0);
rearLeftMotor->Set(0);
frontRightMotor->Set(0);
rearRightMotor->Set(0);
}
*/
}
//Start tele-op lift code
if(!lift->isBraking() && !disengageBrake)
{
PIDLift->SetSetpoint(liftSP);
if(liftSP == 0 && liftPIDSource->PIDGet() < 0.1)
{
//PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, 3*LIFTDERIVGAIN);
PIDLift->SetOutputRange(-liftPIDSource->PIDGet() - 0.1, 1);
}
else PIDLift->SetOutputRange(-0.75, 1);
}
if(lift->isBraking() && lastLiftSP != liftSP)
{
PIDLift->SetSetpoint(lift->getPosition() + 0.04);
PIDLift->SetPID(11.5 + 2*lift->getPosition(), LIFTINTGAIN + 0.4 + 3*lift->getPosition()/10, 0);
lift->brakeOff();
disengageBrake = true;
if(!liftCount) liftCount = counts;
}
//set brake (because near)
if(fabs(PIDLift->GetError()) < 0.015 && !lift->isBraking() && !disengageBrake)
{
if(liftCount == 0) liftCount = counts;
if(counts - liftCount > 1000)
{
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(counts%3000) printf("Braking/Not PID \n");
}
if(lift->isBraking() && !(counts%100000)) PIDLift->SetSetpoint(lift->getPosition());
if(fabs(PIDLift->GetError()) < 0.01 && disengageBrake && counts - liftCount > 1000)
{
disengageBrake = false;
PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, LIFTDERIVGAIN);
liftCount = 0;
}
//End tele-op lift code
#endif
//myRobot.SetLeftRightMotorOutputs(0,0);
break;
case NOLINEMODE: //Fork turn left. Scores on far right column of left grid.
lineFollowDone = true;
if(!lineFollowDone){}
else if(!PIDLift->GetSetpoint() && !popstage && !backStart)
{
//if(counts % 50 == 0)printf("Go backward\n");
frontLeftMotor->Set(.3);
rearLeftMotor->Set(.3);
frontRightMotor->Set(-.3);
rearRightMotor->Set(-.3);
//PIDLift->SetSetpoint(LIFTMID2);
liftSP = LIFTHIGH2;
//fancyWaiter = counts;
backStart = GetClock();
printf("Setpoint set setpoint set setpoint set \n");
/*
if(leftValue && middleValue && rightValue)
{
printf("Stopped 2nd time\n");
goPop = true;
frontLeftMotor->Set(0);
rearLeftMotor->Set(0);
frontRightMotor->Set(0);
rearRightMotor->Set(0);
PIDLift->SetSetpoint(LIFTHIGH2);
}
*/
}
#if 1 //if we've backed up for half a second and we're not popping
else if((backTime - backStart) > 0.65 && !backDone)
{
printf("Stopping!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!ONE\n");
frontLeftMotor->Set(0);
rearLeftMotor->Set(0);
frontRightMotor->Set(0);
rearRightMotor->Set(0);
//PIDLift->SetSetpoint(LIFTMID2);
liftSP = LIFTHIGH2;
backDone = true;
//Wait(.01);
//lift->brakeOff();
//fancyWaiter = counts;
//printf("Fancy waiter set Fancy waiter set Fancy waiter set");
}
/*
else if(lift->getPosition() < LIFTHIGH2)
{
//Move teh lifts
//if(counts % 100 == 0) printf("Stopping because lineFollowDone == true\n");
PIDLift->SetSetpoint(LIFTHIGH2);
}
*/
//if the lift is at the top and we're done backing up
else if(PIDLift->GetSetpoint() && fabs(liftSP - lift->getPosition()) < 0.015 && backDone)
{
if(!popStart) popStart = GetClock();
if(popstage == 0)
{
//lift->brakeOn();
//PIDLift->SetSetpoint(lift->getPosition());
popstage++;
printf("BRAKE BRAKE BRAKE BRAKE BRAKE \n");
}
else if(popstage == 1 && popTime - popStart > 0.1)
{
#if !(INNERGRIP)
gripOpen1->Set(true);
gripOpen2->Set(false);
#else
gripOpen1->Set(false);
gripOpen2->Set(true);
#endif
popstage++;
printf("OPEN OPEN OPEN OPEN OPEN \n");
}
else if(popstage == 2 && popTime - popStart > 0.35)
{
gripPop1->Set(true);
gripPop2->Set(false);
popstage++;
printf("POP POP POP POP POP POP POP \n");
}
else if(popstage == 3 && popTime - popStart > 1.35)
{
gripPop1->Set(false);
gripPop2->Set(true);
frontLeftMotor->Set(.2);
rearLeftMotor->Set(.2);
frontRightMotor->Set(-.2);
rearRightMotor->Set(-.2);
popstage++;
printf("UNPOP UNPOP UNPOP UNPOP UNPOP \n");
}
else if(popstage == 4 && popTime - popStart > 1.85)
{
printf("DOWN DOWN DOWN DOWN DOWN DOWN \n");
frontLeftMotor->Set(0);
rearLeftMotor->Set(0);
frontRightMotor->Set(0);
rearRightMotor->Set(0);
//PIDLift->SetSetpoint(0);
liftSP = 0;
}
}
//Start tele-op lift code
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(lift->getPosition() + 0.04);
PIDLift->SetPID(11.5 + 2*lift->getPosition(), LIFTINTGAIN + 0.4 + 3*lift->getPosition()/10, 0);
lift->brakeOff();
disengageBrake = true;
if(!liftCount) liftCount = counts;
}
//set brake (because near)
if(fabs(PIDLift->GetError()) < 0.015 && !lift->isBraking() && !disengageBrake)
{
if(liftCount == 0) liftCount = counts;
if(counts - liftCount > 1000)
{
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(counts%3000) printf("Braking/Not PID \n");
}
if(lift->isBraking() && !(counts%100000)) PIDLift->SetSetpoint(lift->getPosition());
if(fabs(PIDLift->GetError()) < 0.01 && disengageBrake && counts - liftCount > 1000)
{
disengageBrake = false;
PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, LIFTDERIVGAIN);
liftCount = 0;
}
//End tele-op lift code
#endif
//myRobot.SetLeftRightMotorOutputs(0,0);
break;
case FORKRIGHTMODE://Fork turn right. Scores on far left column of right grid.
if(leftEncoder->GetDistance() <= rightEncoder->GetDistance() + 6)
{
frontLeftMotor->Set(.2);
rearLeftMotor->Set(.2);
frontRightMotor->Set(0);
rearRightMotor->Set(0);
}
else if(getDistance() >= SCOREDISTANCE)
{
followLine();
}
//score here
//myRobot.SetLeftRightMotorOutputs(0,0);
break;
}
}
}
/*frontRightMotor->Set(0);
rearRightMotor->Set(0);
frontLeftMotor->Set(0);
rearLeftMotor->Set(0);*/
Wait(.02);
#endif
}
void followLine()
{
float turnMod;
if(leftValue && middleValue && rightValue && !lineFollowDone){
stopCount++;
if(/*getDistance() < 87 &&*/ stopCount >= 10)
{
frontLeftMotor->Set(0.0);
rearLeftMotor->Set(0.0);
frontRightMotor->Set(0.0);
rearRightMotor->Set(0.0);
lineFollowDone = true;
printf("LINE FOLLOW IS DONE!!!!!!!!!!1\n");
}
}else if(lineFollowDone){
frontLeftMotor->Set(0.0);
rearLeftMotor->Set(0.0);
frontRightMotor->Set(0.0);
rearRightMotor->Set(0.0);
}else{
stopCount = 0;
if (middleValue){
turnMod = 0;
}
else if (leftValue < rightValue)
{
turnMod = -.1;
}
else if (leftValue > rightValue)
{
turnMod = .1;
}
else
{
turnMod = 0;
}
//myRobot.SetLeftRightMotorOutputs(.5 - turnMod, .5 + turnMod);
frontLeftMotor->Set(-.4 + turnMod);
rearLeftMotor->Set(-.4 + turnMod);
frontRightMotor->Set(.4 + turnMod);
rearRightMotor->Set(.4 + turnMod);
}
}
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();
}
}
void TeleopPeriodic(void) {
// increment the number of teleop periodic loops completed
GetWatchdog().Feed();
m_telePeriodicLoops++;
/*
* No longer needed since periodic loops are now synchronized with incoming packets.
if (m_ds->GetPacketNumber() != m_priorPacketNumber) {
*/
/*
* Code placed in here will be called only when a new packet of information
* has been received by the Driver Station. Any code which needs new information
* from the DS should go in here
*/
m_dsPacketsReceivedInCurrentSecond++; // increment DS packets received
// put Driver Station-dependent code here
// Demonstrate the use of the Joystick buttons
// DemonstrateJoystickButtons(m_rightStick, m_rightStickButtonState, "Right Stick", &m_solenoids[1]);
// DemonstrateJoystickButtons(m_leftStick, m_leftStickButtonState, "Left Stick ", &m_solenoids[5]);
/***
// determine if tank or arcade mode, based upon position of "Z" wheel on kit joystick
if (m_rightStick->GetZ() <= 0) { // Logitech Attack3 has z-polarity reversed; up is negative
// use arcade drive
m_robotDrive->ArcadeDrive(m_rightStick); // drive with arcade style (use right stick)
if (m_driveMode != ARCADE_DRIVE) {
// if newly entered arcade drive, print out a message
printf("Arcade Drive\n");
m_driveMode = ARCADE_DRIVE;
}
} else {
// use tank drive
m_robotDrive->TankDrive(m_leftStick, m_rightStick); // drive with tank style
if (m_driveMode != TANK_DRIVE) {
// if newly entered tank drive, print out a message
printf("Tank Drive\n");
m_driveMode = TANK_DRIVE;
}
}
***/
/* Grab z-wheel value and transform from [1, -1] to [0,4600]
* 4600 rpm is a guess */
float rawZ, transformedZ;
rawZ = m_leftStick->GetZ();
// transformedZ = (1.0 - rawZ)/(-2.0);
transformedZ = -2300.0 * rawZ + 2300.0;
/* Driver station display output. */
char msg[256];
static DriverStationLCD *dsLCD = DriverStationLCD::GetInstance();
sprintf(msg, "Launcher Speed = %f RPM", transformedZ);
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, msg);
sprintf(msg, "Loader Limit = %u", m_loaderLimit->Get());
dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, msg);
sprintf(msg, "Loader Trigger = %u", m_leftStick->GetTrigger());
dsLCD->Printf(DriverStationLCD::kUser_Line3, 1, msg);
// line number (enum), starting col, format string, args for format string ...
dsLCD->UpdateLCD();
// use arcade drive
//m_driveGain = (1.0 - m_rightStick->GetZ())/(-2.0);
m_robot->ArcadeDrive(m_rightStick); // drive with arcade style (use right stick)
// m_robotFrontDrive->Drive(-m_driveGain*m_rightStick->GetY(),-m_driveGain*m_rightStick->GetX()); // negative sign to fix bug in turn
// m_robotRearDrive->Drive(m_driveGain*m_rightStick->GetY(),-m_driveGain*m_rightStick->GetX()); // negative sign to fix bug in turn
// -Add an elevation control, add a motor controller to port 8, and add a joystick button pair to control up/down
if (m_rightStick->GetRawButton(m_elevatorUpButton)){
m_elevatorMotor->Set(m_elevatorUpSpeed);
}
else if (m_rightStick->GetRawButton(m_elevatorDownButton)){
m_elevatorMotor->Set(m_elevatorDownSpeed);
}
else {
m_elevatorMotor->Set(0.0);
}
// trigger button activates feeding and loading mechanisms
if ((m_leftStick->GetTop() || m_feeding) && (!m_loading) && (m_feedCounter <= m_feedCount)){
m_feeding = true;
m_feedCounter++;
m_feedMotor->Set(m_feedSpeed);
}
else {
m_feeding = false;
m_feedCounter = 0;
m_feedMotor->Set(0.);
}
m_loaderLimit->Get(); //just testing digital input port 1
// top(2) button uses the Z wheel to control the speed of the loading mechanisms
if (((m_leftStick->GetTrigger() || m_loading) && (!m_feeding)) && (m_loadCounter <= m_loadCount)){//comment out this line
// if (((m_leftStick->GetTrigger() || m_loading) && (!m_feeding)) && (bool) m_loaderLimit->Get()){ //uncomment this line
m_loading = true;
m_loadCounter++; // comment out this line
m_loadMotor->Set(m_loadSpeed); // load breech
}
else if ((m_loading && (!m_feeding)) && (m_loadCounter > m_loadCount) && (m_loadCounter <= ((m_loadCount+m_loadPauseCount)))) { // comment out the pause logic
m_loading = true;// comment out the pause logic
m_loadCounter++;// comment out the pause logic,
m_loadMotor->Set(0.); // pause loader // comment out the pause logic
}
// else if ((m_loading && (!m_feeding)) && (m_loadCounter >= (m_loadCount+m_loadPauseCount)) && (m_loadCounter<= 2*m_loadCount-13)) { // comment out this line
else if ((m_loading && (!m_feeding)) && (m_loadCounter >= (m_loadCount+m_loadPauseCount)) && (m_loaderLimit->Get()!=0)) {
//Retracting loader arm (feeder), have not yet hit the limit switch.
// else if ((m_loading &! m_feeding) && (bool) m_loaderResetLimit->Get()) { // uncomment this line
m_loading = true;
m_loadCounter++; // comment out the pause logic
m_loadMotor->Set(-m_loadSpeed);; // reset loader
}
else {
//Finished retracting loader arm (feeder), so stop it.
m_loading = false;
m_loadCounter = 0;
m_loadMotor->Set(0.);; // stop loader
}
// Set launcher motor command
m_launchMotor->Set(-(1.0 - m_leftStick->GetZ())/(-2.0)); // transform from [-1.0, +1.0] to [0.0, +1.0]
/*
} // if (m_ds->GetPacketNumber()...
*/
// use buttons 8 and 9 to trim load motor position in maintenance mode
/*if (m_leftStick->GetRawButton(8) && (!m_feeding) && (!m_loading)){
m_loadMotor->Set(0.12);
}
else if (m_leftStick->GetRawButton(9) && (!m_feeding) && (!m_loading)){
m_loadMotor->Set(-0.1);
}*/
//Use buttons 10 and 11 to trim feed motor position in maintenance mode
if (m_leftStick->GetRawButton(11) && (!m_feeding) && (!m_loading)){
m_feedMotor->Set(0.15);
}
else if (m_leftStick->GetRawButton(10) && (!m_feeding) && (!m_loading)){
m_feedMotor->Set(-0.1);
}
} // TeleopPeriodic(void)
