/*TODO: fire(); code to hold the servos in place on the camera fix exposure/brightness configuration on camera set to lowest value possible(right now it's at 40,50? so 0?) make sure the switch between manual and auto turret works (that'll be quick) */ #include #include "math.h" #include #include "AxisCamera.h" #include "BaeUtilities.h" #include "FrcError.h" #include "TrackAPI.h" #include "WPILib.h" #include "DashboardDataFormat.h" #include "vxWorks.h" #define MIN_PARTICLE_TO_IMAGE_PERCENT 0.25 // target is too small #define MAX_PARTICLE_TO_IMAGE_PERCENT 10.0 // target is too close /** * This is a demo program showing the use of the color tracking API. * It uses the SimpleRobot class as a base of a robot application that will automatically call * your Autonomous and OperatorControl methods at the right time as controlled by the switches * on the driver station or the field controls. Autonomous mode tracks color, assuming a * camera on a fixed mount. */ class OurRobot : public SimpleRobot { RobotDrive *myRobot; // robot drive system Joystick *turretStick; //for manual control of the turret Joystick *rightStick; // joystick 1 (arcade stick) Joystick *leftStick; DriverStation *ds; // driver station object TrackingThreshold tdataG,tdataP; // image data for tracking ParticleAnalysisReport cameraReportGreen; //we will use this in aim() and fire() ParticleAnalysisReport cameraReportPink; //we will use this in aim() and fire() Victor turret; //the Victor that controls the turret and camera turning bool team; //our team colors: 1- green above pink, 0- pink above green maybe change this Jaguar turretright;//always set these two to the same power! Jaguar turretleft; Victor lifter; Victor frontRollers; enum // Driver Station jumpers to control program operation { ARCADE_MODE = 1, // Tank/Arcade jumper is on DS Input 1 (Jumper present is arcade) ENABLE_AUTONOMOUS = 2, // Autonomous/Teleop jumper is on DS Input 2 (Jumper present is autonomous) } jumpers; public://camera x and y is flipped! /** * Constructor for this robot subclass. * Create an instance of a RobotDrive with left and right motors plugged into PWM * ports 1 and 2 on the first digital module. */ OurRobot(void) : turret(7), turretright(3), turretleft(1), lifter(8), frontRollers(5){ GetWatchdog().SetEnabled(false); /* if (StartCameraTask(13, 0, k160x120, ROT_0) == -1) { dprintf( LOG_ERROR,"Failed to spawn camera task; exiting. Error code %s", GetVisionErrorText(GetLastVisionError()) ); } dprintf(LOG_DEBUG,"Waiting for camera to initialize"); Wait(2.0); // start up the task serving images to PC dprintf(LOG_DEBUG,"Starting image server"); PCVideoServer pc; dprintf(LOG_DEBUG,"Image server Running"); // pc.Start(); */ printf("--------123-----------HEY OUR ROBOT PROGRAM IS STARTED ----------\n"); ds = DriverStation::GetInstance(); myRobot = new RobotDrive(4, 2, 0.5); // robot will use PWM 4,2 for drive motors turretStick = new Joystick(3); rightStick = new Joystick(1); // create the joysticks leftStick = new Joystick(2); team = true; SetDebugFlag(DEBUG_SCREEN_ONLY); // dprintf output goes to terminal/console /*// values for tracking a target - may need tweaking in your environment // see TrackAPI.cpp for threshold values tdataG = GetTrackingData(GREEN, FLUORESCENT); tdataP = GetTrackingData(PINK, FLUORESCENT); */ //GetWatchdog().SetEnabled(false); GetWatchdog().SetExpiration(200); } bool aim() { bool areWeAimed = false; ParticleAnalysisReport parGreen; // particle analysis reports ParticleAnalysisReport parPink; //check how well-aimed we are based on where the target is on the camera FindColor(IMAQ_HSL, &tdataG.hue, &tdataG.saturation, &tdataG.luminance, &parGreen); FindColor(IMAQ_HSL, &tdataP.hue, &tdataP.saturation, &tdataP.luminance, &parPink); //make sure we are aiming at an opponent, not a friend if(team && cameraReportGreen.center_mass_x < cameraReportPink.center_mass_x)//if our team is green over pink { return false; } else if(!team && cameraReportPink.center_mass_x < cameraReportGreen.center_mass_x)//if our team is pink over green { return false; } int centerOfImage = cameraReportGreen.imageHeight / 2;//I'm just using green here instead of pink. int targetLocation = cameraReportGreen.center_mass_y; for(int count = centerOfImage - 5; count <= centerOfImage + 5; count++) { if(targetLocation == count) { areWeAimed = true; } } //if not well-aimed, turn the turret to the right or left, depending on where the target is if(!areWeAimed && targetLocation < centerOfImage) { turret.Set(1); } else if(!areWeAimed && targetLocation > centerOfImage) { turret.Set(-1); } if(areWeAimed) { //turret.Set(0); we want to keep tracking them //fire(); return true; } else { return false; } } /* void search() { printf("searching for a victim..."); //look for a new target to chase or shoot at } I don't think this will be necessary */ void Autonomous(void) { GetWatchdog().SetEnabled(true); GetWatchdog().Feed(); ParticleAnalysisReport parGreen; // particle analysis reports ParticleAnalysisReport parPink; lifter.Set(-1); frontRollers.Set(-1);//we want the front rollers and lifter to run for the duration of autonomous. myRobot->TankDrive(1, 1); Wait(3); myRobot->TankDrive(0.0, 0.0); while (IsAutonomous()) {//this may not work because our camera is not fixed /* this simple test will drive toward GREEN * drive will last until autonomous terminates */ // delay in loop. For frame rate of 10 fps, a new image is available every 100 ms. // put the Wait at the beginning vs end of loop because the first time through // the camera is still initializing. Wait(.05); GetWatchdog().Feed(); FindColor(IMAQ_HSL, &tdataG.hue, &tdataG.saturation, &tdataG.luminance, &parGreen); FindColor(IMAQ_HSL, &tdataP.hue, &tdataP.saturation, &tdataP.luminance, &parPink); //make sure we are aiming at an opponent, not a friend if(team && parGreen.center_mass_x < parPink.center_mass_x)//if our team is green over pink { myRobot->Drive(0.0, 0.0); //implement some kind of tracking function to look for a new target } else if(!team && parPink.center_mass_x < parGreen.center_mass_x)//if our team is pink over green { myRobot->Drive(0.0, 0.0); //implement some kind of tracking function to look for a new target } if ( //FindColor(IMAQ_HSL, &tdata.hue, &tdata.saturation, &tdata.luminance, &parGreen) parGreen.particleToImagePercent < MAX_PARTICLE_TO_IMAGE_PERCENT && parGreen.particleToImagePercent > MIN_PARTICLE_TO_IMAGE_PERCENT ){ // drive toward color myRobot->Drive((float)1.0, (float)parGreen.center_mass_y_normalized); } else { myRobot->Drive(0.0, 0.0); // stop robot //implement some kind of tracking function to look for a new target } if(parGreen.center_mass_y_normalized == 0)//we may change this to give some leeway in shooting { fire(); } } // end while myRobot->Drive(0.0, 0.0); // stop robot } void fire(void) { printf("fire"); } /** * Runs the motors under driver control with either tank or arcade steering selected * by a jumper in DS Digin 0. */ void OperatorControl(void) { GetWatchdog().SetEnabled(true); while ( IsOperatorControl() ) { GetWatchdog().Feed(); turret.Set(-turretStick->GetX()); if(turretStick->GetRawButton(1)) { double zVal = -turretStick->GetZ(); // printf("Not normalized: %f ", zVal); zVal = (zVal + 1) / 2; //printf(" normalized: %f \n", zVal); turretright.Set(zVal); turretleft.Set(-zVal); lifter.Set(-1); } else if(turretStick->GetRawButton(2)) { turretright.Set(-.5); turretleft.Set(.5); } else { turretright.Set(0); turretleft.Set(0); } if(leftStick->GetTrigger()) { lifter.Set(-1); frontRollers.Set(-1); } else if(rightStick->GetTrigger()) { lifter.Set(1); frontRollers.Set(1); } else { if(!turretStick->GetRawButton(1)) lifter.Set(0); frontRollers.Set(0); } myRobot->TankDrive(-rightStick->GetY(), -leftStick->GetY()); } } }; // entry point is FRC_UserProgram_StartupLibraryInit START_ROBOT_CLASS(OurRobot);