void robotField::keyPressEvent(QKeyEvent *event)
{
if(event->key()==Qt::Key_Up)
moveRobot(-5);
if(event->key()==Qt::Key_Down)
moveRobot(5);
if(event->key()==Qt::Key_Right)
rotateRobot(10);;
if(event->key()==Qt::Key_Left)
rotateRobot(-10);
if(event->key()==Qt::Key_0)
rotateRobot(15);
}
bool PewPewBot::camYawAlign()
{
//if (camera->hasData()){
cout << "Gyro: " << drive->getGyro() << " Error: "
<< drive->turnPID->GetError() << endl;
yawAlignState = true;
if (rotateRobot(270,5))
{
yawAlignState = false;
return true;
}
return false;
}
int robotField::qt_metacall(QMetaObject::Call _c, int _id, void **_a)
{
_id = QGraphicsView::qt_metacall(_c, _id, _a);
if (_id < 0)
return _id;
if (_c == QMetaObject::InvokeMetaMethod) {
switch (_id) {
case 0: moveRobot((*reinterpret_cast< int(*)>(_a[1]))); break;
case 1: rotateRobot((*reinterpret_cast< int(*)>(_a[1]))); break;
default: ;
}
_id -= 2;
}
return _id;
}
void PewPewBot::Autonomous()
{
GetWatchdog().SetEnabled(true);
GetWatchdog().SetExpiration(0.2);
drive->shift(false);
drive->resetEncoders();
autonomousMode = kCollect;
stableCount = 0;
//Cleaning
collector->clean();
double startTime = System::currentTimeMillis();
//Spinup shooter
updateShooter(true);
drive->setLight(true);
hasResetItem = false;
while (IsAutonomous() && !IsDisabled())
{
if (PID_SLIDER> .2095)
{
shooter->setPIDAdjust(0.0762 * log(PID_SLIDER) + 0.0407);
} else
{
shooter->setPIDAdjust(-0.15);
}
shooter->update();
#if KINECT
if (kinect->getKinectMode())
{
autonomousMode = kKinect;
}
#endif
drive->updateCompressor();
collector->update();
#if KINECT
kinect->update();
#endif
updateDriverStation();
switch (autonomousMode)
{
case kDoYawAlign:
break;
case kDoDepthAlign:
if (lineDepthAlign())
autonomousMode = kCollect;
break;
case kCollect:
if (collectAllBalls())
autonomousMode = kShoot;
break;
case kShoot:
if (shootAllBalls(AUTONOMOUS_DELAY_SWITCH?startTime + AUTONOMOUS_DELAY:-1))
{
//If there are balls left, cycle back to the collect stage
if (collector->getSense(0) || collector->getSense(1)
|| collector->getSense(2))
{
autonomousMode = kCollect;
} else if (AUTONOMOUS_FULL_AUTO_SWITCH)
{
autonomousMode = kRotate180;
}
#if KINECT
else if (kinect->hasKinect())
{
autonomousMode = kKinect;
}
#endif
else
{
autonomousMode = kDone;
}
}
break;
case kRotate180:
if (rotateRobot(180, 5))
autonomousMode = kMoveToBridge;
break;
case kMoveToBridge:
if (!hasResetItem)
{
drive->resetEncoders();
hasResetItem = true;
}
drive->setSpeedL(.25);
drive->setSpeedR(.25);
double distance = (drive->getLPosition() + drive->getRPosition())
/ 2.0;
if (fabs(distance - 7.0) < 0.5)
{
hasResetItem = false;
autonomousMode = kTipBridge;
}
break;
case kTipBridge:
drive->tip(true);
autonomousMode = kDone;
break;
case kKinect:
#if KINECT
kinectCode();
#else
autonomousMode = kDone;
#endif
break;
default:
//We are done
updateShooter(false);
break;
}
drive->setLight(false);
GetWatchdog().Feed();
Wait(0.02);
}
}
int rightChecker1() {
rotateRobot(1);
return frontChecker();
}
int leftChecker1() {
rotateRobot(-1);
return frontChecker();
}
