task buttons(){
if(strafe){
if(vexRT[Btn8L] == 1){
driveEnable = false;
setDrive(strafeSpeed*-1);
}
else if(vexRT[Btn8R] == 1){
driveEnable = false;
setDrive(strafeSpeed);
}
else if(vexRT[Btn8U] == 1){
driveEnable = false;
runExpansion();
}
else driveEnable = true;
}
if(vexRT[Btn7D] == 1) strafe = true;
if(vexRT[Btn8D] == 1) strafe = false;
if(vexRT[Btn5U] == 1) armState = 1;
else if(vexRT[Btn5D] == 1) armState = 2;
else armState = 0;
if(vexRT[Btn6U] == 1) setIntake(intakeSpeed);
else if(vexRT[Btn6D] == 1) setIntake(intakeSpeed*-1);
else setIntake(0);
}
void runExpansion(){
strafe = true;
setDrive(127,127);
wait1Msec(250);
setDrive(-127,-127);
wait1Msec(250);
motor[leftDriveF] = 127;
motor[leftDriveB] = -127;
motor[rightDriveF] = 127;
motor[rightDriveB] = -127;
wait1Msec(250);
brake();
}
void setRobotPhysLimit( int distance, char direction, int height, int roller, int tray, int platform, int timer )
{
ClearTimer( T1 );
resetEncoders();
//artificiallyresetGyro();
while(time1[T1] < timer && !SensorValue[TowerLimitL] && !SensorValue[TowerLimitR] )
{
if( direction == 'S' )
setDrive( distance );
else if( direction == 'T' )
spinDrive( distance );
else
wheelDrive( distance, direction );
//setArm( height );
if( height == 0 )
moveArm( -10, -10 );
else
{
if( SensorValue[ButtonBlockL] == 1 && SensorValue[ButtonBlockR] == 1 )
moveArm( 7, 7 );
else if( SensorValue[ButtonBlockL] == 1 && SensorValue[ButtonBlockR] == 0 )
moveArm( 7, 20 );
else if( SensorValue[ButtonBlockL] == 0 && SensorValue[ButtonBlockR] == 1 )
moveArm( 20, 7 );
else
setArm( height );
}
moveRollers( roller, roller );
movePiston( tray, platform );
}
stopMotors();
}
task autonomous() {
startTask(FlywheelSpeedControl);
setDrive(127, 127);
delay(500);
setDrive(0, 0);
delay(100);
isFlywheelOn=true;
targetFlywheelSpeed=MaxFlyspeed;
delay(1000);
for(int reps=0; reps<4; reps+=1) {
motor[mIntake] = 127;
delay(500);
motor[mIntake] = 0;
delay(1000);
}
}
GuardianListWidget::GuardianListWidget(Guardian *guardian)
{
this->guardian = guardian;
guardianIconLabel = new QLabel();
guardianIconLabel->setObjectName("guardianiconlabel");
guardianNameLabel = new QLabel(guardian->getName());
guardianNameLabel->setObjectName("namelabel");
driveLabel = new QPushButton();
driveLabel->setObjectName("drivelabel");
setDrive(guardian->getDrive());
auto *rootLayout = new QHBoxLayout;
rootLayout->setContentsMargins(0, 0, 0, 0);
rootLayout->setSpacing(0);
rootLayout->addWidget(guardianIconLabel);
rootLayout->addSpacerItem(new QSpacerItem(15, 0, QSizePolicy::Fixed, QSizePolicy::Fixed));
rootLayout->addWidget(guardianNameLabel);
rootLayout->addSpacerItem(new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Fixed));
rootLayout->addWidget(driveLabel);
rootLayout->setAlignment(Qt::AlignLeft);
setLayout(rootLayout);
setObjectName("guardianlistwidget");
}
task usercontrol() {
startTask(FlywheelSpeedControl);
time1[T1] = 0;
while (true) {
setDrive( vexRT[ChJoyLY], vexRT[ChJoyRY] );
motor[mIntake] = buttonGroupToPower(Btn5);
motor[mPuncher] = buttonToPower(Btn6U);
if( vexRT[Btn7U] ){
isFlywheelOn = true;
} else if( vexRT[Btn7D] ){
isFlywheelOn = false;
}
int speedUp = vexRT[Btn8U];
int speedDown = vexRT[Btn8D];
if( (speedUp || speedDown) && time1[T1] > 500 ){
float delta = speedUp ? 0.5 : -0.5;
targetFlywheelSpeed = bound( targetFlywheelSpeed + delta, MinFlyspeed, MaxFlyspeed );
time1[T1] = 0;
}
delay(10);
}
}
Filter::Filter(void)
{
m_resonance = ONE_OVER_SQRT2;
m_r2 = 1/m_resonance;
m_h = 0.5f;
m_c_low=m_c_band=m_c_high=0.0f;
#ifdef VSTI
m_samplerate = SAMPLERATE;
#endif
m_base_freq=0.0f;
m_mod=0.0f;
m_staticmod=0.0f;
m_bandwidth=2.0f;
setMode(0.0f);
setCutoff(0.5f);
setMod(0.0f,0.0f);
setBandwidth(2.0f);
setDrive(0.0f);
m_staticmod = 0.0f;
setOutputLevel(1.0f);
m_cutoff = 0.0f;
calculateCoefficients();
m_calc_coefficients = false;
m_calc_interval = STATE_CALC_INTERVAL;
reset();
}
PhysXHingeJoint::PhysXHingeJoint(PxPhysics* physx, const HINGE_JOINT_DESC& desc)
:HingeJoint(desc)
{
PxRigidActor* actor0 = nullptr;
if (desc.bodies[0].body != nullptr)
actor0 = static_cast<PhysXRigidbody*>(desc.bodies[0].body)->_getInternal();
PxRigidActor* actor1 = nullptr;
if (desc.bodies[1].body != nullptr)
actor1 = static_cast<PhysXRigidbody*>(desc.bodies[1].body)->_getInternal();
PxTransform tfrm0 = toPxTransform(desc.bodies[0].position, desc.bodies[0].rotation);
PxTransform tfrm1 = toPxTransform(desc.bodies[1].position, desc.bodies[1].rotation);
PxRevoluteJoint* joint = PxRevoluteJointCreate(*physx, actor0, tfrm0, actor1, tfrm1);
joint->userData = this;
mInternal = bs_new<FPhysXJoint>(joint, desc);
PxRevoluteJointFlags flags;
if (((UINT32)desc.flag & (UINT32)Flag::Limit) != 0)
flags |= PxRevoluteJointFlag::eLIMIT_ENABLED;
if (((UINT32)desc.flag & (UINT32)Flag::Drive) != 0)
flags |= PxRevoluteJointFlag::eDRIVE_ENABLED;
joint->setRevoluteJointFlags(flags);
// Must be set after global flags, as it will append to them.
// Calls to virtual methods are okay here.
setLimit(desc.limit);
setDrive(desc.drive);
}
PhysXD6Joint::PhysXD6Joint(PxPhysics* physx, const D6_JOINT_DESC& desc)
:D6Joint(desc)
{
PxRigidActor* actor0 = nullptr;
if (desc.bodies[0].body != nullptr)
actor0 = static_cast<PhysXRigidbody*>(desc.bodies[0].body)->_getInternal();
PxRigidActor* actor1 = nullptr;
if (desc.bodies[1].body != nullptr)
actor1 = static_cast<PhysXRigidbody*>(desc.bodies[1].body)->_getInternal();
PxTransform tfrm0 = toPxTransform(desc.bodies[0].position, desc.bodies[0].rotation);
PxTransform tfrm1 = toPxTransform(desc.bodies[1].position, desc.bodies[1].rotation);
PxD6Joint* joint = PxD6JointCreate(*physx, actor0, tfrm0, actor1, tfrm1);
joint->userData = this;
mInternal = bs_new<FPhysXJoint>(joint, desc);
// Calls to virtual methods are okay here
for (UINT32 i = 0; i < (UINT32)D6JointAxis::Count; i++)
setMotion((D6JointAxis)i, desc.motion[i]);
for (UINT32 i = 0; i < (UINT32)D6JointDriveType::Count; i++)
setDrive((D6JointDriveType)i, desc.drive[i]);
setLimitLinear(desc.limitLinear);
setLimitTwist(desc.limitTwist);
setLimitSwing(desc.limitSwing);
setDriveTransform(desc.drivePosition, desc.driveRotation);
setDriveVelocity(desc.driveLinearVelocity, desc.driveAngularVelocity);
}
LadderFilter<Type>::LadderFilter() : state (2)
{
setSampleRate (Type (1000)); // intentionally setting unrealistic default
// sample rate to catch missing initialisation bugs
setResonance (Type (0));
setDrive (Type (1.2));
setMode (Mode::LPF12);
}
/**
* Sets the name of the file name.
* If the name has an extension, the extension will be set
* If the name has a full path (starts with drive or separator), the path will be set
* If the name has a relative path, the path will be appended to the current path
*
* @param name the name
*/
void Filename::setName(const char *name)
{
if (name == NULL || *name == '\0')
m_name = "";
else
{
#ifdef WIN32
if (isalpha(*name) && *(name + 1) == ':')
{
setDrive(name);
name += 2;
}
#endif
std::string localname = name;
convertToSystemPath(localname);
const char *dot = strrchr(localname.c_str(), '.');
const char *firstSeparator = strchr(localname.c_str(), PATH_SEPARATOR);
const char *lastSeparator = strrchr(localname.c_str(), PATH_SEPARATOR);
if (firstSeparator != NULL)
{
// name has a path
if (firstSeparator == localname)
{
// set path
m_path = std::string(firstSeparator, lastSeparator - firstSeparator + 1);
}
else
{
// append path
m_path += std::string(localname.c_str(), lastSeparator -
localname.c_str() + 1);
}
}
if (dot != NULL && (lastSeparator == NULL || dot > lastSeparator))
{
// name has an extension
setExtension(dot);
if (lastSeparator == NULL)
m_name = std::string(localname.c_str(), dot - localname.c_str());
else
m_name = std::string(lastSeparator + 1, dot - (lastSeparator + 1));
}
else
{
// name doesn't have an extension
if (lastSeparator == NULL)
m_name = localname;
else
m_name = std::string(lastSeparator + 1);
}
}
makeFullPath();
} // Filename::setName
task usercontrol() {
int stick[4],
drivePwrs;
signed char liftTargCt = 0,
liftTargCtLast;
const signed char stickThresh = 7;
while (true) {
for(int i = 0; i < 4; i++)
stick[i] =
(fabs(vexRT[i]) >= stickThresh)
? vexRT[i]
: 0;
upToggle(&liftUpToggle, LIFT_UP_BTN);
upToggle(&liftDwnToggle, LIFT_DWN_BTN);
liftTargCtLast = liftTargCt;
liftTargCt += (pressToggle(&liftUpToggle, LIFT_UP_BTN) ^ pressToggle(&liftDwnToggle, LIFT_DWN_BTN))
? (pressToggle(&liftUpToggle, LIFT_UP_BTN))
? 1
: -1
: 0;
if(liftTargCt > 2)
liftTargCt = 0;
else if(liftTargCt < 0)
liftTargCt = 2;
setDrive(arcade(&drivePwrs, stick[LY], stick[RX]));
if(liftTargCt != liftTargCtLast)
setPid(&liftPid, liftSetPts[liftTargCt]);
setLift(upPid(&liftPid));
setClaw((CLAW_OPEN_BTN ^ CLAW_CLOSE_BTN)
? (CLAW_OPEN_BTN)
? 127
: -127
: 0);
wait1Msec(20);
}
}
CdFilenameStr& CdFilenameStr::ChangeDir( const CdFilenameStr& ev )
{
// --------------------------------
// ディレクトリ
// --------------------------------
if ( ev.IsRelativePath() ){
// 相対パスの場合は、ディレクトリは相対移動となります。
int idx ;
for ( idx = 0 ; idx < ev.DirCount() ; idx ++ ){
std::wstring strDir = ev.DirAt( idx ) ;
if ( strDir == L"." ){
} else if ( strDir == L".." ){
if ( m_contstrDir.size() > 0 ){
m_contstrDir.pop_back() ;
}
} else {
m_contstrDir.push_back( strDir ) ;
}
}
} else {
// 絶対パスの場合は、ディレクトリはおきかえになります。
m_contstrDir.clear() ;
int idx ;
for ( idx = 0 ; idx < ev.DirCount() ; idx ++ ){
m_contstrDir.push_back( ev.DirAt( idx ) ) ;
}
}
// --------------------------------
// ドライブ
// --------------------------------
if ( ev.Drive().length() > 0 ){
setDrive( ev.Drive() ) ;
}
// --------------------------------
// ファイル名
// --------------------------------
if ( ev.Filename().length() > 0 ){
setFilename( ev.Filename() ) ;
}
return ( *this ) ;
}
int FormSetup::qt_metacall(QMetaObject::Call _c, int _id, void **_a)
{
_id = QWidget::qt_metacall(_c, _id, _a);
if (_id < 0)
return _id;
if (_c == QMetaObject::InvokeMetaMethod) {
switch (_id) {
case 0: ApplyB(); break;
case 1: addCD(); break;
case 2: FsetupTimer(); break;
case 3: eject(); break;
case 4: closeTB(); break;
case 5: setDrive(); break;
case 6: CHKENABLED(); break;
case 7: driveE((*reinterpret_cast< QString(*)>(_a[1]))); break;
default: ;
}
_id -= 8;
}
return _id;
}
/**
* Sets the path of the file name.
* If the path has a drive letter, the drive will be set (Winodws only)
*
* @param path the path
*/
void Filename::setPath(const char *path)
{
if (path == NULL || *path == '\0')
m_path = "";
else
{
#ifdef WIN32
if (isalpha(*path) && *(path + 1) == ':')
{
setDrive(path);
path += 2;
}
#endif
m_path = path;
convertToSystemPath(m_path);
if (m_path[strlen(path) - 1] != PATH_SEPARATOR)
m_path += PATH_SEPARATOR;
}
makeFullPath();
} // Filename::setPath
void startSlam(int v)
{
FileStorage fs("config.yml", FileStorage::READ);
Mat cam, dist, rpos, q, s;
fs["cameraMatrix"] >> cam;
fs["distMatrix"] >> dist;
fs["sensorPos"] >> rpos;
fs["q"] >> q;
fs["s"] >> s;
fs.release();
ml = new MarkerLocator(cam, dist, rpos, 100.);
// Mat q = (Mat_<double>(2,1) << .01, .1*M_PI/180);
// Mat s = (Mat_<double>(2,1) << 10, .5*M_PI/180);
slam = new EkfSlam(&scan, q, s);
#ifdef GUI
namedWindow("EKFSlam");
disp = new MapDisplay("Map");
waitKey();
#endif
Mat u = (Mat_<double>(2,1) << 0, 0);
int fd = openPort();
oiStart();
setSafeMode();
readDist();
readAngle();
VideoCapture cap(0);
if(!cap.isOpened()) {
printf("Failed capture\n");
return;
}
VideoWriter record("/tmp/demo.avi", CV_FOURCC('D','I','V','X'), 30, Size(1340,600), true);
if(!record.isOpened()) {
printf("Failed writer\n");
return;
}
clock_t t;
time_t stime;
while(1) {
t = clock();
setDrive(v,v);
double mindist = 99999;
while(msElap(t) < 100 && !hitWall()) {
u.at<double>(0) = (double)readDist();
u.at<double>(1) = M_PI*(double)readAngle()/180;
mindist = slamStep(u, cap, record);
}
// backup before turn when running into something
if(hitWall() || mindist < 700) {
if(hitWall()) {
// backup
setDrive(-15,-15);
t = clock();
while(msElap(t) < 500) {
u.at<double>(0) = (double)readDist();
u.at<double>(1) = M_PI*(double)readAngle()/180;
slamStep(u, cap, record);
}
}
// turn
if(time(&stime)%60 < 30) {
setDrive(-5, 5);
}
else
setDrive(5,-5);
t = clock();
while(msElap(t) < 250) {
u.at<double>(0) = 0;
u.at<double>(1) = M_PI*(double)readAngle()/180;
slamStep(u, cap, record);
}
}
if(waitKey(10) == 27)
break;
}
setDrive(0,0);
imwrite("lastFrame.png", g_outFrame);
close(fd);
delete ml;
delete slam;
#ifdef GUI
delete disp;
#endif
}
int CdFilenameStr::ChangeToRelativeDir( const CdFilenameStr& ev )
{
int merrRv = 0 ;
CdFilenameStr aFilenameStrBase( ev ) ;
// --------------------------------
// ドライブ
// --------------------------------
if ( merrRv >= 0 ){
if ( Drive().length() > 0 && ev.Drive().length() > 0
&& Drive() != ev.Drive() ){
// 双方別々のドライブが指定されている場合は、
// 相対パスへの変換はできません。
merrRv = -1 ; // ドライブが異なるため、相対パスにはできません。
} else {
setDrive( L"" ) ;
aFilenameStrBase.setDrive( L"" ) ;
}
}
// --------------------------------
// ディレクトリ
// --------------------------------
if ( merrRv >= 0 ){
if ( IsRelativePath() ){
merrRv = -2 ; // thisが相対パスです
} else if ( aFilenameStrBase.IsRelativePath() ){
merrRv = -3 ; // evが相対パスです
}
}
if ( merrRv >= 0 ){
//thisを相対パスに変更します。
setIsRelativePath( true ) ;
aFilenameStrBase.setIsRelativePath( true ) ;
while ( DirCount() > 0
&& aFilenameStrBase.DirCount() > 0
&& DirAt( 0 ) == aFilenameStrBase.DirAt( 0 )
&& DirAt( 0 ) != L"."
&& DirAt( 0 ) != L".." ){
delDirAt( 0 ) ;
aFilenameStrBase.delDirAt( 0 ) ;
}
// 先頭よりの共通のディレクトリを削除します。
//
int iLi ;
for ( iLi = 0 ; iLi < aFilenameStrBase.DirCount() ; iLi ++ ){
addDirAt( 0 , L".." ) ;
}
}
// --------------------------------
// ファイル名
// --------------------------------
if ( merrRv >= 0 ){
if ( Filename().length() > 0 ){
if ( aFilenameStrBase.Filename().length() > 0 ){
setFilename( ev.Filename() ) ;
}
}
}
return ( merrRv ) ;
};
task drive(){
if(driveEnable) setDrive(vexRT[Ch3], vexRT[Ch2]);
}
void brake(){
setDrive(0, 0);
}
bool NavigationControl::moveToLine(int x, int y){
//Returns 1 if on top of the line, 0 otherwise
int target_x = x;
int target_y = y;
if (!target_x && !target_y ) {
// Centered = true;
Begun = false;
start_x = 0;
start_y = 0;
setStrafe(0);
setDrive(0);
return 1; //Centered
}
if (!Begun){ // First assignment: create starting points
start_x = x;
start_y = y;
Begun = true;
// Centered = false;
}
// Calculate the directions what direction do we have to move?
int dir_x = target_x>0 ? 1 : -1; //Left -1, Right +1
int dir_y = target_y>0 ? 1 : -1; //Reverse -1, Forward +1
// Calculate the percentage (OR SIMPLY USE ERROR??)
float per_x = (target_x/240);//start_x);
float per_y = (target_y/320);//start_y);
// Control Process (calculate the right thrust)
// Use less than 100% based on how big the number is.
// Convert the distance percentage to thrust percentage
// These are set assuming there is no bias on the voltage
// sent to the thrusters
int minT = 50; // to avoid sending values under the minimum.
//maxT = 90; //To avoid moving too fast and sucking too much power
int range = 40;
int thrust_x = 0;
int thrust_y = 0;
if((per_x) <MAX_THRESHOLD ){
if((per_x)>MIN_THRESHOLD){ //Send a negative thrust to decrease the speed
thrust_x = -60*dir_x;
} else {
thrust_x = 0; // this line may be unnecessary
}
} else {
thrust_x = (range*per_x+minT)*dir_x;
}
if((per_y) <MAX_THRESHOLD ){
if((per_y)>MIN_THRESHOLD){ //Send a negative thrust to decrease the speed
thrust_y = -60*dir_y;
} else {
thrust_y = 0; // We zeroed out in this direction
}
} else {
if(dir_y>0){ //
range = 30; //So we don't go up too fast
thrust_y = (range*per_y+minT)*dir_y;
}
}
if (!thrust_x && !thrust_y)
return 1; //No speed means we're <1% away
//Send the calculated speed to the motor
setStrafe(thrust_x);
setDrive(thrust_y);
return 0;
}
