RobotSystem(void): robotInted(false) ,stick(1) // as they are declared above. ,stick2(2) ,line1(10) ,line2(11) ,line3(12) //,camera(AxisCamera::GetInstance()) ,updateCAN("CANUpdate",(FUNCPTR)UpdateCAN) ,cameraTask("CAMERA", (FUNCPTR)CameraTask) ,compressor(14,1) ,EncArm(2,3) ,EncClaw(5,6) ,PIDArm(.04,0,0) // .002, .033 ,PIDClaw(.014,.0000014,0) ,LowArm(.1) /* ,MiniBot1(4) ,MiniBot2(2) ,ClawGrip(3) */ ,MiniBot1a(8,1) ,MiniBot1b(8,2) ,MiniBot2a(8,3) ,MiniBot2b(8,4) ,ClawOpen(8, 8) ,ClawClose(8,7) ,LimitClaw(7) ,LimitArm(13) { // myRobot.SetExpiration(0.1); GetWatchdog().SetEnabled(false); GetWatchdog().SetExpiration(1); compressor.Start(); debug("Waiting to init CAN"); Wait(2); Dlf = new CANJaguar(6,CANJaguar::kSpeed); Dlb = new CANJaguar(3,CANJaguar::kSpeed); Drf = new CANJaguar(7,CANJaguar::kSpeed); Drb = new CANJaguar(2,CANJaguar::kSpeed); arm1 = new CANJaguar(5); arm1_sec = new CANJaguar(8); arm2 = new CANJaguar(4); EncArm.SetDistancePerPulse(.00025); EncClaw.SetDistancePerPulse(.00025); EncClaw.SetReverseDirection(false); EncArm.SetReverseDirection(true); EncArm.Reset(); EncClaw.Reset(); updateCAN.Start((int)this); //cameraTask.Start((int)this); EncArm.Start(); EncClaw.Start(); debug("done initing"); }
void OperatorControl(void) { myRobot->SetSafetyEnabled(false); LEDLights (true); //turn camera lights on shooterspeedTask->Start((UINT32)this); //start counting shooter speed kickerTask->Start((UINT32)this); //turns on the kicker task kicker_in_motion = false; while (IsOperatorControl() && !IsDisabled()) { if (ControllBox->GetDigital(3)) //turn tracking on with switch 3 on controll box { tracking(ControllBox->GetDigital(7)); } else { myRobot->TankDrive(leftstick, rightstick); //if tracking is off, enable human drivers Wait(0.005); // wait for a motor update time } Shooter_onoff=ControllBox->GetDigital(4); //shoot if switch 4 is on ballgatherer(ControllBox->GetDigital(5), rightstick->GetRawButton(10)); kicker_onoff=lonelystick->GetRawButton(1); bridgeboot(ControllBox->GetDigital(6)); kicker_cancel=lonelystick->GetRawButton(2); //kicker_down=rightstick->GetRawButton(11)); } LEDLights (false); shooterspeedTask->Stop(); kickerTask->Stop(); ballgatherer(false, false); kickermotor->Set(Relay::kOff); }
CANRobotDemo() : speedJag(2, CANJaguar::kSpeed) , stick(1) , axis(Joystick::kXAxis) , commandTask("DashboardCommandServer", (FUNCPTR)DashboardCommandServerStub) { GetWatchdog().SetExpiration(100); speedJag.ConfigEncoderCodesPerRev(360); speedJag.ConfigMaxOutputVoltage(6.0); speedJag.ConfigNeutralMode(CANJaguar::kNeutralMode_Brake); wpi_stackTraceEnable(true); commandTask.Start((INT32)this); }
/** * * Start the robot code. * This function starts the robot code running by spawning a task. Currently tasks seemed to be * started by LVRT without setting the VX_FP_TASK flag so floating point context is not saved on * interrupts. Therefore the program experiences hard to debug and unpredictable results. So the * LVRT code starts this function, and it, in turn, starts the actual user program. */ void RobotBase::startRobotTask(FUNCPTR factory) { #ifdef SVN_REV if (strlen(SVN_REV)) { printf("WPILib was compiled from SVN revision %s\n", SVN_REV); } else { printf("WPILib was compiled from a location that is not source controlled.\n"); } #else printf("WPILib was compiled without -D'SVN_REV=nnnn'\n"); #endif // Check for startup code already running int32_t oldId = taskNameToId(const_cast<char*>("FRC_RobotTask")); if (oldId != ERROR) { // Find the startup code module. char moduleName[256]; moduleNameFindBySymbolName("FRC_UserProgram_StartupLibraryInit", moduleName); MODULE_ID startupModId = moduleFindByName(moduleName); if (startupModId != NULL) { // Remove the startup code. unldByModuleId(startupModId, 0); printf("!!! Error: Default code was still running... It was unloaded for you... Please try again.\n"); return; } // This case should no longer get hit. printf("!!! Error: Other robot code is still running... Unload it and then try again.\n"); return; } // Let the framework know that we are starting a new user program so the Driver Station can disable. FRC_NetworkCommunication_observeUserProgramStarting(); // Let the Usage Reporting framework know that there is a C++ program running nUsageReporting::report(nUsageReporting::kResourceType_Language, nUsageReporting::kLanguage_CPlusPlus); RobotBase::WriteVersionString(); // Start robot task // This is done to ensure that the C++ robot task is spawned with the floating point // context save parameter. Task *task = new Task("RobotTask", (FUNCPTR)RobotBase::robotTask, Task::kDefaultPriority, 64000); task->Start((int32_t)factory, (int32_t)task); }
/** * Drive left & right motors for 2 seconds then stop */ void Autonomous(void) { myRobot->SetSafetyEnabled(false); //shooter on kickerTask->Start((UINT32)this); shooterspeedTask->Start((UINT32)this); Shooter_onoff=true; //if (speederror < 10); //track+adjust LEDLights(true); //turn tracking on //while (tracking(false) == false) { // } //if while returns true, then shoot //might have to wait for encoder once capabilities have been enabled Wait(2.0); AutonomousShooting(true); AutonomousShooting(true); AutonomousShooting(true); //load /* Wait(1.0); kicker_onoff = true; while(kicker_in_motion == false) { Wait(0.005); printf ("kicker_onoff is false, kicker_onoff is true\n"); } kicker_onoff = false; while (kicker_in_motion == true) { Wait(0.005); printf ("kicker_in_motion is true, kicker_onoff is false\n"); } //shoot, by itself, because the shooter motor was already on. //gather ballgatherer(true, false); Wait(4.0); printf ("ballgatherer on\n"); ballgatherer(false, false); printf ("ballgatherer off\n"); //load kicker_onoff = true; while(kicker_in_motion == false) { Wait(0.005); printf ("kicker_in_motion is false, kicker_onoff is true\n"); } kicker_onoff = false; while (kicker_in_motion == true) { Wait(0.005); printf ("kicker_in_motion is true, kicker_onoff is false \n"); } //shoot *does by itself because the shooter is already on, AGAIN! :D ballgatherer(true, false); Wait(4.0); printf ("ballgatherer on\n"); ballgatherer(false, false); printf ("ballgatherer off\n"); kicker_onoff = true; while(kicker_in_motion == false) { Wait(0.005); printf ("kicker_onoff is false, kicker_onoff is true\n"); } kicker_onoff = false; while (kicker_in_motion == true) { Wait(0.005); printf ("kicker_in_motion is true, kicker_onoff is false\n"); } //shoot, by itself, because the shooter motor was already on. //gather */ kickerTask->Stop(); shooterspeedTask->Stop(); Shooter_onoff=false; LEDLights(false); }
void Autonomous(void) { Timer t; int step = 0; myRobot.SetSafetyEnabled(false); //Value that indicates error in Image Tracking setTurnOffset(0); setHeight(0); locateBackboard.Start(); susanpid->SetSetpoint(0.0); //if (!susanpid->IsEnabled()) susanpid->Enable(); float launchSpeed; t.Start(); //myRobot.TankDrive(1.0, 1.0); while( IsAutonomous() && !IsDisabled() ) { //Sets the error of imageProcessing //Creates a critical Section to Protect turnOffset double error = -getTurnOffset(); //vex->Set(0.5); //myRobot.TankDrive(0.5, 0.5); //double ratio = getWidth() / 320; //dist = 12.0 / tan(3.14159265358979323846264338 * 23.5 / 180.0) / ratio; //dist = (12 / tan(3.14159265 * 23.5 / 180)) / ((float)getHeight() / 320); dist = 580.0 / (float)getHeight(); dist = ((dist >= 12.0 && dist <= 20.0 ) ? dist : 19.0); //Moves the turret //moveSusan(false, susanSpeed); /*if (step == 0 && t.Get() >= 0.0) { //launchSpeed = scalePower( distToNormalizedPower( dist ) 2.70, DriverStation::GetInstance()->GetBatteryVoltage()); //launchSpeed = max(0.16, min(launchSpeed, 0.25)); //launchSpeed = 0.22; susanpid->Disable(); step++; susanSpeed = 0.0; float backSpin = (true) ? BS : 0; //launchSpeed = scalePower( distToNormalizedPower( dist ), DriverStation::GetInstance()->GetBatteryVoltage()); //launchSpeed = max(0.2, min(launchSpeed, 0.45)); //launchSpeed = 0.217; //Actuates the launchers based on launchSpeed launch1.Set(-launchSpeed - backSpin); launch2.Set(-launchSpeed + backSpin); wheelf.Set(Relay::kOn); t.Reset(); } else if (step == 1 && t.Get() >= 0.3) { step++; //launchSpeed = scalePower( 2.70, DriverStation::GetInstance()->GetBatteryVoltage()); //launchSpeed = max(0.16, min(launchSpeed, 0.25)); launchSpeed = 0.205; wheelf.Set(Relay::kOff); float backSpin = BS; launch1.Set(-launchSpeed * (1 - backSpin) ); launch2.Set(-launchSpeed * (1 + backSpin) ); t.Reset(); } else if (step == 2 && t.Get() >= 5.0) { step++; conveyorf.Set(Relay::kOn); t.Reset(); } else if (step == 3 && t.Get() >= 1.0) { step++; conveyorf.Set(Relay::kOff); t.Reset(); } else if (step == 4 && t.Get() >= 6.5) { step++; conveyorf.Set(Relay::kOn); t.Reset(); } else if (step == 5 && t.Get() >= 15.0) { step++; conveyorf.Set(Relay::kOff); launch1.Set(0.0); launch2.Set(0.0); //wheelf.Set(Relay::kOn); t.Reset(); } */ //2 point shot /*if (step == 0 && t.Get() >= 0.0) { step++; //launchSpeed = scalePower( 2.70, DriverStation::GetInstance()->GetBatteryVoltage()); //launchSpeed = max(0.16, min(launchSpeed, 0.25)); launchSpeed = 0.1625; float backSpin = BS; launch1.Set(-launchSpeed * (1 - backSpin) ); launch2.Set(-launchSpeed * (1 + backSpin) ); t.Reset(); myRobot.ArcadeDrive(0.5, 0.0); } else if (step == 1 && t.Get() >= 5.0) { step++; myRobot.ArcadeDrive(0.0,0.0); wheelf.Set(Relay::kOn); t.Reset(); } else if (step == 2 && t.Get() >= 0.3) { step++; wheelf.Set(Relay::kOff); //conveyorf.Set(Relay::kOn); t.Reset(); } else if (step == 3 && t.Get() >= 1.0) { step++; conveyorf.Set(Relay::kOn); t.Reset(); }else if (step == 4 && t.Get() >= 1.0) { step++; conveyorf.Set(Relay::kOff); t.Reset(); } else if (step == 5 && t.Get() >= 4.5) { step++; conveyorf.Set(Relay::kOn); t.Reset(); } else if (step == 6 && t.Get() >= 15.0) { step++; conveyorf.Set(Relay::kOff); launch1.Set(0.0); launch2.Set(0.0); //wheelf.Set(Relay::kOn); t.Reset(); }*/ /*else if (step == 6 && t.Get() >= 0.5) { step++; wheelf.Set(Relay::kOff); myRobot.ArcadeDrive(0, 0.9, false); t.Reset(); } else if (step == 7 && t.Get() >= 1.8) { step++; myRobot.ArcadeDrive(0, 0, false); t.Reset(); } else if (step == 8 && t.Get() >= 0.3) { step++; myRobot.ArcadeDrive(0.7, 0, false); t.Reset(); } else if (step == 9 && t.Get() >= 2.333333) { step++; myRobot.ArcadeDrive(0, 0, false); t.Reset(); }*/ //Updates Dashboard for feedback text->Clear(); text->Printf(DriverStationLCD::kUser_Line1, 1, "%s", "Team 3324 Autonomous" ); text->Printf(DriverStationLCD::kUser_Line2, 1, "Power: %f", launchSpeed ); text->Printf(DriverStationLCD::kUser_Line3, 1, "Track: %f", getTurnOffset() ); text->Printf(DriverStationLCD::kUser_Line4, 1, "dist: %f", getHeight() ); text->Printf(DriverStationLCD::kUser_Line5, 1, "step: %d, time: %f", step, t.Get()); text->Printf(DriverStationLCD::kUser_Line6, 1, "encoders: %d %d %d %d\n", flEncoder.Get(), frEncoder.Get(), blEncoder.Get(), brEncoder.Get()); text->UpdateLCD(); } }
/** * Image processing code to identify 2013 Vision targets */ void Process(void) { processTask.Start((UINT32) &hotGoal); }