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;
}
