float Cypress::GetAnalogIn(int channel) {
int getchan=abs(channel-4)+1;
if (enhanced)
return m_ds->GetEnhancedIO().GetAnalogIn(getchan);
else
return m_ds->GetAnalogIn(getchan);
}
void DataSending::SendTheData(){
strIndex = 0;
Dashboard &dash = DriverStation::GetInstance()->GetHighPriorityDashboardPacker();
DriverStation *drive = DriverStation::GetInstance();
Send(true);//yes this is the dataSending Code
Send(drive->GetBatteryVoltage());//battery info
Send((batteryCurrent->GetVoltage()-2.5)*AMPS_CONSTANT);
GetDriveJoystickInfo();//joystick info
GetOperatorJoystickInfo();//moar joystick info
GetVicInfo();//victor info
Send(RobotMap::launcherSolenoidLeft->Get());//solenoid info
Send(RobotMap::launcherSolenoidRight->Get());
Send(RobotMap::shifterDoubleSolenoid->Get());
Send((bool)RobotMap::launcherCompressor->GetPressureSwitchValue());//sensor info
Send(RobotMap::robotRangeFinderUltrasonicSensor->GetVoltage()/VoltsPerCM);
Send(RobotMap::driveTrainGyro->GetAngle());
Send(RobotMap::launcherPressureSwitch->GetVoltage()*PSI_CONSTANT);//transducer1
Send(RobotMap::collectorLeftRoller->Get()*-1);//talon info
Send(RobotMap::collectorRightRoller->Get());
Send(count++);//number of packets
Send(timesPerSecond);//every nth number data is sent from a 50 hz source
//a.k.a 50 / above number = Hz
Send(drive->GetMatchTime());
Send(drive->IsEnabled());
dash.AddString(strBuffer);
dash.Finalize();
UpdateUserLCD();
}
void Cypress::SetDigitalOut(int channel, bool value) {
int getchan=abs(channel-8)+1;
if (enhanced) {
getchan+=8;
m_ds->GetEnhancedIO().SetDigitalOutput(getchan, value);
} else
m_ds->SetDigitalOut(getchan, value);
}
OI::OI() :
pIO(NULL),
driverStick(DRIVER_STICK),
gunnerStick(GUNNER_STICK)
{
DriverStation *pDS = DriverStation::GetInstance();
pIO = &pDS->GetEnhancedIO();
}
MedicOperatorInterface::MedicOperatorInterface()
{
joyDrive = new Joystick(1);//TODO:real value
joyManip = new Joystick(2);//TODO:real value
DriverStation *dsSimple = DriverStation::GetInstance();
ds = &dsSimple->GetEnhancedIO();
dsLCD = DriverStationLCD::GetInstance();
dashboard->init();
}
bool Cypress::GetDigitalIn(int channel)
{
int getchan=abs(channel-8)+1;
if (enhanced)
return !m_ds->GetEnhancedIO().GetDigital(getchan);
else
return m_ds->GetDigitalIn(getchan);
}
void DriverStation::MergeFrom(const DriverStation& from) {
GOOGLE_CHECK_NE(&from, this);
if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
if (from.has_enabled()) {
set_enabled(from.enabled());
}
if (from.has_state()) {
set_state(from.state());
}
}
mutable_unknown_fields()->MergeFrom(from.unknown_fields());
}
void DisabledInit() {
//Config loading
try {
cameraLight->Set(Relay::kOff);
if (!Config::LoadFromFile("config.txt")) {
cout << "Something happened during the load." << endl;
}
Config::Dump();
myDrive->DisablePID();
myDrive->ResetPID();
if(fout.is_open() && !freshStart && !ds->IsFMSAttached()){
fout.close();
myShooter->ResetShooterProcess();
lc->holdState(false);
}
} catch (exception ex) {
cout << "Disabled exception. Trying again." << endl;
cout << "Exception: " << ex.what() << endl;
}
//ResetShooterMotors();
/*
SmartDashboard::PutNumber("Target Info S",1741);
cout<<SmartDashboard::GetNumber("Target Info S");
*/
}
/**
* Check if this motor has exceeded its timeout.
* This method is called periodically to determine if this motor has exceeded its timeout
* value. If it has, the stop method is called, and the motor is shut down until its value is
* updated again.
*/
void MotorSafetyHelper::Check()
{
DriverStation *ds = DriverStation::GetInstance();
if (!m_enabled || ds->IsDisabled() || ds->IsTest()) return;
::std::unique_lock<ReentrantMutex> sync(m_syncMutex);
if (m_stopTime < Timer::GetFPGATimestamp())
{
char buf[128];
char desc[64];
m_safeObject->GetDescription(desc);
snprintf(buf, 128, "%s... Output not updated often enough.", desc);
wpi_setWPIErrorWithContext(Timeout, buf);
m_safeObject->StopMotor();
}
}
/**
* Runs the motors under driver control with either tank or arcade steering selected
* by a jumper in DS Digin 0. Also an arm will operate based on a joystick Y-axis.
*/
void OperatorControl(void)
{
Victor armMotor(5); // create arm motor instance
while (IsOperatorControl())
{
GetWatchdog().Feed();
// determine if tank or arcade mode; default with no jumper is for tank drive
if (ds->GetDigitalIn(ARCADE_MODE) == 0) {
myRobot->TankDrive(leftStick, rightStick); // drive with tank style
} else{
myRobot->ArcadeDrive(rightStick); // drive with arcade style (use right stick)
}
// Control the movement of the arm using the joystick
// Use the "Y" value of the arm joystick to control the movement of the arm
float armStickDirection = armStick->GetY();
// if at a limit and telling the arm to move past the limit, don't drive the motor
if ((armUpperLimit->Get() == 0) && (armStickDirection > 0.0)) {
armStickDirection = 0;
} else if ((armLowerLimit->Get() == 0) && (armStickDirection < 0.0)) {
armStickDirection = 0;
}
// Set the motor value
armMotor.Set(armStickDirection);
}
}
bool Cypress::GetDigitalIn(int channel)
{
return m_ds->GetEnhancedIO().GetDigital(channel);
}
// Test Autonomous
void Autonomous()
{
robotDrive.SetSafetyEnabled(false);
// STEP 1: Set all of the states.
// SAFETY AND SANITY - SET ALL TO ZERO
loaded = winchSwitch.Get();
loading = false;
intake.Set(0.0);
rightWinch.Set(0.0);
leftWinch.Set(0.0);
// STEP 2: Move forward to optimum shooting position
Drive(-AUTO_DRIVE_SPEED, SHOT_POSN_DIST);
// STEP 3: Drop the arm for a clean shot
arm.Set(DoubleSolenoid::kForward);
Wait(1.0); // Ken
// STEP 4: Launch the catapult
LaunchCatapult();
Wait (1.0); // Ken
if (ds->GetDigitalIn(1))
{
// STEP 5: Start the intake motor and backup to our origin position to pick up another ball
InitiateLoad();
intake.Set(-INTAKE_COLLECT);
while (CheckLoad());
Drive(AUTO_DRIVE_SPEED, SHOT_POSN_DIST);
Wait(1.0); // For the ball to collect
// STEP 6: Shut off the intake, bring up the arm and move to shooting position
intake.Set(0.0);
arm.Set(DoubleSolenoid::kReverse);
Wait (1.0); // "Settle down"
Drive(-AUTO_DRIVE_SPEED, SHOT_POSN_DIST);
// Step 7: drop the arm for a clean shot and shoot
arm.Set(DoubleSolenoid::kForward);
Drive(AUTO_DRIVE_SPEED, SHOT_POSN_DIST);
// UNTESTED KICKED OFF FIELD
Wait(1.0); // For arm to go down
LaunchCatapult();
}
// Get us fully into the zone for 5 points
Drive(-AUTO_DRIVE_SPEED, INTO_ZONE_DIST - SHOT_POSN_DIST);
// SAFETY AND SANITY - SET ALL TO ZERO
intake.Set(0.0);
rightWinch.Set(0.0);
leftWinch.Set(0.0);
}
/**
* Drive left & right motors for 2 seconds, enabled by a jumper (jumper
* must be in for autonomous to operate).
*/
void Autonomous(void) {
myRobot->SetSafetyEnabled(false);
if (ds->GetDigitalIn(ENABLE_AUTONOMOUS) == 1) // only run the autonomous program if jumper is in place
{
myRobot->Drive(0.5, 0.0); // drive forwards half speed
Wait(2.0); // for 2 seconds
myRobot->Drive(0.0, 0.0); // stop robot\
}
myRobot->SetSafetyEnabled(true);
}
void DashBoardInput() {
int i = 0;
GetWatchdog().SetEnabled(true);
while (IsAutonomous() && IsEnabled()) {
i++;
GetWatchdog().Feed();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "%f, %i",
driverStation->GetAnalogIn(1), i);
dsLCD->UpdateLCD();
}
}
void RA14Robot::logging() {
if (fout.is_open()) {
fout << missionTimer->Get() << ",";
#ifndef DISABLE_SHOOTER
myCam->log(fout);
#endif //Ends DISABLE_SHOOTER
myDrive->log(fout);
CurrentSensorSlot * slots[4] = { camMotor1Slot, camMotor2Slot,
driveLeftSlot, driveRightSlot };
DriverStation * ds = DriverStation::GetInstance();
for (int i = 0; i < 4; ++i) {
fout << slots[i]->Get() << ",";
}
fout << auto_case << "," << gyro->GetAngle() << "," << dropSensor->GetPosition() << "," << ds->GetBatteryVoltage() << ",";
fout << target->IsValid() << "," << target->IsHot() << "," << target->GetDistance() << "," << target->GetX() << ",";
fout << target->GetY() << "," << target->IsLeft() << "," << target->IsRight() << ",";
fout << ds->GetMatchTime() << "," << auto_state << ",";
fout << endl;
}
}
/**
* unchanged from SimpleDemo:
* Runs the motors under driver control with either tank or arcade steering selected
* by a jumper in DS Digin 0.
*/
void OperatorControl(void)
{
DPRINTF(LOG_DEBUG, "OperatorControl");
GetWatchdog().Feed();
while (1)
{
// determine if tank or arcade mode; default with no jumper is for tank drive
if (ds->GetDigitalIn(ARCADE_MODE) == 0) {
myRobot->TankDrive(leftStick, rightStick); // drive with tank style
} else{
myRobot->ArcadeDrive(rightStick); // drive with arcade style (use right stick)
}
}
} // end operator control
void logdata()
{
//log data
fout << logTimer->Get() << ',' << CurrentMode() << ',' << ds->GetBatteryVoltage() << ',';
fout << magicBox->getGyroAngle() << ',';
autoLog();
myDrive->log(fout);
myShooter->log(fout);
//myClimber->Log(fout);
log_gamepad(fout, driverGamepad);
log_gamepad(fout, operatorGamepad);
fout<<LEDTimer<<",";
fout<<LEDPercent(LEDTimer)<<",";
fout<<LEDPercent(LEDTimer + 30)<<",";
fout<<endl;
}
Robot(void) : dseio(ds->GetInstance()->GetEnhancedIO())
{
this->SetPeriod(.05);
FL = new Jaguar(PWM_PORT_5);
RL = new Jaguar(PWM_PORT_7);
FR = new Jaguar(PWM_PORT_4);
RR = new Jaguar(PWM_PORT_6);
mygyro = new Gyro(AI_PORT_1);
drive = new RobotDrive(FL,RL,FR,RR);
drive->SetInvertedMotor(RobotDrive::kFrontRightMotor, true);
drive->SetInvertedMotor(RobotDrive::kRearRightMotor,true);
drive->SetInvertedMotor(RobotDrive::kFrontLeftMotor, true);//inversed because motor physically spins in the rwrong direction
drive->SetInvertedMotor(RobotDrive::kRearLeftMotor, false);
drive->SetExpiration(15);
left = new DigitalInput(DI_PORT_1);
middle = new DigitalInput(DI_PORT_2);
right = new DigitalInput(DI_PORT_3);
StraightLineSwitch = new DigitalInput(DI_PORT_5);
GoLeftSwitch = new DigitalInput(DI_PORT_6);
robot_compressor = new Compressor(DI_PORT_4,DO_PORT_1);
rightStick = new Joystick(1);
leftStick = new Joystick(2);
myarm = new DDCArm();
deploy = new Deployment();
autotimer = new Timer();
}
/**
* Drive left & right motors for 2 seconds then stop
*/
void Autonomous(void)
{
Saftey->SetEnabled(false);
//myRobot->SetSafetyEnabled(false);
//myRobot->Drive(0.5, 0.0); // drive forwards half speed
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Clear();
//dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Hello World" );
//dsLCD->UpdateLCD();
Wait(0.5);
ds = DriverStation::GetInstance();
switch(ds->GetLocation())
{
case 1:
//Execute Autonomous code #1
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Executing Autonomous 1");
break;
case 2:
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Executing Autonomous 2");
//Execute Autonomous code #2
break;
case 3:
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Executing Autonomous 3");
//Execute Autonomous code #3
break;
}
dsLCD->UpdateLCD();
Saftey->SetEnabled(false);
Wait(0.5); // for 2 seconds
delete Jaguar1;
delete Jaguar2;
delete Saftey;
delete dsLCD;
delete ds;
}
void TeleopPeriodic() {
if(tick==10) if (ds->IsSysBrownedOut()) {
ds->ReportError("[ERROR] BROWNOUT DETECTED!!");
}
if(tick == 15) if (!ds->IsNewControlData()) {
ds->ReportError(
"[ERROR] NO DATA FROM DRIVER STATION IN THIS TICK!");
}
if(tick==20) if (!ds->IsDSAttached()) {
ds->ReportError("[ERROR] DRIVER STATION NOT DETECTED!");
}
if (stick.GetRawButton(10))
zeroSanics();
if (stick.GetRawButton(8)) {
leftIRZero = 0;
rightIRZero = 0;
}
tick++;
if (liftStick.GetRawButton(2)) {
double canScale = liftStick.GetRawAxis(2);
canScale += 1;
canScale = 2 - canScale;
canScale /= 2;
canGrabber.SetSpeed(canScale);
} else if (liftStick.GetRawButton(3)) {
double canScale = liftStick.GetRawAxis(2);
canScale += 1;
canScale = 2 - canScale;
canScale /= 2;
canGrabber.SetSpeed(-canScale);
} else
canGrabber.SetSpeed(0);
double speed;
//Calculate scalar to use for POV/Adjusted drive
double scale = stick.GetRawAxis(3);
scale += 1;
scale = 2 - scale;
scale /= 2;
//Use pov/hat switch for movement if enabled
if (stick.GetRawButton(1) && stick.GetRawButton(2)) {
AutomaticLineup();
} else if (stick.GetRawButton(1)) {
double leftVolts = leftIR.GetAverageVoltage() - leftIRZero;
double rightVolts = rightIR.GetAverageVoltage() - leftIRZero;
if (rightVolts + VOLTAGE_TOLERANCE > leftVolts
&& rightVolts - VOLTAGE_TOLERANCE < leftVolts) {
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
} else if (rightVolts > leftVolts)
robotDrive.MecanumDrive_Cartesian(0, 0, 0.2);
else if (leftVolts > rightVolts)
robotDrive.MecanumDrive_Cartesian(0, 0, -0.2);
} else if (stick.GetRawButton(6)) {
//Rotate
robotDrive.MecanumDrive_Polar(0, 0, scale);
} else if (stick.GetRawButton(5)) {
//Rotate
robotDrive.MecanumDrive_Polar(0, 0, -scale);
} else if (stick.GetPOV(0) != -1) {
//If POV moved, move polar (getPOV returns an angle in degrees)
robotDrive.MecanumDrive_Polar(scale, -stick.GetPOV(0), 0);
} else if (stick.GetRawButton(2)) {
//Drive with scalar
robotDrive.MecanumDrive_Cartesian(-stick.GetRawAxis(0) * scale,
stick.GetRawAxis(1) * scale, stick.GetRawAxis(2) * scale);
} else {
//Drive normally
robotDrive.MecanumDrive_Cartesian(-stick.GetX(), stick.GetY(),
stick.GetZ());
}
speed = -liftStick.GetY();
//bool canGoUp = maxUp.Get();
bool canGoUp = true;
//bool canGoDown = maxDown.Get();
bool canGoDown = true;
//If at a limit switch and moving in that direction, stop
if (speed > 0 && !canGoUp)
speed = 0;
if (speed < 0 && !canGoDown)
speed = 0;
chainLift.SetSpeed(speed);
if (tick >50) {
if (SmartDashboard::GetBoolean("Smart Dashboard Enabled")) {
//Smart Dash outputs
//SmartDashboard::PutNumber("X Acceleration: ", accel.GetX());
//SmartDashboard::PutNumber("Y Acceleration: ", accel.GetY());
//SmartDashboard::PutNumber("Z Acceleration: ", accel.GetZ());
SmartDashboard::PutBoolean("Switch 1: (up)", maxUp.Get());
SmartDashboard::PutBoolean("Switch 2: (down)", maxDown.Get());
SmartDashboard::PutBoolean("Switch 3: (mid)", midPoint.Get());
SmartDashboard::PutBoolean("Auto switch A: ",
autoSwitch1.Get());
SmartDashboard::PutBoolean("Auto switch B: ",
autoSwitch2.Get());
//SmartDashboard::PutBoolean("RobotDrive Alive?",
// robotDrive.IsAlive());
//SmartDashboard::PutBoolean("ChainLift Alive?",
// robotDrive.IsAlive());
SmartDashboard::PutNumber("Left Sensor",
leftIR.GetAverageVoltage());
SmartDashboard::PutNumber("Right Sensor",
rightIR.GetAverageVoltage());
SmartDashboard::PutNumber("Left w zero",
leftIR.GetAverageVoltage() - leftIRZero);
SmartDashboard::PutNumber("Rigt w zero",
rightIR.GetAverageVoltage() - rightIRZero);
SmartDashboard::PutNumber("PDP 14 Current", pdp.GetCurrent(14));
SmartDashboard::PutNumber("PDP 15 Current", pdp.GetCurrent(15));
}
tick = 0;
}
}
/**
* Robot-wide initialization code should go here.
*
* Use this method for default Robot-wide initialization which will
* be called when the robot is first powered on. It will be called exactly 1 time.
*/
void RobotInit()
{
printf(">>> RobotInit\n");
LogInit();
#ifdef HAVE_COMPRESSOR
compressor = new Compressor(1, 1);
#endif
#ifdef HAVE_TOP_WHEEL
#ifdef HAVE_TOP_CAN1
topWheel1 = new CANJaguar(1);
topWheel1->SetSafetyEnabled(false); // motor safety off while configuring
topWheel1->SetSpeedReference( CANJaguar::kSpeedRef_Encoder );
topWheel1->ConfigEncoderCodesPerRev( 1 );
#endif
#ifdef HAVE_TOP_PWM1
topWheel1 = new Victor(1);
topWheel1->SetSafetyEnabled(false); // motor safety off while configuring
#endif
#ifdef HAVE_TOP_CAN2
topWheel2 = new CANJaguar(2);
topWheel2->SetSafetyEnabled(false); // motor safety off while configuring
topWheel2->SetSpeedReference( CANJaguar::kSpeedRef_Encoder );
topWheel2->ConfigEncoderCodesPerRev( 1 );
#endif
topTach = new Tachometer(2);
#endif
#ifdef HAVE_BOTTOM_WHEEL
#ifdef HAVE_BOTTOM_CAN1
bottomWheel1 = new CANJaguar(3);
bottomWheel1->SetSafetyEnabled(false); // motor safety off while configuring
bottomWheel1->SetSpeedReference( CANJaguar::kSpeedRef_Encoder );
bottomWheel1->ConfigEncoderCodesPerRev( 1 );
#endif
#ifdef HAVE_BOTTOM_PWM1
bottomWheel1 = new Victor(2);
bottomWheel1->SetSafetyEnabled(false); // motor safety off while configuring
#endif
#ifdef HAVE_BOTTOM_CAN2
bottomWheel2 = new CANJaguar(4);
bottomWheel2->SetSafetyEnabled(false); // motor safety off while configuring
bottomWheel2->SetSpeedReference( CANJaguar::kSpeedRef_Encoder );
bottomWheel2->ConfigEncoderCodesPerRev( 1 );
#endif
bottomTach = new Tachometer(3);
#endif
#ifdef HAVE_ARM
arm = new DoubleSolenoid(2, 1);
#endif
#ifdef HAVE_INJECTOR
injectorL = new DoubleSolenoid(5, 3);
injectorR = new DoubleSolenoid(6, 4);
#endif
#ifdef HAVE_EJECTOR
ejector = new Solenoid(7);
#endif
#ifdef HAVE_LEGS
legs = new Solenoid(8);
#endif
ds = DriverStation::GetInstance();
eio = &ds->GetEnhancedIO();
gamepad = new Joystick(1);
LiveWindow *lw = LiveWindow::GetInstance();
#ifdef HAVE_COMPRESSOR
lw->AddActuator("K9", "Compressor", compressor);
#endif
#ifdef HAVE_TOP_WHEEL
#ifdef HAVE_TOP_CAN1
lw->AddActuator("K9", "Top1", topWheel1);
#endif
#ifdef HAVE_TOP_PWM1
lw->AddActuator("K9", "Top1", topWheel1);
#endif
#ifdef HAVE_TOP_CAN2
lw->AddActuator("K9", "Top2", topWheel2);
#endif
#endif
#ifdef HAVE_BOTTOM_WHEEL
#ifdef HAVE_BOTTOM_CAN1
lw->AddActuator("K9", "Bottom1", bottomWheel1);
#endif
#ifdef HAVE_BOTTOM_PWM1
lw->AddActuator("K9", "Bottom1", bottomWheel1);
#endif
#ifdef HAVE_BOTTOM_CAN2
lw->AddActuator("K9", "Bottom2", bottomWheel2);
#endif
#endif
#ifdef HAVE_ARM
lw->AddActuator("K9", "Arm", arm);
#endif
#ifdef HAVE_INJECTOR
lw->AddActuator("K9", "InjectorL", injectorL);
lw->AddActuator("K9", "InjectorR", injectorR);
#endif
#ifdef HAVE_EJECTOR
lw->AddActuator("K9", "Ejector", ejector);
#endif
#ifdef HAVE_LEGS
lw->AddActuator("K9", "Legs", legs);
#endif
SmartDashboard::PutNumber("Shooter P", kP);
SmartDashboard::PutNumber("Shooter I", kI);
SmartDashboard::PutNumber("Shooter D", kD);
spinFastNow = false;
#ifdef HAVE_TOP_WHEEL
SmartDashboard::PutNumber("Top Set ", topSpeed);
#ifdef HAVE_TOP_CAN1
SmartDashboard::PutNumber("Top Current 1", 0.0);
#endif
#ifdef HAVE_TOP_CAN2
SmartDashboard::PutNumber("Top Current 2", 0.0);
SmartDashboard::PutNumber("Top Jag ", 0.0);
#endif
SmartDashboard::PutNumber("Top Tach ", 0.0);
#endif
#ifdef HAVE_BOTTOM_WHEEL
SmartDashboard::PutNumber("Bottom Set ", bottomSpeed);
#ifdef HAVE_BOTTOM_CAN1
SmartDashboard::PutNumber("Bottom Current 1", 0.0);
#endif
#ifdef HAVE_BOTTOM_CAN2
SmartDashboard::PutNumber("Bottom Current 2", 0.0);
SmartDashboard::PutNumber("Bottom Jag ", 0.0);
#endif
SmartDashboard::PutNumber("Bottom Tach ", 0.0);
#endif
SetPeriod(0); //Set update period to sync with robot control packets (20ms nominal)
printf("<<< RobotInit\n");
}
void
DashboardSender::SendData(Robot1073 *p)
{
static unsigned int packetCt = 0;
float tempFloat;
DriverStation *ds = DriverStation::GetInstance();
Dashboard &dash = ds->GetHighPriorityDashboardPacker();
dash.AddCluster();
dash.AddU32(packetCt++);
dash.AddU32(0xFFFFFFFF);
unsigned short packData = 1;
if(p->IsEnabled()){packData += 2;}
if(false){packData += 4;} // has tube
if(p->IsOperatorControl()){packData += 8;}
else if(p->IsAutonomous()){packData += 16;}
else {packData += 24;}
if(false){packData += 32;} // has line
if(p->arm->IsUpLimitSwitchActive()){packData += 64;}
if(p->arm->IsDownLimitSwitchActive()){packData += 128;}
if(p->pincer->IsClosedLimitSwitchActive()){packData += 256;}
if(p->pincer->IsOpenLimitSwitchActive()){packData += 512;}
if(p->leftLineSensor->Get()){ packData += 1024; }
if(p->middleLineSensor->Get()){packData += 2048; }
if(p->rightLineSensor->Get()){packData += 4096; }
if(p->kraken->GetMode() != p->kraken->IdleMode ) { packData += 1 << 13; }
dash.AddU16(packData);
//printf("left = %d middle = %d right = %d\n", p->leftLineSensor->Get(),p->middleLineSensor->Get(),p->rightLineSensor->Get());
for (int i = 1; i <= 8; i++) {
tempFloat = (float) AnalogModule::GetInstance(1)->GetAverageVoltage(i);
dash.AddFloat(tempFloat); // analogs
//printf("Float %d = %fV\n", i, tempFloat);
}
int module = 1;
DigitalModule * digitalModule = DigitalModule::GetInstance(module);
unsigned char relayForward = digitalModule->GetRelayForward();
dash.AddU8(relayForward); // relays (forward)
dash.AddU8(DigitalModule::GetInstance(module)->GetRelayReverse()); // relays (reverse)
dash.AddU16((short)DigitalModule::GetInstance(module)->GetDIO()); // state
dash.AddU16(DigitalModule::GetInstance(module)->GetDIODirection());//direction
for (int i = 1; i <= 10; i++) {
dash.AddU8((unsigned char) DigitalModule::GetInstance(module)->GetPWM(i)); // pwm's
//printf("PWM %d %02X\n" ,i, DigitalModule::GetInstance(module)->GetPWM(i));
}
dash.AddFloat(p->matchTimer->GetTimeRemaining());
dash.AddFloat(ds->GetBatteryVoltage());
dash.AddFloat(p->gyro->GetAngle());
dash.AddFloat(p->leftJoystick->GetX());
dash.AddFloat(p->rightJoystick->GetX());
dash.AddFloat(p->leftJoystick->GetY());
dash.AddFloat(p->rightJoystick->GetY());
std::pair<double, double> lrDistance = p->encoders->GetDistance();
dash.AddFloat((float)lrDistance.first);
dash.AddFloat((float)lrDistance.second);
// Navigation Data
dash.AddFloat(p->navigation->GetX());
dash.AddFloat(p->navigation->GetY());
dash.AddFloat(5);//accel temp
dash.AddFloat(5);//accel temp
dash.AddFloat(p->navigation->GetXVelocity());
dash.AddFloat(p->navigation->GetYVelocity());
dash.AddFloat(sqrt(p->navigation->GetXVar()));
dash.AddFloat(sqrt(p->navigation->GetYVar()));
dash.AddFloat(p->navigation->GetHeading());
dash.AddFloat(p->elevator->GetCurrentPositionFeet());
dash.AddFloat(p->elevator->GetTargetPositionFeet());
// jags++
//+++++++++++++++++++++++
dash.AddFloat((float)p->leftMotorJaguar->GetOutputCurrent());
dash.AddFloat((float)p->rightMotorJaguar->GetOutputCurrent());
dash.AddFloat((float)p->pincerJaguar->GetOutputCurrent());
dash.AddFloat((float)p->armJaguar->GetOutputCurrent());
dash.AddFloat((float)p->elevatorJaguarMotorA->GetOutputCurrent());
dash.AddFloat(35); // pincher % open
dash.AddU32(dashboardIndex);
dash.AddFloat(p->matchTimer->GetElapsedTime());
dash.AddFloat((float)p->systemTimer->Get());
int x = p->GetTargetPole();
int y = (p->GetTargetFoot()+1)/2;
dash.AddU8(x);
dash.AddU8(y);
dash.AddFloat(p->navigation->GetHeadingToPeg(x));
dash.AddFloat(p->navigation->GetHeadingToBait(x));
dash.AddFloat(p->navigation->GetDistanceToPeg(x));
dash.AddFloat(p->navigation->GetDistanceToBait(x));
dash.AddFloat(p->magEncoder->GetVoltage());
// dash.AddFloat(p->arm->magEncoder->GetVoltage());
dash.AddFloat(42.0);
dash.AddU32(0xDCBA25); // temporary test data to make sure data is aligned
dash.FinalizeCluster();
dash.Finalize();
}
float Cypress::GetAnalogIn(int channel) {
return m_ds->GetEnhancedIO().GetAnalogIn(channel);
}
void Cypress::SetDigitalOut(int channel, bool value) {
m_ds->GetEnhancedIO().SetDigitalOutput(channel, value);
}
void Arm::DriveArm() {
float claw_target= CLAW_OPEN;
DriverStation *ds = DriverStation::GetInstance();
if(ds->GetDigitalIn(3) && !ds->GetDigitalIn(4))
{
float tower_target = (ds->GetEnhancedIO().GetAnalogInRatio(1) * (TOWER_MAX - TOWER_MIN)) + TOWER_MIN;
float wrist_target = (ds->GetEnhancedIO().GetAnalogInRatio(3) * (WRIST_MAX-WRIST_MIN)) + WRIST_MIN;
if (tower_target > TOWER_MAX)
tower_target = TOWER_MAX;
else if (tower_target < TOWER_MIN)
tower_target = TOWER_MIN;
if (wrist_target < WRIST_MIN)
wrist_target = WRIST_MIN;
if (wrist_target > WRIST_MAX)
wrist_target = WRIST_MAX;
if (ds->GetDigitalIn(1)) {
claw_target = CLAW_CLOSED;
}
float tower_voltage = towerPID->GetOutput(GetTowerPot(), tower_target, 0,
.03) * -1;
float claw_voltage = clawPID->GetOutput(GetClawPot(), claw_target, 0, .1);
float wrist_voltage = wristPID->GetOutput(GetWristPot(), wrist_target, 0,
.2) * -1;
if(!towerLimit->Get() && tower_voltage < 0)
{
tower_voltage = 0;
towerMotor->Set(0);
}
if(fabs(claw_voltage) > .5)
wrist_voltage = 0;
towerMotor->Set(tower_voltage);
wristMotor->Set(wrist_voltage);
clawMotor->Set(claw_voltage);
}
else if(ds->GetDigitalIn(3) && ds->GetDigitalIn(4))
{
float tower_voltage = towerPID->GetOutput(GetTowerPot(), TOWER_AUTO_DOWN, 0,
.03) * -1;
float claw_voltage = clawPID->GetOutput(GetClawPot(), CLAW_OPEN, 0, .1);
float wrist_voltage = wristPID->GetOutput(GetWristPot(), WRIST_AUTO_DOWN, 0,
.2) * -1;
towerMotor->Set(tower_voltage);
wristMotor->Set(wrist_voltage);
clawMotor->Set(claw_voltage);
}
else if(!ds->GetDigitalIn(3) && ds->GetDigitalIn(4))
{
float tower_voltage = towerPID->GetOutput(GetTowerPot(), TOWER_AUTO_UP, 0,
.03) * -1;
float claw_voltage = clawPID->GetOutput(GetClawPot(), CLAW_CLOSED, 0, .1);
float wrist_voltage = wristPID->GetOutput(GetWristPot(), WRIST_AUTO_UP, 0,
.2) * -1;
towerMotor->Set(tower_voltage);
wristMotor->Set(wrist_voltage);
clawMotor->Set(claw_voltage);
}
}
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
}
}
/**
* 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();
}
}
/**
* unchanged from SimpleDemo:
*
* Runs the motors under driver control with either tank or arcade
* steering selected by a jumper in DS Digin 0.
*
* added for vision:
*
* Adjusts the servo gimbal based on the color tracked. Driving the
* robot or operating an arm based on color input from gimbal-mounted
* camera is currently left as an exercise for the teams.
*/
void OperatorControl(void)
{
char funcName[] = "OperatorControl";
DPRINTF(LOG_DEBUG, "OperatorControl");
//GetWatchdog().Feed();
TrackingThreshold td = GetTrackingData(GREEN, FLUORESCENT);
/* for controlling loop execution time */
float loopTime = 0.05;
double currentTime = GetTime();
double lastTime = currentTime;
double savedImageTimestamp = 0.0;
bool foundColor = false;
bool staleImage = false;
while (IsOperatorControl())
{
setServoPositions(rightStick->GetX(), rightStick->GetY());
/* calculate gimbal position based on color found */
if (FindColor
(IMAQ_HSL, &td.hue, &td.saturation, &td.luminance, &par,
&cReport))
{
foundColor = true;
if (par.imageTimestamp == savedImageTimestamp)
{
// This image has been processed already,
// so don't do anything for this loop
staleImage = true;
}
else
{
staleImage = false;
savedImageTimestamp = par.imageTimestamp;
// compute final H & V destination
horizontalDestination = par.center_mass_x_normalized;
verticalDestination = par.center_mass_y_normalized;
}
}
else
{
foundColor = false;
}
PrintReport(&cReport);
if (!staleImage)
{
if (foundColor)
{
/* Move the servo a bit each loop toward the
* destination. Alternative ways to task servos are
* to move immediately vs. incrementally toward the
* final destination. Incremental method reduces the
* need for calibration of the servo movement while
* moving toward the target.
*/
ShowActivity
("** %s found: Servo: x: %f y: %f",
td.name, horizontalDestination, verticalDestination);
}
else
{
ShowActivity("** %s not found", td.name);
}
}
dashboardData.UpdateAndSend();
// sleep to keep loop at constant rate
// elapsed time can vary significantly due to debug printout
currentTime = GetTime();
lastTime = currentTime;
if (loopTime > ElapsedTime(lastTime))
{
Wait(loopTime - ElapsedTime(lastTime));
}
}
while (IsOperatorControl())
{
// determine if tank or arcade mode; default with no jumper is
// for tank drive
if (ds->GetDigitalIn(ARCADE_MODE) == 0)
{
// drive with tank style
myRobot->TankDrive(leftStick, rightStick);
}
else
{
// drive with arcade style (use right stick)
myRobot->ArcadeDrive(rightStick);
}
}
} // end operator control
void RhsRobotBase::StartCompetition() //Robot's main function
{
DriverStation *pDS = DriverStation::GetInstance();
Init(); //Initialize the robot
while(true)
{
if(!pDS->IsNewControlData())
{
Wait(0.002);
continue;
}
//Checks the current state of the robot
if(IsDisabled())
{
currentRobotState = ROBOT_STATE_DISABLED;
}
else if(IsEnabled() && IsAutonomous())
{
currentRobotState = ROBOT_STATE_AUTONOMOUS;
}
else if(IsEnabled() && IsOperatorControl())
{
currentRobotState = ROBOT_STATE_TELEOPERATED;
}
else if(IsEnabled() && IsTest())
{
currentRobotState = ROBOT_STATE_TEST;
}
else
{
currentRobotState = ROBOT_STATE_UNKNOWN;
}
if(HasStateChanged()) //Checks for state changes
{
switch(GetCurrentRobotState())
{
case ROBOT_STATE_DISABLED:
printf("ROBOT_STATE_DISABLED\n");
robotMessage.command = COMMAND_ROBOT_STATE_DISABLED;
//robotMessage.robotMode = ROBOT_STATE_DISABLED;
break;
case ROBOT_STATE_AUTONOMOUS:
printf("ROBOT_STATE_AUTONOMOUS\n");
robotMessage.command = COMMAND_ROBOT_STATE_AUTONOMOUS;
//robotMessage.robotMode = ROBOT_STATE_AUTONOMOUS;
break;
case ROBOT_STATE_TELEOPERATED:
printf("ROBOT_STATE_TELEOPERATED\n");
robotMessage.command = COMMAND_ROBOT_STATE_TELEOPERATED;
//robotMessage.robotMode = ROBOT_STATE_TELEOPERATED;
break;
case ROBOT_STATE_TEST:
printf("ROBOT_STATE_TEST\n");
robotMessage.command = COMMAND_ROBOT_STATE_TEST;
//robotMessage.robotMode = ROBOT_STATE_TEST;
break;
case ROBOT_STATE_UNKNOWN:
printf("ROBOT_STATE_UNKNOWN\n");
robotMessage.command = COMMAND_ROBOT_STATE_UNKNOWN;
//robotMessage.robotMode = ROBOT_STATE_UNKNOWN;
break;
}
OnStateChange(); //Handles the state change
}
if(IsEnabled())
{
if((currentRobotState == ROBOT_STATE_TELEOPERATED) ||
(currentRobotState == ROBOT_STATE_TEST) ||
(currentRobotState == ROBOT_STATE_AUTONOMOUS))
{
Run(); //Robot logic
}
}
previousRobotState = currentRobotState;
++loop; //Increment the loop counter
}
}
