task autonomous()
{
/*while (bIfiAutonomousMode)
{
getInput();
processAutonomous();
//doOutput();
}*/
/*Drive(FORWARD); //Drive
Lift(UP); //Raise Lift
wait1Msec(2000);
SingleTurnRight(127); //Wiggle Right
wait1Msec(700);
Drive(STOP);
wait1Msec(100);
SingleTurnLeft(127); //Wiggle Left
wait1Msec(900);
SingleTurnRight(127); //Wiggle Back Right
wait1Msec(200);
Drive(FORWARD); //Drive
wait1Msec(700);
SingleTurnRight(127); //Turn More Right
wait1Msec(500);
Drive(FORWARD); //Drive into goal
wait1Msec(1500);
Drive(BACKWARD); //Back up a bit
wait1Msec(500);
Drive(STOP);
Intake(OUT); //Release the Muffins!
wait1Msec(1500);
wait1Msec(1500);
Intake(OUT);
wait1Msec(1500);
Intake(STOP);*/
Lift(UP); //Raise Lift
wait1Msec(1100);
while(true)
{
Drive(FORWARD); //Drive
wait1Msec(1500);
Drive(STOP);
wait1Msec(500);
Lift(20); //Hold the cage up
wait1Msec(500);
Intake(OUT); //Release the Muffins!
wait1Msec(5000);
Intake(STOP);
Drive(BACKWARD);
wait1Msec(1800);
Drive(STOP);
//Again again!
while (nLCDButtons == 0)
{
}
wait1Msec(200);
}
}
// Real Autonomous
// * Code to be run autonomously for the first ten (10) seconds of the match.
// * Launch catapult
// * Drive robot forward ENCODER_DIST ticks.
void Autonomous()
{
robotDrive.SetSafetyEnabled(false);
// STEP 1: Set all of the states.
// SAFETY AND SANITY - SET ALL TO ZERO
loaded = winchSwitch.Get();
loading = false;
intake.Set(0.0);
rightWinch.Set(0.0);
leftWinch.Set(0.0);
// STEP 2: Move forward to optimum shooting position
Drive(-AUTO_DRIVE_SPEED, SHOT_POSN_DIST);
// STEP 3: Drop the arm for a clean shot
arm.Set(DoubleSolenoid::kForward);
Wait(1.0); // Ken
// STEP 4: Launch the catapult
LaunchCatapult();
Wait (1.0); // Ken
// Get us fully into the zone for 5 points
Drive(-AUTO_DRIVE_SPEED, INTO_ZONE_DIST - SHOT_POSN_DIST);
// SAFETY AND SANITY - SET ALL TO ZERO
intake.Set(0.0);
rightWinch.Set(0.0);
leftWinch.Set(0.0);
}
void Autonomous(void)
{
SetWatchdogEnabled(false);
Drive(0.5, 0.0);
Wait(2.0);
Drive(0.0, 0.0);
}
/**
Write aligned members of TFatBootSector to media
@param aMediaPos media position the data will be written to
@param aBootSector data to write
@return Media write error code
*/
TInt CFatMountCB::DoWriteBootSector(TInt64 aMediaPos, const TFatBootSector& aBootSector) const
{
__PRINT2(_L("#- CFatMountCB::DoWriteBootSector() drv:%d, pos:0x%x"),Drive().DriveNumber(), (TUint32)aMediaPos);
ASSERT(aMediaPos>=0);
TBuf8<KDefaultSectorSize> bootSecBuf(KDefaultSectorSize);
bootSecBuf.FillZ();
//-- externalize boot sector to the data buffer
aBootSector.Externalize(bootSecBuf);
//-- put a boot sector signature to the last 2 bytes
bootSecBuf[KDefaultSectorSize-2] = 0x55;
bootSecBuf[KDefaultSectorSize-1] = 0xaa;
//-- write boot sector to the media
TInt r=LocalDrive()->Write(aMediaPos, bootSecBuf);
if (r!=KErrNone)
{//-- write failure
__PRINT2(_L("CFatMountCB::DoWriteBootSector() failed! drv:%d, code:%d"),Drive().DriveNumber(),r);
}
return r;
}
// Test Autonomous
void Autonomous()
{
robotDrive.SetSafetyEnabled(false);
// STEP 1: Set all of the states.
// SAFETY AND SANITY - SET ALL TO ZERO
loaded = winchSwitch.Get();
loading = false;
intake.Set(0.0);
rightWinch.Set(0.0);
leftWinch.Set(0.0);
// STEP 2: Move forward to optimum shooting position
Drive(-AUTO_DRIVE_SPEED, SHOT_POSN_DIST);
// STEP 3: Drop the arm for a clean shot
arm.Set(DoubleSolenoid::kForward);
Wait(1.0); // Ken
// STEP 4: Launch the catapult
LaunchCatapult();
Wait (1.0); // Ken
if (ds->GetDigitalIn(1))
{
// STEP 5: Start the intake motor and backup to our origin position to pick up another ball
InitiateLoad();
intake.Set(-INTAKE_COLLECT);
while (CheckLoad());
Drive(AUTO_DRIVE_SPEED, SHOT_POSN_DIST);
Wait(1.0); // For the ball to collect
// STEP 6: Shut off the intake, bring up the arm and move to shooting position
intake.Set(0.0);
arm.Set(DoubleSolenoid::kReverse);
Wait (1.0); // "Settle down"
Drive(-AUTO_DRIVE_SPEED, SHOT_POSN_DIST);
// Step 7: drop the arm for a clean shot and shoot
arm.Set(DoubleSolenoid::kForward);
Drive(AUTO_DRIVE_SPEED, SHOT_POSN_DIST);
// UNTESTED KICKED OFF FIELD
Wait(1.0); // For arm to go down
LaunchCatapult();
}
// Get us fully into the zone for 5 points
Drive(-AUTO_DRIVE_SPEED, INTO_ZONE_DIST - SHOT_POSN_DIST);
// SAFETY AND SANITY - SET ALL TO ZERO
intake.Set(0.0);
rightWinch.Set(0.0);
leftWinch.Set(0.0);
}
/**
Read non aligned boot data from media into TFatBootSector structure
@param aMediaPos media position the data will be read from
@param aBootSector refrence to TFatBootSector populate
@return Media read error code
*/
TInt CFatMountCB::DoReadBootSector(TInt64 aMediaPos, TFatBootSector& aBootSector) const
{
__PRINT2(_L("#- CFatMountCB::DoReadBootSector() drv:%d, pos:0x%x"),Drive().DriveNumber(), (TUint32)aMediaPos);
ASSERT(aMediaPos>=0);
TBuf8<KSizeOfFatBootSector> bootSecBuf(KSizeOfFatBootSector);
//-- read boot sector from the media
TInt r=LocalDrive()->Read(aMediaPos, KSizeOfFatBootSector, bootSecBuf);
if (r != KErrNone)
{
__PRINT2(_L("CFatMountCB::DoReadBootSector() failed! drv:%d, code:%d"),Drive().DriveNumber(),r);
//-- fiddling with the error code; taken from MountL()
if (r==KErrNotSupported)
return KErrNotReady;
#if defined(_LOCKABLE_MEDIA)
else if(r==KErrLocked)
return KErrLocked;
#endif
else if (r!=KErrNoMemory && r!=KErrNotReady && r!=KErrCorrupt && r!=KErrUnknown)
return KErrCorrupt;
return r;
}
ASSERT(r==KErrNone);
//-- initialise TFatBootSector object
aBootSector.Internalize(bootSecBuf);
//-- Validate the partition size, and fix up if the out of bounds
TLocalDriveCapsV2Buf localDriveCaps;
r = LocalDrive()->Caps(localDriveCaps);
if (r != KErrNone)
{
//-- fiddling with the error code; taken from MountL()
if (r!=KErrNoMemory && r!=KErrNotReady && r!=KErrCorrupt && r!=KErrUnknown)
return KErrCorrupt;
else
return r;
}
if(!(localDriveCaps().iMediaAtt & KMediaAttVariableSize))
{//-- this is not a RAM drive.
const TUint32 maxSectors = I64LOW(localDriveCaps().iSize >> KDefSectorSzLog2);
if(aBootSector.TotalSectors())
aBootSector.SetTotalSectors(Min(aBootSector.TotalSectors(), maxSectors));
else
aBootSector.SetHugeSectors(Min(aBootSector.HugeSectors(), maxSectors));
}
return KErrNone;
}
void CCar::DriveForward()
{
Clutch();
if(0==CurrentTransmission()) Transmission(1);
if(1==CurrentTransmission()||0==CurrentTransmission())Starter();
Drive();
}
void CCar::DriveBack()
{
Clutch();
Transmission(0);
if(1==CurrentTransmission()||0==CurrentTransmission())Starter();
Drive();
}
TBool CAgnAttachmentFile::CompareL(const CAgnAttachment& aSource) const
{
if (!CAgnAttachment::CompareL(aSource))
{
return EFalse;
}
// CAgnAttachment::CompareL already compared disposition, so both are file attachments
const CAgnAttachmentFile& KFileSource = static_cast<const CAgnAttachmentFile&>(aSource);
if (ContentId() != KFileSource.ContentId())
{
return EFalse;
}
if (LastModifiedTimeUtc() != KFileSource.LastModifiedTimeUtc())
{
return EFalse;
}
TDriveName drv1 = Drive();
TDriveName drv2 = KFileSource.Drive();
drv1.UpperCase();
drv2.UpperCase();
if (drv1 != drv2)
{
return EFalse;
}
return ETrue;
}
void CSVPHostMountCB::ReadUidL(const TDesC& aName,TEntry& anEntry) const
{
DP(_L("** (SVPHOSTMNT) CSVPHostMountCB::ReadUidL()"));
TBuf<KMaxPath> name;
TUint driveNumber = Drive().DriveNumber();
CanonicalizePathname(aName, driveNumber, name);
TSVPHostFsFileOpenInfo fileOpenInfo(driveNumber, name,EFileRead,EFileOpen);
TInt err = SVP_HOST_FS_DEVICE().FileOpen(fileOpenInfo);
User::LeaveIfError(err);
TBuf8<sizeof(TCheckedUid)> uidBuf;
uidBuf.SetLength(sizeof(TCheckedUid));
TSVPHostFsFileReadInfo fileReadInfo(driveNumber, fileOpenInfo.iHandle,sizeof(TCheckedUid),0,(char*)uidBuf.Ptr());
if (KErrNone != SVP_HOST_FS_DEVICE().FileRead(fileReadInfo))
User::LeaveIfError(SVP_HOST_FS_DEVICE().FileClose(driveNumber, fileOpenInfo.iHandle));
DP(_L("** (SVPHOSTMNT) CSVPHostFileCB::ReadUidL sizeof(TCheckedUid) %d fileOpenInfo.iLength %d "), sizeof(TCheckedUid), fileReadInfo.iLength);
if (fileReadInfo.iLength!=sizeof(TCheckedUid))
User::LeaveIfError(SVP_HOST_FS_DEVICE().FileClose(driveNumber, fileOpenInfo.iHandle));
TCheckedUid uid(uidBuf);
anEntry.iType=uid.UidType();
User::LeaveIfError(SVP_HOST_FS_DEVICE().FileClose(driveNumber, fileOpenInfo.iHandle));
}
TBool CRemoteMountCB::IsRomDrive() const
//
// Returns ETrue if the drive == EDriveZ
//
{
return(Drive().DriveNumber()==EDriveZ);
}
void CSccb::Write(unsigned char val)
{
unsigned char i;
for (i=0; i<8; i++, val<<=1)
{
if (val&0x80) // send msb first
{
PortWrite(0, 1);
PortWrite(1, 1);
PortWrite(0, 1);
}
else
{
PortWrite(0, 0);
PortWrite(1, 0);
PortWrite(0, 0);
}
}
// send "don't care" bit
TriState();
PortWrite(0, 0);
PortWrite(1, 0);
PortWrite(0, 0);
Drive();
}
void Arm::UsePIDOutput(double output){
// Use output to drive your system, like a motor
// e.g. yourMotor->Set(output);
// Write speed to the motors. Note that we don't have to reverse them since we took care of that already.
Drive((float)output);
}
unsigned char CSccb::Read(unsigned char addr)
{
unsigned char i, val;
// 2 phase
Start();
Write(m_dev);
Write(addr);
Stop();
Start();
Write(m_dev + 1);
TriState();
for (i=0, val=0; i<8; i++)
{
val <<= 1;
PortWrite(1, 0);
if (DATA_REG&DATA_MASK)
val |= 1;
PortWrite(0, 0);
}
// send NA bit
PortWrite(0, 1);
Drive();
PortWrite(1, 1);
PortWrite(0, 1);
Stop();
return val;
}
task autonomous()
{
switch(AutoSelect)
{
//AUTO SKILLS
case 1:
RedSkills();
break;
// AUTO ROUTINES
case 2:
//BALL AUTO
Balls(3500);
break;
case 3:
Park();
break;
case 4:
WalloB();
break;
case 5:
WalloR();
break;
case 6:
FrontMiddleL();
break;
case 7:
FrontMiddleR();
break;
case 8:
SideMiddleB();
break;
case 9:
SideMiddleR();
break;
///// TEST CODE /////
case 17:
//PID TURN TEST
TurnLeft(900);
wait1Msec(500);
TurnRight(900);
break;
case 18:
//PID DRIVE TEST
Drive(36);
break;
case 19:
// LEFT TEST
LeftTest();
break;
case 20:
// RIGHT TEST
RightTest();
break;
default:
//NOTHING
}
}
void CSVPHostMountCB::DirOpenL(const TDesC& aName ,CDirCB* aDir)
{
DP(_L("CFatMountCB::DirOpenL, drv:%d, %S"), DriveNumber(), &aName);
CSVPHostDirCB& dir=(*((CSVPHostDirCB*)aDir));
TBuf<KMaxPath> name;
TUint driveNumber = Drive().DriveNumber();
CanonicalizePathname(aName, driveNumber, name);
TSVPHostFsDirOpenInfo info(driveNumber, name);
TInt err = SVP_HOST_FS_DEVICE().DirOpen(info);
User::LeaveIfError(err);
dir.SetHandle(info.iHandle);
TFileName n(TDriveUnit(Drive().DriveNumber()).Name());
n.Append(aName);
dir.SetFullName(n);
}
void CSVPHostMountCB::DeleteL(const TDesC& aName)
{
DP(_L("** (SVPHOSTMNT) CSVPHostMountCB::DeleteL()"));
TBuf<KMaxPath> name;
TUint driveNumber = Drive().DriveNumber();
CanonicalizePathname(aName, driveNumber, name);
TSVPHostFsDeleteInfo info(driveNumber, name);
User::LeaveIfError(SVP_HOST_FS_DEVICE().Delete(info));
}
WheelController::WheelController(ISerial *port, int iWheelCount/* = 3*/) : ThreadedClass("WheelController"),
m_iWheelCount(iWheelCount), m_pComPort(port)
{
targetSpeed = { 0, 0, 0 };
#ifndef THREADLESS_WHEELS
Start();
#endif
Drive(0);
};
void Drive::strafeFix(double moveValue,double slideValue){
double heading;
heading = Drive().gyro->GetAngle();
if(heading > 0){
Robot::drive->HDrive(moveValue, -heading*0.65,slideValue);
}else if(heading < 0){
Robot::drive->HDrive(moveValue, heading*0.65, slideValue);
}
}
void
Turn(int direction, int speed)
{
switch(speed) {
case Fast:
switch(direction) {
case Right: Drive(200,200,200); break;
case Left: Drive(50,50,50); break;
}
break;
case Slow:
switch(direction) {
case Right: Drive(140,140,140); break;
case Left: Drive(100,100,100); break;
}
break;
}
}
void CSVPHostMountCB::VolumeL(TVolumeInfo& aVolume) const
{
DP(_L("** (SVPHOSTMNT) CSVPHostMountCB::VolumeL()"));
TInt64 s,f(0);
TFileName n;
TInt d=Drive().DriveNumber();
User::LeaveIfError(GetMediaSize(d,s,f));
aVolume.iFree=f;
}
/******************************************************************************
* Name: AddVelocity
*
* Description: Change current commanded velocity by the given amount. Can be either
* positive or negative.
******************************************************************************/
void DCBrushedMotor::AddVelocity
(
float velocity_change // cm / sec
)
{
float new_velocity = get_commanded_velocity() + velocity_change;
Drive(new_velocity);
} // AddVelocity()
//-------------------------------------------------------------
// Task : Set path's drive
//-------------------------------------------------------------
void CPath::SetDrive(TCHAR chDrive)
{
stdstr_f Drive("%c",chDrive);
std::string Directory;
std::string Name;
std::string Extension;
GetComponents(NULL,&Directory,&Name,&Extension);
SetComponents(Drive.c_str(),Directory.c_str(),Name.c_str(),Extension.c_str());
}
void CHlpFileEntry::GetFullNameAndPath(TDes& aName) const
{
TChar driveLetter = '?';
RFs::DriveToChar(Drive(), driveLetter);
aName.Zero();
aName.Append(driveLetter);
aName.Append(':');
aName.Append(KHlpFileSearchPath);
aName.Append(FileName());
}
void CCar::ReleaseHandBreak()
{
xr_vector<SWheelBreak>::iterator i,e;
i=m_breaking_wheels.begin();
e=m_breaking_wheels.end();
for(;i!=e;++i)
i->Neutral();
if(e_state_drive==drive)
Drive();
}
void CSVPHostMountCB::RenameL(const TDesC& anOldName,const TDesC& aNewName)
{
DP(_L("** (SVPHOSTMNT) CSVPHostMountCB::RenameL()"));
// TODO: do we allow renaming across drives?
TBuf<KMaxPath> oldName, newName;
TUint driveNumber = Drive().DriveNumber();
CanonicalizePathname(anOldName, driveNumber, oldName);
CanonicalizePathname(aNewName, driveNumber, newName);
TSVPHostFsRenameInfo info(driveNumber, oldName, newName);
User::LeaveIfError(SVP_HOST_FS_DEVICE().Rename(info));
}
void CCar::StopBreaking()
{
xr_vector<SWheelBreak>::iterator i,e;
i=m_breaking_wheels.begin();
e=m_breaking_wheels.end();
for(;i!=e;++i)
i->Neutral();
if(e_state_drive==drive)
Drive();
b_breaks=false;
}
/**
Checks that the disk media is still mounted.
@return KErrNone if the media is still mounted; KErrDisMounted otherwise.
*/
EXPORT_C TInt CFormatCB::CheckMount()
{
TDrive& d=Drive();
TInt r=d.CheckMount();
if (r!=KErrNone)
return(r);
if (&Mount()!=&d.CurrentMount())
return(KErrDisMounted);
return(KErrNone);
}
BOOL FarChDir(const string& NewDir, BOOL ChangeDir)
{
if (NewDir.empty())
return FALSE;
BOOL rc=FALSE;
string Drive(L"=A:");
string strCurDir;
// если указана только буква диска, то путь возьмем из переменной
if (NewDir.size() == 2 && NewDir[1]==L':')
{
Drive[1] = ToUpper(NewDir[0]);
if (!api::env::get_variable(Drive, strCurDir))
{
strCurDir = NewDir;
AddEndSlash(strCurDir);
ReplaceSlashToBSlash(strCurDir);
}
if (ChangeDir)
{
rc=api::SetCurrentDirectory(strCurDir);
}
}
else
{
if (ChangeDir)
{
strCurDir = NewDir;
if (strCurDir == L"\\")
api::GetCurrentDirectory(strCurDir); // здесь берем корень
ReplaceSlashToBSlash(strCurDir);
ConvertNameToFull(NewDir,strCurDir);
PrepareDiskPath(strCurDir,false); // resolving not needed, very slow
rc=api::SetCurrentDirectory(strCurDir);
}
}
if (rc || !ChangeDir)
{
if ((!ChangeDir || api::GetCurrentDirectory(strCurDir)) &&
strCurDir.size() > 1 && strCurDir[1]==L':')
{
Drive[1] = ToUpper(strCurDir[0]);
api::env::set_variable(Drive, strCurDir);
}
}
return rc;
}
/** disconnect drive according to lun
@param aLun - lun
@return system wide error code
*/
TInt CDriveManager::Disconnect(TUint aLun)
{
TInt err = KErrNone;
CMassStorageDrive* drive = Drive(aLun, err);
if(drive)
{
drive->SetMountDisconnecting();
}
return err;
}