void Init(){
sonarInit();
PIDInit(myWallFollower,0.05,5,0);
DriveInit(myWheels);
DriveSpeedLinear(myWheels, 10);
printf("Init done");
fflush(stdout);
}
/*FUNCTION*----------------------------------------------------------------
*
* Function Name : DriveRepair
* Returned Value : SUCCESS or ERROR
* - SUCCESS
* - ERROR_DDI_LDL_LDRIVE_INVALID_DRIVE_NUMBER
* - ERROR_DDI_LDL_LDRIVE_FS_FORMAT_REQUIRED
* - Others possible from drive repair API.
* Comments :
* Attempt to repair a drive by doing a low-level format.
*
*END*--------------------------------------------------------------------*/
RtStatus_t DriveRepair
(
/* [IN] Unique tag for the drive to operate on */
DriveTag_t tag,
/* [IN] Temporary unused */
uint32_t u32MagicNumber,
/* [IN] A flag to decide scanning bad blocks or not */
boolean bIsScanBad
)
{ /* Body */
RtStatus_t status;
/* Get the drive object. */
LogicalDrive * drive = DriveGetDriveFromTag(tag);
if (!drive)
{
return ERROR_DDI_LDL_LDRIVE_INVALID_DRIVE_NUMBER;
} /* Endif */
/* Shutdown the drive if it was already initialized. */
if (drive->isInitialized())
{
status = DriveShutdown(tag);
if (status != SUCCESS)
{
return status;
} /* Endif */
} /* Endif */
bool bMediaErased = false;
/* Repair */
status = drive->repair(bIsScanBad);
/* If media was erased, remember to return this error to the caller. */
if (status == ERROR_DDI_LDL_LDRIVE_FS_FORMAT_REQUIRED)
{
bMediaErased = true;
status = SUCCESS;
} /* Endif */
if (status != SUCCESS)
{
return status;
} /* Endif */
/* Initialize the drive. */
status = DriveInit(tag);
if (status != SUCCESS)
{
return status;
} /* Endif */
return bMediaErased ? ERROR_DDI_LDL_LDRIVE_FS_FORMAT_REQUIRED : SUCCESS;
} /* Endbody */
/////////////////////////////////////////////////////////////////////////////////////////
// Program main
int _tmain(int argc, _TCHAR* argv[])
{
PrintInstruction();
memset(controlWord, 0, LEG_COUNT*LEG_JDOF*sizeof(controlWord[0][0]));
memset(statusWord, 0, LEG_COUNT*LEG_JDOF*sizeof(statusWord[0][0]));
memset(homingStatus, 0, LEG_COUNT*LEG_JDOF*sizeof(homingStatus[0][0]));
for (int ch = 0; ch < CAN_Ch_COUNT; ch++) {
for (int node = 0; node < NODE_COUNT; node++) {
if (node == 0) homingStatus[ch][node] = HOMING_DONE;
else homingStatus[ch][node] = HOMING_NONE;
}
}
// open CAN channel:
if (!OpenCAN())
return -1;
DriveReset();
DriveInit();
// start periodic communication:
// printf("start periodic communication...\n");
// StartCANListenThread();
/*SetModeOfOperation();
Sleep(50);
ReadyToSwitchOn();
Sleep(50);
SwitchedOn();
Sleep(50);
OperationEnable();
Sleep(50);*/
/*Shutdown();
Sleep(50);*/
// loop wait user input:
printf("main loop...\n");
MainLoop();
// stop periodic communication:
// printf("stop periodic communication...\n");
// StopCANListenThread();
DriveOff();
// close CAN channel:
CloseCAN();
return 0;
}
//*****************************************************************************
//
// The main entry point for the qs-autonomous example. It initializes the
// system, then eners a forever loop where it runs the scheduler. The
// scheduler then periodically calls all the tasks in the example to keep
// the program running.
//
//*****************************************************************************
int
main (void)
{
//
// Set the system clock to run at 50MHz from the PLL
//
ROM_SysCtlClockSet(SYSCTL_SYSDIV_4 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN |
SYSCTL_XTAL_16MHZ);
//
// Enable UART0, to be used as a serial console.
//
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_UART0);
ROM_GPIOPinTypeUART(GPIO_PORTA_BASE, GPIO_PIN_0 | GPIO_PIN_1);
//
// Initialize the UART standard I/O.
//
UARTStdioInit(0);
UARTprintf("EVALBOT starting\n");
//
// Initialize the simple scheduler to use a tick rate of 100 Hz.
//
SchedulerInit(100);
//
// Initialize all the tasks used in the example.
//
DriveInit();
DisplayTaskInit();
LEDTaskInit();
SoundTaskInit();
AutoTaskInit();
//
// Enter a forever loop and call the scheduler. The scheduler will
// periodically call all the tasks in the system according to the timeout
// value of each task.
//
for(;;)
{
SchedulerRun();
}
}
////////////////////////////////////////////////////////////////////////////////
//
//> Name: Main
//
// Type: Function
//
// Description: Main entry point
//
// Inputs: none
//
// Outputs: none
//
// Notes: none
//<
////////////////////////////////////////////////////////////////////////////////
void _reentrant Task0EntryPoint(void)
{
WORD i,j, k;
BOOL bComplete;
RETCODE rtCode;
WORD wTag;
WORD wResourceDriveNumber;
BOOL bFound;
DWORD dwNumSectors;
DWORD dwSectorCount;
// Init the media
if(MediaInit(0) != SUCCESS)
{
SystemHalt();
}
// Allocate the test drives
if(MediaDiscoverAllocation(0) != SUCCESS)
{
SystemHalt();
}
// Init the drives
for(i=0;i<g_wNumDrives;i++)
{
if(DriveInit(i) != SUCCESS)
{
}
}
// Find the resource.bin drive
bFound = FALSE;
wResourceDriveNumber = 0;
for(i=0;i<g_wNumDrives;i++)
{
if(DriveGetInfo(i, DriveInfoTag, &wTag) != SUCCESS)
{
SystemHalt();
}
if(wTag == DRIVE_TAG_RESOURCE_BIN)
{
wResourceDriveNumber = (WORD)i;
bFound = TRUE;
break;
}
}
if(!bFound)
SystemHalt();
//
// Write & verify all the sectors of all drives
//
// First, loop through all drives & write patterns to all sectors
dwSectorCount = 1;
for(i=0;i<g_wNumDrives;i++)
{
// First get the total number of sectors for the drive
if(DriveGetInfo(i, DriveInfoSizeInSectors, &dwNumSectors) != SUCCESS)
{
SystemHalt();
}
// Now loop through all sectors
for(j=0;j<dwNumSectors;j++)
{
// Init the sector
// for(k=0;k<171;k++) //tt RAM_SECTOR_SIZE is actually 170
for(k=0;k<RAM_SECTOR_SIZE_IN_WORDS;k++)
{
pWriteBuffer[k] = (WORD)(dwSectorCount*0x111111);
}
// Write the sector
if(DriveWriteSector(i, j, pWriteBuffer) != SUCCESS)
{
SystemHalt();
}
// Bump the sector count
dwSectorCount++;
}
}
// Now loop through all drives & verify each sector
dwSectorCount = 1;
for(i=0;i<g_wNumDrives;i++)
{
// First get the total number of sectors for the drive
if(DriveGetInfo(i, DriveInfoSizeInSectors, &dwNumSectors) != SUCCESS)
{
SystemHalt();
}
// Now loop through all sectors
for(j=0;j<dwNumSectors;j++)
{
// Init the read sector
// for(k=0;k<171;k++) //tt RAM_SECTOR_SIZE is actually 170
for(k=0;k<RAM_SECTOR_SIZE_IN_WORDS;k++)
{
pReadBuffer[k] = 0;
}
// Read the sector
if(DriveReadSector(i, j, pReadBuffer) != SUCCESS)
{
SystemHalt();
}
// Verify the sector
// for(k=0;k<171;k++) //tt RAM_SECTOR_SIZE is actually 170
for(k=0;k<RAM_SECTOR_SIZE_IN_WORDS;k++)
{
pWriteBuffer[k] = (WORD)(dwSectorCount*0x111111);
}
// for(k=0;k<171;k++) //tt RAM_SECTOR_SIZE is actually 170
for(k=0;k<RAM_SECTOR_SIZE_IN_WORDS;k++)
{
if(pReadBuffer[k] != pWriteBuffer[k])
{
SystemHalt();
}
}
// Bump the sector count
dwSectorCount++;
}
}
////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////
// SUCCESS
////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////
SystemHalt();
}
VOID FAR _cdecl S506Str1 (VOID)
{
PRPH pRPH; /* Pointer to RPH (Request Packet Header) */
_asm {
mov word ptr pRPH[0], bx /* pRPH is initialize to */
mov word ptr pRPH[2], es /* ES:BX passed from the kernel */
}
StatusError (pRPH, STATUS_DONE); // default to done
switch (pRPH->Cmd) {
case CMDInitBase :
LoadFlatGS();
DriveInit ((PRPINITIN) pRPH);
break;
case CMDGenIOCTL : {
USHORT AwakeCount;
USHORT BlockRet;
DISABLE
if (ReqCount >= 1) {
PUCHAR DataPacketSave;
++ReqCount;
DataPacketSave = ((PRP_GENIOCTL)pRPH)->DataPacket; /* save data packet addr */
/* setup up queue and wait for wakeup */
((PRP_GENIOCTL)pRPH)->DataPacket = (PUCHAR)(struct _ILockEntry FAR*)Queue;
Queue = (ULONG) pRPH;
pRPH->Status |= STBUI;
while (pRPH->Status & STBUI) {
BlockRet = DevHelp_ProcBlock ((ULONG) pRPH, 5000, WAIT_IS_INTERRUPTABLE);
DISABLE
}
/* woke up, clean up and run command now */
((PRP_GENIOCTL)pRPH)->DataPacket = DataPacketSave; /* restore data packet addr */
if (BlockRet == WAIT_TIMED_OUT) {
StatusError (pRPH, STATUS_DONE | STERR);
goto ExitIOCtl;
}
} else {
++ReqCount; /* now processing a command */
Queue = 0L; /* force queue empty */
}
ENABLE
DoIOCTLs ((PRP_GENIOCTL)pRPH); /* process IOCTL subfunctions */
DISABLE
--ReqCount; /* decrement request count */
if (ReqCount) {
/* pick request packet for process to wake up and remove from chain */
pRPH = (PRPH)Queue;
Queue = (ULONG)(((PRP_GENIOCTL)pRPH)->DataPacket);
pRPH->Status &= ~STBUI;
DevHelp_ProcRun ((ULONG)pRPH, &AwakeCount); /* run blocked process */
}
ExitIOCtl:
ENABLE
break;
}
