void Camera::setMotionAdjustment(const QVector3D& vector)
{
Q_D(Camera);
if (d->motionAdjustment != vector) {
d->motionAdjustment = vector;
if (vector.x() == 0.0f && vector.y() == 0.0f) {
// If the vector is centered, then don't perform any rotations.
d->motionQuaternion = QQuaternion();
} else {
// Determine the pan and tilt angles from the vector.
QVector3D view = -vector.normalized();
if (view.z() < 0.0f)
view = -view;
qreal xangle = asin(view.x()) * 180.0f / M_PI;
qreal yangle = asin(-view.y()) * 180.0f / M_PI;
// Construct the pan and tilt quaternions.
if (qFuzzyIsNull(xangle))
d->motionQuaternion = tilt(yangle);
else if (qFuzzyIsNull(yangle))
d->motionQuaternion = pan(xangle);
else
d->motionQuaternion = tilt(yangle) * pan(xangle);
}
emit viewChanged();
}
}
int main() {
ssc_init("/dev/ttyUSB0", 9600);
pan( 0 );
tilt( 0 );
cascade = (CvHaarClassifierCascade*)cvLoad(
"cascade.xml", 0, 0, 0 );
if( !cascade )
{
fprintf( stderr, "ERROR: Could not load classifier cascade\n" );
return 1;
}
storage = cvCreateMemStorage(0);
struct camera *cam = init_camera( LCAM );
load_params( cam );
cvNamedWindow( "Face", CV_WINDOW_AUTOSIZE );
CvPoint pt1, pt2;
CvRect *r;
CvSeq *faces;
float multip = 0.1;
int x_diff, y_diff;
while( cvWaitKey( 10 ) == -1 ) {
cap_frame( cam );
undistort( cam );
cvClearMemStorage( storage );
faces = cvHaarDetectObjects( cam->undist, cascade, storage,
1.1, 2, CV_HAAR_DO_CANNY_PRUNING, cvSize(40, 40) );
if (faces->total > 0) {
;
r = (CvRect*)cvGetSeqElem( faces, 0 );
pt1.x = r->x;
pt2.x = r->x + r->width;
pt1.y = r->y;
pt2.y = r->y + r->height;
cvRectangle( cam->undist, pt1, pt2, CV_RGB(255,0,0), 3, 8, 0 );
x_diff = (cam->frame->width / 2) - (r->x + (r->width/2)) ;
y_diff = (cam->frame->height / 2) - (r->y + (r->height/2)) ;
pan( -(int)(x_diff * multip) );
tilt( (int)(y_diff * multip) );
}
cvShowImage( "Face", cam->undist );
}
cvDestroyAllWindows();
cvReleaseMemStorage( &storage );
free_camera( cam );
}
//------------------------------------------------------------------------------
//!
bool
CameraManipulator::onPointerMove( const Event& ev )
{
if( _mode == 0 ) return false;
switch( _mode )
{
case 1:
{
float radP = CGM::degToRad( _grabPos.x - ev.position().x );
float radS = CGM::degToRad( ev.position().y - _grabPos.y );
radP *= _direction;
if( _isLocked )
{
orbit( radP*0.75f, radS*0.75f );
}
else
{
tilt( radP*0.25f, radS*0.25f );
}
return true;
} break;
case 2: forward( ev.position() );
return true;
case 3: pan( ev.position() );
return true;
default:;
}
return false;
}
AREXPORT bool ArAMPTU::panTilt_i(double panDeg, double tiltDeg)
{
if (panDeg > getMaxPosPan_i())
panDeg = getMaxPosPan_i();
if (panDeg < getMaxNegPan_i())
panDeg = getMaxNegPan_i();
if (tiltDeg > getMaxPosTilt_i())
tiltDeg = getMaxPosTilt_i();
if (tiltDeg < getMaxNegTilt_i())
tiltDeg = getMaxNegTilt_i();
if (myPan - panDeg == 0 && myTilt - tiltDeg == 0)
return true;
if (myPan - panDeg == 0)
return tilt(tiltDeg);
if (myTilt - tiltDeg == 0)
return pan(panDeg);
myPan = panDeg;
myTilt = tiltDeg;
myPacket.empty();
myPacket.byteToBuf(ArAMPTUCommands::PANTILT);
myPacket.byte2ToBuf(ArMath::roundInt(myPan +
(getMaxPosPan_i() - getMaxNegPan_i())/2));
myPacket.byteToBuf(ArMath::roundInt(myTilt + (getMaxPosTilt_i() -
getMaxNegTilt_i())/2));
return sendPacket(&myPacket);
}
// get data as soon as it comes in
void ofApp::tabletMoved(TabletData &data) {
// set up coordinate frame based on tablet data
ofVec3f translate((data.abs_screen[0]-0.5)*gridsize,(data.abs_screen[1]-0.5)*gridsize, 0);
ofVec3f tilt(data.tilt_vec[0], data.tilt_vec[1], data.tilt_vec[2]);
ofQuaternion tiltquat;
tiltquat.makeRotate(ofVec3f(0,0,1), tilt);
tabmtx.setRotate(tiltquat);
tabmtx.setTranslation(translate);
}
/*!
Tilts the eye() up or down by \a tiltAngle degrees,
pans the eye() left or right by \a panAngle degrees,
and rolls the eye() left or right by \a rollAngle degrees,
all in a single fluid movement. The \a order parameter
indicates the order in which to perform the rotations.
This function is accessible to QML on the Camera item.
It is provided as a convenience for navigation items that
rotate the eye in multiple directions at the same time
based on mouse movements.
\sa tiltPanRollCenter()
*/
void QGLCamera::tiltPanRollEye
(float tiltAngle, float panAngle, float rollAngle,
QGLCamera::RotateOrder order)
{
switch (order) {
case QGLCamera::TiltPanRoll:
rotateEye(roll(rollAngle) * pan(panAngle) * tilt(tiltAngle));
break;
case QGLCamera::TiltRollPan:
rotateEye(pan(panAngle) * roll(rollAngle) * tilt(tiltAngle));
break;
case QGLCamera::PanTiltRoll:
rotateEye(roll(rollAngle) * tilt(tiltAngle) * pan(panAngle));
break;
case QGLCamera::PanRollTilt:
rotateEye(tilt(tiltAngle) * roll(rollAngle) * pan(panAngle));
break;
case QGLCamera::RollTiltPan:
rotateEye(pan(panAngle) * tilt(tiltAngle) * roll(rollAngle));
break;
case QGLCamera::RollPanTilt:
rotateEye(tilt(tiltAngle) * pan(panAngle) * roll(rollAngle));
break;
}
}
//----------------------------------------
void ofxViewportCam::tumble(float yaw, float pitch)
{
const float tumbleAmount = 0.25f;
this->tiltAmount += pitch * tumbleAmount;
this->panAmount += yaw * tumbleAmount;
if(this->isTargetSet)
{
// Move to tumble point.
const ofVec3f targetPos(target.getGlobalPosition());
setPosition(targetPos);
// Reset orientation to identity.
setOrientation(ofQuaternion(0, 0, 0, 1));
// Apply tumble.
pan(this->panAmount);
tilt(this->tiltAmount);
// Re-apply pre-existing track and zoom.
dolly(this->distance);
truck(this->truckAmount);
boom(this->boomAmount);
}
else
{
// Move to tumble point.
dolly(-this->distance);
// Reset orientation to identity.
setOrientation(ofQuaternion(0, 0, 0, 1));
// Apply tumble.
pan(this->panAmount);
tilt(this->tiltAmount);
// Move back.
dolly(this->distance);
}
}
static void command (int cmd)
{
cmd = locase (cmd);
if (cmd == ' ')
{
reset();
}
else if (cmd == 'a') {
tilt();
}
else if (cmd == 't') {
show_contacts = !show_contacts;
}
else if (cmd == '1') {
write_world = true;
}
}
//----------------------------------------
void ofxViewportCam::setTargetPosition(const ofVec3f &pos)
{
this->target.setPosition(pos);
this->isTargetSet = true;
// Move to tumble point.
const ofVec3f targetPos(target.getGlobalPosition());
setPosition(targetPos);
// Reset orientation to identity.
setOrientation(ofQuaternion(0, 0, 0, 1));
// Apply tumble.
pan(this->panAmount);
tilt(this->tiltAmount);
// Re-apply pre-existing track and zoom.
dolly(this->distance);
truck(this->truckAmount);
boom(this->boomAmount);
}
/*
double Autonomous::getTime()
{
}
*/
void Autonomous::updateHighGoal()
{
static int output=0;
switch (stage)
{
case IDLE:
printf("AUTO switch to DRIVE_AIM_WINCH\n");
stage = DRIVE_AIM_WINCH;
return;
case DRIVE_AIM_WINCH:
bool driveDone=moveForward(DISTANCE);
bool aimDone=tilt(HIGHGOAL_AUTOANGLE);
bool winchDone=wormPull();
if(output%20==0)
printf("drive: %i, aim: %i, winch: %i\n",driveDone,aimDone,winchDone);
if(driveDone && aimDone && winchDone)
{
printf("AUTO switch to SMART_FIRE\n");
stage = SMART_FIRE;
return;
}
break;
case SMART_FIRE:
if(smartFire())
{
printf("AUTO done\n");
stage = DONE;
return;
}
break;
case DONE:
robot->drive->TankDrive(0.0,0.0);
break;
default:
break;
}
previousStage = stage;
output++;
}
void ofxGamepadCamera::update(ofEventArgs& e) {
if(pad == NULL)
return;
float curTime = ofGetElapsedTimef();
float mult = curTime - lastTime;
pan(-pad->getAxisValue(PS3_STICK_R_X)*speedRotation*mult);
tilt(-pad->getAxisValue(PS3_STICK_R_Y)*speedRotation*mult);
truck(pad->getAxisValue(PS3_STICK_L_X)*speedMove*mult);
boom(-pad->getAxisValue(PS3_STICK_L_Y)*speedMove*mult);
if(useAnalogueDolly) {
dolly(-(pad->getAxisValueU(PS3_THRUST_L2)-.5)*speedMove*mult);
dolly((pad->getAxisValueU(PS3_THRUST_R2)-.5)*speedMove*mult);
} else {
if(pad->getButtonValue(PS3_BTN_L2)) {
dolly(-speedMove*2*mult);
}
if(pad->getButtonValue(PS3_BTN_R2)) {
dolly(speedMove*2*mult);
}
}
if(pad->getButtonValue(PS3_BTN_L1)) {
roll(-speedRotation*.7*mult);
}
if(pad->getButtonValue(PS3_BTN_R1)) {
roll(speedRotation*.7*mult);
}
if(pad->getButtonValue(PS3_BTN_SELECT))
reset();
lastTime = curTime;
}
gep::FreeCamera::FreeCamera()
{
look(vec2(90.f, 0.f));
tilt(-91.f);
}
/*============================================================================
* Long integer function S_solpos, adapted from the VAX solar libraries
*
* This function calculates the apparent solar position and the
* intensity of the sun (theoretical maximum solar energy) from
* time and place on Earth.
*
* Requires (from the struct posdata parameter):
* Date and time:
* year
* daynum (requirement depends on the S_DOY switch)
* month (requirement depends on the S_DOY switch)
* day (requirement depends on the S_DOY switch)
* hour
* minute
* second
* interval DEFAULT 0
* Location:
* latitude
* longitude
* Location/time adjuster:
* timezone
* Atmospheric pressure and temperature:
* press DEFAULT 1013.0 mb
* temp DEFAULT 10.0 degrees C
* Tilt of flat surface that receives solar energy:
* aspect DEFAULT 180 (South)
* tilt DEFAULT 0 (Horizontal)
* Function Switch (codes defined in solpos.h)
* function DEFAULT S_ALL
*
* Returns (via the struct posdata parameter):
* everything defined in the struct posdata in solpos.h.
*----------------------------------------------------------------------------*/
long S_solpos(struct posdata *pdat)
{
long int retval;
struct trigdata trigdat, *tdat;
tdat = &trigdat; /* point to the structure */
/* initialize the trig structure */
tdat->sd = -999.0; /* flag to force calculation of trig data */
tdat->cd = 1.0;
tdat->ch = 1.0; /* set the rest of these to something safe */
tdat->cl = 1.0;
tdat->sl = 1.0;
if ((retval = validate(pdat)) != 0) /* validate the inputs */
return retval;
if (pdat->function & L_DOY)
doy2dom(pdat); /* convert input doy to month-day */
else
dom2doy(pdat); /* convert input month-day to doy */
if (pdat->function & L_GEOM)
geometry(pdat); /* do basic geometry calculations */
if (pdat->function & L_ZENETR) /* etr at non-refracted zenith angle */
zen_no_ref(pdat, tdat);
if (pdat->function & L_SSHA) /* Sunset hour calculation */
ssha(pdat, tdat);
if (pdat->function & L_SBCF) /* Shadowband correction factor */
sbcf(pdat, tdat);
if (pdat->function & L_TST) /* true solar time */
tst(pdat);
if (pdat->function & L_SRSS) /* sunrise/sunset calculations */
srss(pdat);
if (pdat->function & L_SOLAZM) /* solar azimuth calculations */
sazm(pdat, tdat);
if (pdat->function & L_REFRAC) /* atmospheric refraction calculations */
refrac(pdat);
if (pdat->function & L_AMASS) /* airmass calculations */
amass(pdat);
if (pdat->function & L_PRIME) /* kt-prime/unprime calculations */
prime(pdat);
if (pdat->function & L_ETR) /* ETR and ETRN (refracted) */
etr(pdat);
if (pdat->function & L_TILT) /* tilt calculations */
tilt(pdat);
return 0;
}
void skypeCallback( const std_msgs::String& msgSkype)
{
std_msgs::String ReceivedCommands = msgSkype;
numSteps = strlen( (const char* ) msgSkype.data.c_str());
std::string skypeString = msgSkype.data.c_str();
ROS_INFO("%s", skypeString.c_str());
if ( numSteps > 7 ) return; // invalid format, more than 6 characters
if (skypeString.compare(CAMERA_CHANGE_STRING) == 0 || skypeString.compare(CAMERA_CHANGE_STRING_CAP) == 0) // swap cameras, both video and pan tilt
{
currentVideoDeviceMsg = baseMsg;
if (usingFirstVideoDevice)
{
currentVideoDeviceMsg.append(SECOND_VIDEO_DEVICE);
usingFirstVideoDevice = false;
}
else
{
currentVideoDeviceMsg.append(FIRST_VIDEO_DEVICE);
usingFirstVideoDevice = true;
}
ROS_INFO("%s", "pan tilt camera change");
return;
}
for (int i = 1; i < numSteps; i++) if ( (msgSkype.data[i] != msgSkype.data[0]) && msgSkype.data[i] != msgSkype.data[0] + 32 ) return;
// if string is not all identical characters, allowing for first character to be a capital, return
char cmd = msgSkype.data[0];
switch(cmd)
{
case 'u': // tilt up
tilt(-numSteps * DELTA_TILT);
break;
case 'U': // tilt up
tilt(-numSteps * DELTA_TILT);
break;
case 'n': // tilt down
tilt(numSteps * DELTA_TILT);
break;
case 'N': // tilt down
tilt(numSteps * DELTA_TILT);
break;
case 'b': // center tilt
cmdMsg = currentVideoDeviceMsg;
cmdMsg.append(" -s \"Pan/tilt Reset\" -- 2");
system(cmdMsg.c_str());
break;
case 'B': // center tilt
cmdMsg = currentVideoDeviceMsg;
cmdMsg.append(" -s \"Pan/tilt Reset\" -- 2");
system(cmdMsg.c_str());
break;
case 'k': // pan right
pan(-numSteps * DELTA_PAN);
break;
case 'K': // pan right
pan(-numSteps * DELTA_PAN);
break;
case 'h': // pan left
pan(numSteps * DELTA_PAN);
break;
case 'H': // pan left
pan(numSteps * DELTA_PAN);
break;
case 'g': // center pan
cmdMsg = currentVideoDeviceMsg;
cmdMsg.append(" -s \"Pan/tilt Reset\" -- 1");
system(cmdMsg.c_str());
break;
case 'G': // center pan
cmdMsg = currentVideoDeviceMsg;
cmdMsg.append(" -s \"Pan/tilt Reset\" -- 1");
system(cmdMsg.c_str());
break;
case 'j': //center all
cmdMsg = currentVideoDeviceMsg;
cmdMsg.append(" -s \"Pan/tilt Reset\" -- 3");
system(cmdMsg.c_str());
break;
case 'J': //center all
cmdMsg = currentVideoDeviceMsg;
cmdMsg.append(" -s \"Pan/tilt Reset\" -- 3");
system(cmdMsg.c_str());
break;
case 'm': //max down tilt
tilt(MAX_TILT * 2); // could be as high as max tilt up, so need to go down twice the range.
break;
case 'M': //max down tilt
tilt(MAX_TILT * 2);
break;
case '0': // swap video device, using different command than used to swap both video and pan tilt, this is pan tilt only
currentVideoDeviceMsg = baseMsg;
if (usingFirstVideoDevice)
{
currentVideoDeviceMsg.append(SECOND_VIDEO_DEVICE);
usingFirstVideoDevice = false;
}
else
{
currentVideoDeviceMsg.append(FIRST_VIDEO_DEVICE);
usingFirstVideoDevice = true;
}
break;
case '1': // use first video device
currentVideoDeviceMsg = baseMsg;
currentVideoDeviceMsg.append(FIRST_VIDEO_DEVICE);
usingFirstVideoDevice = true;
break;
case '2': // use second video device
currentVideoDeviceMsg = baseMsg;
currentVideoDeviceMsg.append(SECOND_VIDEO_DEVICE);
usingFirstVideoDevice = false;
break;
default: // unknown command
break;
}
}
