JNIEXPORT void JNICALL Java_org_pointclouds_bodyparts_Grabber_stop (JNIEnv * env, jobject object) { Grabber * ptr = getPtr<Grabber>(env, object); ptr->stop(); }
JNIEXPORT jboolean JNICALL Java_org_pointclouds_bodyparts_Grabber_isConnected (JNIEnv * env, jobject object) { Grabber * ptr = getPtr<Grabber>(env, object); return ptr->isConnected(); }
JNIEXPORT void JNICALL Java_org_pointclouds_bodyparts_Grabber_getFrame (JNIEnv * env, jobject object, jobject frame) { Grabber * ptr = getPtr<Grabber>(env, object); Cloud * frame_ptr = getPtr<Cloud>(env, frame); ptr->getFrame(*frame_ptr); }
void grabAndSend () { OpenNIGrabber* grabber = new OpenNIGrabber (); grabber->getDevice ()->setDepthOutputFormat (depth_mode_); Grabber* interface = grabber; boost::function<void (const typename PointCloud<PointT>::ConstPtr&)> f = boost::bind (&Producer::grabberCallBack, this, _1); interface->registerCallback (f); interface->start (); while (true) { if (is_done) { break; } char c; { // boost::mutex::scoped_lock io_lock (io_mutex); c = getch(); } if (c == 'q') { boost::mutex::scoped_lock io_lock (io_mutex); print_info ("\n'q' detected, exit condition set to true.\n"); is_done = true; } else if (!is_done && c == 's') { { boost::mutex::scoped_lock wflag_lock (wflag_mutex); write_once = true; } } else if (!is_done && c == 'n'){ boost::mutex::scoped_lock io_lock (noise_flag_mutex); if (!calculate_noise) { calculate_noise = true; num_pcd_sample = 100; depths_sample.clear(); } } boost::this_thread::sleep (boost::posix_time::seconds (1)); } interface->stop (); }
void grabAndSend () { OpenNIGrabber* grabber = new OpenNIGrabber (); grabber->getDevice ()->setDepthOutputFormat (depth_mode_); Grabber* interface = grabber; boost::function<void (const typename PointCloud<PointT>::ConstPtr&)> f = boost::bind (&Producer::grabberCallBack, this, _1); interface->registerCallback (f); interface->start (); while (true) { if (is_done) break; boost::this_thread::sleep (boost::posix_time::seconds (1)); } interface->stop (); }
int main () { Grabber v; v.run (); //_CrtDumpMemoryLeaks(); return (0); //int tst; //std::cin >> tst; //audio::audioParams params; //params.freq = 440.f; //params.attack = 0.1; //params.decay = 0.1; //params.release = 0.1; //params.sustain = 0.1; //params.texture = 0.1; //params.dampThres = 0.5; //params.overtone_amps.push_back(0.9); //params.overtone_amps.push_back(0.3); //params.overtone_amps.push_back(0.9); //params.overtone_amps.push_back(0.3); ///* Hue Names: http://www.procato.com/rgb+index/ // * 0 = Red 392 // * 30 = Orange 440 // * 60 = Yellow 466.16 // * 120 = Green 523.25 // * 180 = Cyan 554.37 // * 240 = Blue 587.33 // * 300 = Magenta 698.46 // * 360 = Red // * Frequency adjusted to scale based on A4=440.f // */ //std::map<int, int> colorBins; // //for(int i =0; i<tst; i++) // AE.createSound(params); //AE.playSounds(); }
/** * Runs during test mode ``````` */ void Test() { TestMode tester(m_ds); driveDistanceRight.Reset(); driveDistanceRight.Start(); ballGrabber.resetSetPoint(); shooter.motorShutOff(); while (IsTest() && IsEnabled()){ lcd->Clear(); tester.PerformTesting(&gamePad, &driveDistanceRight, lcd, &rightJoyStick, &leftJoyStick, &testSwitch, &testTalons, &frontUltrasonic, &backUltrasonic, &ballGrabber.ballDetector, &analogTestSwitch, &shooter, &ballGrabber ); lcd->UpdateLCD(); Wait(0.1); } driveDistanceRight.Stop(); }
void OperatorControl() { if(!m_FromAutonomous){ init(); } m_FromAutonomous = false; driveTrain.SetSafetyEnabled(true); int printDelay = 0; int shootDelay = 0; //bool SavePreferencesToFlash = false; while (IsOperatorControl() && IsEnabled()) { /* bool SavePreferences = gamePad.GetRawButton(8); if (SavePreferences){ double elevatorAngleValue = SmartDashboard::GetNumber("Angle"); dashboardPreferences->PutDouble("Angle", elevatorAngleValue); SavePreferencesToFlash = true; } */ printDelay ++; float rJoyStick = limitSpeed(rightJoyStick.GetY()); float lJoyStick = limitSpeed(leftJoyStick.GetY()); bool button6 = gamePad.GetRawButton(6); //speedLimiter.SetMaxOutput(SmartDashboard::GetNumber("Slider 1")); driveTrain.TankDrive(lJoyStick, rJoyStick); //manual mode(no PID) for elevator //float dPadThumbstick = TestMode::GetThumbstickWithZero(&gamePad); //ballGrabber.DriveElevatorTestMode(dPadThumbstick); //Sets motor equal to the elevator sensor. //TODO Probably don't need but want to test because called inside operate grabber. ballGrabber.OperatePIDLoop(); if(printDelay == 100){ lcd->Clear(); if(m_display_page_1) { lcd->PrintfLine(DriverStationLCD::kUser_Line1, "Teleop pg1"); lcd->PrintfLine(DriverStationLCD::kUser_Line2, "FR %4.0f, BA %4.0f", frontUltrasonic.GetAverageDistance(), backUltrasonic.GetAverageDistance()); shooter.PrintShooterState(DriverStationLCD::kUser_Line3, lcd); SmartDashboard::PutNumber("UltrasonicF", 1); SmartDashboard::PutNumber("UltrasonicB", 1); SmartDashboard::PutNumber("ElvatorAngle", 2);//Change keyname to ElavatorAngle from (ElvatorAngle) //^^ if(button6){ m_display_page_1 = false; } } else{ lcd->PrintfLine(DriverStationLCD::kUser_Line1, "Teleop pg2 %c", button6 ? '1':'0'); ballGrabber.DisplayDebugInfo(DriverStationLCD::kUser_Line2,lcd); //lcd->PrintfLine(DriverStationLCD::kUser_Line3, "G%f", ballGrabber.ballDetector.GetDistance()); //ballGrabber.UpDateWithState(DriverStationLCD::kUser_Line3,lcd); shooter.PrintShooterState(DriverStationLCD::kUser_Line3, lcd); //lcd->PrintfLine(DriverStationLCD::kUser_Line4, "EV%6.2f", ballGrabber.elevatorAngleSensor.GetVoltage()); shooter.DisplayDebugInfo(DriverStationLCD::kUser_Line4, lcd); //lcd->PrintfLine(DriverStationLCD::kUser_Line4, "%5.3f %5.3f %5.3f", lJoyStick, rJoyStick, SmartDashboard::GetNumber("Slider 1")); lcd->PrintfLine(DriverStationLCD::kUser_Line5, "DEV=%6.3f", ballGrabber.m_desiredElevatorVoltage); lcd->PrintfLine(DriverStationLCD::kUser_Line6, "CEV=%5.2f", ballGrabber.elevatorAngleSensor.GetVoltage()); if(button6){ m_display_page_1 = true; } } lcd->UpdateLCD(); printDelay = 0; } //int rotation = elevation.Get(); //the above is commented because we are not using it yet bool shooterButton = gamePad.GetRawButton(7) || gamePad.GetRawButton(8);//TODO make constants bool automaticAimButton = gamePad.GetRawButton(1); //float distanceToWall = frontUltrasonic.GetAverageDistance(); //bool loadShooterButton = gamePad.GetRawButton(8); if (shooterButton && shootDelay == 0){ shootDelay++; } if(shootDelay>0){ shootDelay++; } bool ReadyToShoot = (shootDelay>PHOENIX2014_LOOP_COUNT_FOR_SHOOT_DELAY); shooter.OperateShooter(ReadyToShoot); if (ReadyToShoot){ shootDelay = 0; } bool okToGrab = (shootDelay == 0);//Normaly 0 unless delaying ballGrabber.OperateGrabber(shooterButton, okToGrab); //Trying to make some things happen automatically during teleoperated if(automaticAimButton){ ballGrabber.m_desiredElevatorVoltage = PHOENIX2014_TELEOP_ELEVATOR_ANGLE; } //((distanceToWall > (12.0*11.0)) && distanceToWall < (12.0*13.0)){ //lightBulb.Set(Relay::kOn); //} //else{ #ifdef WANTWEIRDPULSING if (printDelay < 30) { lightBulb.Set(Relay::kForward); } else if (printDelay >= 30 && printDelay < 65) { lightBulb.Set(Relay::kReverse); } else { lightBulb.Set(Relay::kOff); } #endif Wait(0.005);// wait for a motor update time } // end of while enabled driveTrain.StopMotor(); ballGrabber.StopPidLoop(); shooter.motorShutOff(); /* if(SavePreferencesToFlash){ dashboardPreferences->Save(); SavePreferencesToFlash = false; } */ } // end of OperatorControl()
/** * Drive left & right motors for 2 seconds then stop */ void Autonomous() { init(); lcd->PrintfLine(DriverStationLCD::kUser_Line1, "Entered Autonomous"); driveTrain.SetSafetyEnabled(false); bool checkBox1 = SmartDashboard::GetBoolean("Checkbox 1"); m_FromAutonomous = true; //float rightDriveSpeed = -1.0; //float leftDriveSpeed = -1.0; //int rangeToWallClose = 60; //int rangeToWallFar = 120; //Initialize encoder. //int distanceToShoot = 133; //int shootDelay = 0; //ballGrabber.elevatorController.SetSetpoint(PHOENIX2014_INITIAL_AUTONOMOUS_ELEVATOR_ANGLE); //TODO Remove encoders from code?? driveDistanceRight.Reset(); driveDistanceLeft.Reset(); driveDistanceRight.SetDistancePerPulse(PHOENIX2014_DRIVE_DISTANCE_PER_PULSE_RIGHT); driveDistanceLeft.SetDistancePerPulse(PHOENIX2014_DRIVE_DISTANCE_PER_PULSE_LEFT); driveDistanceRight.Start(); driveDistanceLeft.Start(); //int printDelay = 0; float minDistance = 52.0; // Closer to the wall than this is too close float maxDistance = 12.0*11.0; // Once at least this close, within shooting range //float closeDistance = maxDistance + 24.0; float autoDriveSpeed = 0.55; //float autoDriveSlowSpeed = 0.38; int time = 0; int maxDriveLoop = 4*200; // 4 seconds @200 times/sec bool shootingBall = false; bool wantFirstShot = true; if(checkBox1 == false){ int printDelay = 0; //Ultrasonic Autonomous //bool isInRange = false; while(IsAutonomous() && IsEnabled()) { float currentDistance = frontUltrasonic.GetAverageDistance(); // Transition isInRange from false to true once if((minDistance < currentDistance) && (currentDistance < maxDistance)){ //isInRange = true; } time++; bool motorDriveTimeExceeded = time > maxDriveLoop; bool motorMinMet = time > m_MinDriveLoop; if(/*isInRange ||*/ motorMinMet){ driveTrain.TankDrive(0.0,0.0); if((ballGrabber.elevatorAngleSensor.GetVoltage() > PHOENIX2014_AUTONOMOUS_ELEVATOR_ANGLE - 0.1) && (ballGrabber.elevatorAngleSensor.GetVoltage() < PHOENIX2014_AUTONOMOUS_ELEVATOR_ANGLE + 0.1)){ //Enough to cover break release and start winding again. shootingBall = shooter.OperateShooter(wantFirstShot); } if(shootingBall == true){ wantFirstShot = false; } } else if(motorDriveTimeExceeded){ driveTrain.TankDrive(0.0,0.0); } else{ //if((currentDistance < closeDistance) && motorMinMet){ // autoDriveSpeed = autoDriveSlowSpeed; //} driveTrain.TankDrive(-0.97 * autoDriveSpeed, -1.0 * autoDriveSpeed);//the DriveTrain is BACKWARD } ballGrabber.RunElevatorAutonomous(PHOENIX2014_AUTONOMOUS_ELEVATOR_ANGLE); printDelay = printDelay++; if(printDelay >= 200){ lcd->Clear(); lcd->PrintfLine(DriverStationLCD::kUser_Line1, "In Autonomous"); shooter.DisplayDebugInfo(DriverStationLCD::kUser_Line2, lcd); shooter.PrintShooterState(DriverStationLCD::kUser_Line3, lcd); lcd->PrintfLine(DriverStationLCD::kUser_Line4, "Ulra=%f", frontUltrasonic.GetAverageDistance()); //lcd->PrintfLine(DriverStationLCD::kUser_Line5, "CEV=%f", ballGrabber.elevatorAngleSensor.GetVoltage()); //lcd->PrintfLine(DriverStationLCD::kUser_Line6, "DEV=%f", ballGrabber.m_desiredElevatorVoltage); lcd->UpdateLCD(); printDelay = 0; } Wait(0.005); } lcd->Clear(); lcd->UpdateLCD(); lcd->PrintfLine(DriverStationLCD::kUser_Line2, "Exiting Autonomous"); } else { //Timer Autonomous driveTrain.TankDrive(-0.5,-0.5); ballGrabber.DriveElevatorTestMode(-1.0); lcd->Clear(); lcd->PrintfLine(DriverStationLCD::kUser_Line1, "Skip Auto"); lcd->PrintfLine(DriverStationLCD::kUser_Line2, "CheckBox Checked"); lcd->UpdateLCD(); Wait(2.0); bool shooting = true; driveTrain.TankDrive(0.0,0.0); int counter = 0; while(counter < 600){ shooter.OperateShooter(shooting); Wait(0.005); } } }
//this called when the robot is enabled void init(){ shooter.init(); ballGrabber.init(); }
int FFGrabber::doCapture() { AVbinPacket packet; packet.structure_size = sizeof(packet); streammap::iterator tmp; int needseek=1; bool allDone = false; while (!avbin_read(file, &packet)) { if ((tmp = streams.find(packet.stream_index)) != streams.end()) { Grabber* G = tmp->second; G->Grab(&packet); if (G->done) { allDone = true; for (streammap::iterator i = streams.begin(); i != streams.end() && allDone; i++) { allDone = allDone && i->second->done; } } #ifdef MATLAB_MEX_FILE if (!G->isAudio) runMatlabCommand(G); #endif } else if (DEBUG) FFprintf("Unknown packet %d\n",packet.stream_index); if (tryseeking && needseek) { if (stopTime && startTime > 0) { if (DEBUG) FFprintf("try seeking to %lf\n",startTime); av_seek_frame(file->context, -1, (AVbinTimestamp)(startTime*1000*1000), AVSEEK_FLAG_BACKWARD); } needseek = 0; } if (allDone) { if (DEBUG) FFprintf("stopForced\n"); stopForced = true; break; } } #ifdef MATLAB_MEX_FILE if (prhs[0]) { mxDestroyArray(prhs[0]); if (prhs[1]) mxDestroyArray(prhs[1]); if (prhs[2]) mxDestroyArray(prhs[2]); if (prhs[3]) mxDestroyArray(prhs[3]); if (prhs[4]) mxDestroyArray(prhs[4]); } prhs[0] = NULL; #endif return 0; }