C++ IORD函数代码示例

//Plays the audio backwards by iterating from end of buffer to the beginning
int ReversePlay(data_file file_, int l_, int* clusters_)
{
	int i, j;
	BYTE playBuffer[BUF_SIZE] = {0};

	for (i = l_ * BPB_SecPerClus; i > 0; i--)
	{
		if (_playing == 0)
			break;

		get_rel_sector(&file_, playBuffer, clusters_, i);

		//Starts from the end and works backwards
		for (j = 508; j > 0; j-=6)
		{
			//Wait for signal to write for audio, then write to it with prepared bytes
			while(IORD(AUD_FULL_BASE, 0)){}
			IOWR(AUDIO_0_BASE, 0 , BytePrep(playBuffer[j], playBuffer[j + 1]));

			//To play the next 2 bits of reverse batch (right side)
			j+=2;
			while(IORD(AUD_FULL_BASE, 0)){}
			IOWR(AUDIO_0_BASE, 0 , BytePrep(playBuffer[j], playBuffer[j + 1]));
		}
	}
}

// this function is called immediately upon exiting your main program to softly shut down the 324EGM and report test details
void finalize(void)
{
	int nMissed; // missed events
	int nLatency; // maximum latency (in 1024ths of a period)
	int nLatency_ms; // maximum latency (in milliseconds)

	// immediately disable the 324EGM
	IOWR(PIO_EGMENABLE_BASE, 0, 0);
	IOWR(PIO_RESPONSE_BASE, 0, 1);
	usleep(10);

	// obtain the test results
	nLatency = IORD(PIO_LATENCY_BASE, 0);
	nMissed = IORD(PIO_MISSED_BASE, 0);

	// calculate the latency in milliseconds
	nLatency_ms = nLatency * (g_period + 1) * 3 / 25;

	// print the results
	printf("Test complete.\n");
	printf("Results:\n");
	printf("  missed = %d pulse(s)\n", nMissed);
	printf("  max latency = %d / 1024th(s) of a period\n", nLatency);
	printf("  max latency = %d microsecond(s)\n", nLatency_ms);
	printf("  task units processed = %d units\n\n", g_taskProcessed);
	printf("Exiting...\n");

	// this tells the program that we're done
	// removed to allow looping.
	// printf("\004");
	return;
}

/* Tests basic sigmoid functionality */
int test_sigmoid(void){

	float M,time,result;

	alltypes_u input;

	M = 2.24;
	time = 3.8;
	result = 0;

	fprintf(stderr,"Starting sigmoid test....\n");

	fprintf(stderr,"Sigmoid block test register returns [%X].\n", IORD(EULERBLOCK_0_BASE,3));

	fprintf(stderr,"Floats have byte count of [%d].\n",(int)sizeof(float));

	input.fl = M;

	IOWR(EULERBLOCK_0_BASE,0,input.ui);

	/* Check it's corrent */
	input.ui = IORD(EULERBLOCK_0_BASE, 0);


	fprintf(stderr,"Inputs are M=[%4.2f], time=[%4.2f].\n",M,time);

	fprintf(stderr,"IORD returns [%4.8f].\n", input.fl);

	sigmoid(M, time, &result);

	fprintf(stderr,"Sigmoid returned [%4.8f].\n", result);

	return 0;
}

int DelayPlay(data_file file_, int l_, int* clusters_)
{
	int i, j;

	//Create an additional delay buffer with the size of sample rate
	BYTE playBuffer[BUF_SIZE] = {0};
	UINT16 delayBuffer[SAMPLE_RATE] = {0};

	int idxDelay, flag = 0;

	//Iterate through the audio
	for (i = 0; i < l_ * BPB_SecPerClus; i++)
	{
		if (_playing == 0)
			break;

		get_rel_sector(&file_, playBuffer, clusters_, i);

		for (j = 0; j < BUF_SIZE; j+=2)
		{
			//Wait for signal to write for audio, then write to it with prepared bytes
			while(IORD(AUD_FULL_BASE, 0)){}

			//Populate delayBuffer using idxDelay with what's in playBuffer on an edge
			if (flag == 0){
				IOWR(AUDIO_0_BASE, 0 , BytePrep(playBuffer[j], playBuffer[j + 1]));
				flag = 1;
			}else{
				IOWR(AUDIO_0_BASE, 0, delayBuffer[idxDelay]);
				delayBuffer[idxDelay] = BytePrep(playBuffer[j], playBuffer[j + 1]);
				flag = 0;
			}

			//If we exceed the buffer size, loop around to beginning of array
			idxDelay++;
			if (idxDelay > SAMPLE_RATE)
				idxDelay = idxDelay % SAMPLE_RATE;
		}
	}

	//Finish from current delay index to end of delay array
	for (i = idxDelay; i < SAMPLE_RATE; i++)
	{
		if (_playing == 0)
			break;

		while(IORD(AUD_FULL_BASE, 0)){}
		IOWR(AUDIO_0_BASE, 0, delayBuffer[i]);
	}

	//Finish last remaining audio data from beginning to delayIdx
	for (i = 0; i < idxDelay; i++)
	{
		if (_playing == 0)
			break;

		while(IORD(AUD_FULL_BASE, 0)){}
		IOWR(AUDIO_0_BASE, 0, delayBuffer[i]);
	}
}

USHORT Hal4D13_ReadEndpointWOClearBuffer(UCHAR bEPIndex, UCHAR * buf, USHORT len)
{
    USHORT i, j,c;
    IOWR(USB_0_BASE,D13_COMMAND_PORT, D13CMD_EP_RD_FIFO + bEPIndex);
    /* read Buffer */
    j = IORD(USB_0_BASE,D13_DATA_PORT);
    if(j > len)
        j = len;
    i=0;
    while (i<j) //<<    
   //for(i<j; i=i+2, buf++ )
    {
         c = IORD(USB_0_BASE,D13_DATA_PORT);
         *buf = (UCHAR)c;//printf("WOC= %02X ",*buf);//<<        
         i++;//<<
         if (i >= j) break; //<<
         buf++;
         *buf = (UCHAR)(c>>8); //printf("WOC= %02X ",*buf);//<<
         i++;//<<
     buf++;
    }
   // printf("\n",c);
    /* Clear Buffer */
    IOWR(USB_0_BASE,D13_COMMAND_PORT, D13CMD_EP_CLEAR_BUF+bEPIndex);

    return j;
}

ULONG Hal4D13_GetIntEnable(void)
{
    ULONG i;
    IOWR(USB_0_BASE,D13_COMMAND_PORT,D13CMD_DEV_RD_INTEN);
    i = IORD(USB_0_BASE,D13_DATA_PORT);
    i += (((ULONG)IORD(USB_0_BASE,D13_DATA_PORT)) << 16);
    return i;
}

void init_task(void *pdata)
{
    /* Tells the user that we are up and running. */
    WARNING(0, "System boot !");

#if 0
    /* Inits the custom math lib. */
    NOTICE(0, "Fast math init.");
    fast_math_init();
#endif

    cvra_beacon_init(&robot.beacon, AVOIDING_BASE, AVOIDING_IRQ, 100, 10., 0.);
    cvra_beacon_set_direction_offset(&robot.beacon, 123);



    ip_stack_init();
    list_netifs();

    /* If the logic power supply is off, kindly ask the user to turn it on. */
    if ((IORD(PIO_BASE, 0) & 0xff) == 0) {
        printf("Hey sac a pain, la commande c'est en option ?\n");

        /* We refuse to let the user to a shell before he turns it on. */
        while ((IORD(PIO_BASE, 0) & 0xff) == 0);
        printf("Merci bien !\n");
    }

    /* Inits all the trajectory stuff, PID, odometry, etc... */
#if 1
    NOTICE(0, "Main control system init.");
    cvra_cs_init();
#endif



    /* Sets the bounding box for the avoidance module. */
    const int robot_size = 150;
    polygon_set_boundingbox(robot_size, robot_size, 3000-robot_size, 2000-robot_size);

    arm_highlevel_init();

    lua_do_settings();

    luaconsole_init();

    OSTaskCreateExt(heartbeat_task,
                    NULL,
                    &heartbeat_task_stk[TASK_STACKSIZE-1],
                    HEARTBEAT_TASK_PRIORITY,
                    HEARTBEAT_TASK_PRIORITY,
                    &heartbeat_task_stk[0],
                    TASK_STACKSIZE,
                    NULL, NULL);

    /* Tasks must delete themselves before exiting. */
    OSTaskDel(OS_PRIO_SELF);
}

// return 0 : success
//        1 : fail
int iic_read(
    unsigned char addr, // 7bit-addr
    int num,
    unsigned char *data )
{
    int i, r, err;

    err = 0;

    IOWR( IIC_0_BASE, 0, 0x100 ); // I2C Start
    r = IORD( IIC_0_BASE, 0 );
    while( (r & 0x100) ) {
        r = IORD( IIC_0_BASE, 0 );
    }
//  alt_printf("S%x\n", r );

    addr = (addr << 1) | 1;

    IOWR( IIC_0_BASE, 0, addr ); // I2C WRITE
    r = IORD( IIC_0_BASE, 0 );
    while( (r & 0x100) ) {
        r = IORD( IIC_0_BASE, 0 );
    }
//  alt_printf("C%x\n", r );
    if( r & 0x400 ) { // NACK
        err = 1;
        goto end;
    }

    for( i = 0; i < num-1; i++ ) {
        IOWR( IIC_0_BASE, 0, 0x300 ); // I2C READ with ACK
        r = IORD( IIC_0_BASE, 0 );
        while( (r & 0x100) ) {
            r = IORD( IIC_0_BASE, 0 );
        }
//      alt_printf("R%x\n", r );
        data[i] = r;
    }

    IOWR( IIC_0_BASE, 0, 0x301 ); // I2C READ with NACK
    r = IORD( IIC_0_BASE, 0 );
    while( (r & 0x100) ) {
        r = IORD( IIC_0_BASE, 0 );
    }
//  alt_printf("R%x\n", r );
    data[i] = r;

end:
    IOWR( IIC_0_BASE, 0, 0x200 ); // I2C STOP
    r = IORD( IIC_0_BASE, 0 );
    while( (r & 0x100) ) {
        r = IORD( IIC_0_BASE, 0 );
    }
//  alt_printf("P%x\n", r );

    return err;
}

ULONG Hal4D13_ReadInterruptRegister(void)
{
    ULONG j,i = 0;
    IOWR(USB_0_BASE,D13_COMMAND_PORT, D13CMD_DEV_INT_SRC);
    i = IORD(USB_0_BASE,D13_DATA_PORT);
    j = IORD(USB_0_BASE,D13_DATA_PORT);
    j = ((j<<16) & 0xffff0000 ) + (i & 0xffff);
    return i;
}

void DEMO_ENCODERS(void) {
    //read from pins like for PWM
    double encode_0_a, encode_1_a; //encode_b;
    double counts_0_new, counts_0_old;
    double counts_1_new, counts_1_old;
    counts_0_old = 0;
    counts_1_old = 0;

    /* Init motor */
    IOWR(PWM_0_BASE, 0, 0);
    IOWR(PWM_1_BASE, 0, 0);
    IOWR(PWM_2_BASE, 0, 0);
    IOWR(PWM_3_BASE, 0, 0);

    IOWR(PWM_0_BASE, 0, 1 * 31250);
    //IOWR(PWM_2_BASE, 0, 0.5 * 31250);

    //64 counts per revolution, @ 100% is 80rev/min
    //H-bridge slows voltage output, motors see around 8V max
    //(64 counts per revolution of motor shaft) * (131 revolutions of motor shaft per revolutions of output shaft) * (1 revolution of output shaft per 2*PI*4 distance)
    // = 210712.9025
    //    60 / (duty * 80)
/// 43 increments (12V), 1.3333 revs per second
    /*
    	counting only the rising edges of channel A = half of counting the rising/falling edges of channel A+B
    	So 64 counts => 32 counts per revolution
    */

    while (1)
    {
        //1st motor
        counts_0_new = ((double) IORD(ENCODER_0A_BASE, 0)) / 32;
        encode_0_a = (counts_0_new-counts_0_old);
        //encode_b = (((double) IORD(ENCODER_0B_BASE, 0)) / 32) - encode_b;
        if (encode_0_a < 0) {
            encode_0_a = counts_0_new;
        }
        counts_0_old = counts_0_new;

        //2nd motor
        counts_1_new = ((double) IORD(ENCODER_1A_BASE, 0)) / 32;
        encode_1_a = (counts_1_new-counts_1_old);
        if (encode_1_a < 0) {
            encode_1_a = counts_1_new;
        }
        counts_1_old = counts_1_new;

        printf("Motor 0 encoder A value is :( %f ) \n", encode_0_a);
        printf("Motor 0 revolutions per second :( %f ) \n", encode_0_a / 64);
        printf("Motor 1 encoder A value is :( %f ) \n", encode_1_a);
        printf("Motor 1 revolutions per second :( %f ) \n", encode_1_a / 64);

        //printf("Current encoder B value is :( %f ) \n", encode_b / 32);
        usleep(1000000);
    }
}

static void pulse_ISR(void* context, alt_u32 id)
{
	   if(IORD(PIO_PULSE_BASE, 0) == 0 && IORD(PIO_RESPONSE_BASE, 0) == 1){
	   		IOWR(PIO_RESPONSE_BASE, 0, 0);
	   		eventCounterPulse++;
	   }
	   if(IORD(PIO_PULSE_BASE, 0) == 1 && IORD(PIO_RESPONSE_BASE, 0) == 0){
	      		IOWR(PIO_RESPONSE_BASE, 0, 1);
	   }
	   // acknowledge the interrupt by clearing the TO bit in the status register
	   IOWR(PIO_PULSE_BASE, 3, 0x0);
}

/* A normalized sigmoid - result is always 0-1
 * M is a time value which must be the same unit as the "time" parameter and represents
 * the half-time point*/
int sigmoid(float M, float time, float*result) {

	alltypes_u input,output;

	/* Semaphore is normally created by the movement init function.. */
	if(xSemaphore == NULL){
		xSemaphore = xSemaphoreCreateMutex();
	}

	/* The M value is the half time point where the gradient is maximum */

	/* Boundary checks ? */

	/* Do the sigmoid calcs */
	input.fl = (-1 * (SIGMOID_ERR) * (time - M)) / M;

	if (xSemaphore != NULL) {
		// See if we can obtain the semaphore.  If the semaphore is not available
		// wait 10 ticks to see if it becomes free.
		if (xSemaphoreTake( xSemaphore, ( portTickType ) 10 ) == pdTRUE) {

			/* Write to block and wait for the euler block to become ready -
			 * this is known at 17 cycles. So
			 * really we should just run 17nops. Remember a CS takes many more ! So let's
			 * give it the benefit of the doubt and just run nop if we can't use it. If all the
			 * other servos are trying to use it right at this moment then maybe we've got problems.
			 * We could make this a CRITICAL section...*/

			IOWR(EULERBLOCK_0_BASE,0,input.ui);

			while (IORD(EULERBLOCK_0_BASE,2)==0);

			output.ui = IORD(EULERBLOCK_0_BASE, 1);

			xSemaphoreGive( xSemaphore);

		} else {
			fprintf(stderr,"Servo couldn't get EULERBLOCK semaphore! \n");
			// We could not obtain the semaphore and can therefore not access
			// the shared resource safely.
		}

	}

	else{
		fprintf(stderr,"ERROR: Sigmoid sempahore is NULL. Returning.\n");
		return ECD_ERROR;
		}

	*result = 1.0/(1 + output.fl);

	return ECD_OK;
}

unsigned short readPixel(int col, int row, int region)
{
    int address = (row<<8) + col;
    if(region == 0)
        return IORD(TL_BASE, address);
    else if(region == 1)
        return IORD(TR_BASE, address);
    else if(region == 2)
        return IORD(BL_BASE, address);
    else if(region == 3)
        return IORD(BR_BASE, address);    
    return 0;
}

static void timer_ISR(void* context, alt_u32 id)
{
   // acknowledge the interrupt by clearing the TO bit in the status register
   IOWR(TIMER_1_BASE, 0, 0x0);
   // only write the current value to the RESPONSE_BASE when the values of PULSE_BASE and RESPONSE_BASE are different
   if(IORD(PIO_PULSE_BASE, 0) == 0 && IORD(PIO_RESPONSE_BASE, 0) == 1){
   		IOWR(PIO_RESPONSE_BASE, 0, 0);
   		eventCounter++;
   }
   if(IORD(PIO_PULSE_BASE, 0) == 1 && IORD(PIO_RESPONSE_BASE, 0) == 0){
      		IOWR(PIO_RESPONSE_BASE, 0, 1);
   }
}

/* MMCから1バイト受信 */
alt_u8 mmc_spi_Recvbyte(void)
{
	alt_u32 res;

	while( !(IORD(mmc_spi_reg, mmcreg_status) & mmc_commexit) ) {}
	IOWR(mmc_spi_reg, mmcreg_status, (mmc_commstart | mmc_spi_ncs | 0xff));

	do {
		res = IORD(mmc_spi_reg, mmcreg_status);
	} while( !(res & mmc_commexit) );

	return (alt_u8)(res & 0xff);
}