gyro_board/src/usb/main.c

/*
    FreeRTOS V6.0.5 - Copyright (C) 2010 Real Time Engineers Ltd.

    This file is part of the FreeRTOS distribution.

    FreeRTOS is free software; you can redistribute it and/or modify it under
    the terms of the GNU General Public License (version 2) as published by the
    Free Software Foundation AND MODIFIED BY the FreeRTOS exception.
    ***NOTE*** The exception to the GPL is included to allow you to distribute
    a combined work that includes FreeRTOS without being obliged to provide the
    source code for proprietary components outside of the FreeRTOS kernel.
    FreeRTOS is distributed in the hope that it will be useful, but WITHOUT
    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
    more details. You should have received a copy of the GNU General Public
    License and the FreeRTOS license exception along with FreeRTOS; if not it
    can be viewed here: http://www.freertos.org/a00114.html and also obtained
    by writing to Richard Barry, contact details for whom are available on the
    FreeRTOS WEB site.

*/

/* Standard includes. */
#include "stdio.h"

/* Scheduler includes. */
#include "FreeRTOS.h"
#include "queue.h"
#include "task.h"

/* Demo app includes. */
#include "flash.h"
#include "partest.h"
#include "analog.h"
#include "spi.h"
#include "LPCUSB/usbapi.h"

/*-----------------------------------------------------------*/

/* The time between cycles of the 'check' functionality (defined within the
tick hook. */
#define mainCHECK_DELAY			((portTickType) 5000 / portTICK_RATE_MS)

/* Task priorities. */
#define mainQUEUE_POLL_PRIORITY		(tskIDLE_PRIORITY + 2)
#define mainSEM_TEST_PRIORITY		(tskIDLE_PRIORITY + 1)
#define mainBLOCK_Q_PRIORITY		(tskIDLE_PRIORITY + 2)
#define mainUIP_TASK_PRIORITY		(tskIDLE_PRIORITY + 3)
#define mainINTEGER_TASK_PRIORITY	(tskIDLE_PRIORITY)
#define mainGEN_QUEUE_TASK_PRIORITY	(tskIDLE_PRIORITY)
#define mainFLASH_TASK_PRIORITY		(tskIDLE_PRIORITY + 2)

/* The WEB server has a larger stack as it utilises stack hungry string
handling library calls. */
#define mainBASIC_WEB_STACK_SIZE	(configMINIMAL_STACK_SIZE * 4)

int32_t goal = 0;
int64_t gyro_angle = 0;

/*-----------------------------------------------------------*/

/*
 * Configure the hardware for the demo.
 */
static void prvSetupHardware(void);

/*
 * The task that handles the USB stack.
 */
extern void vUSBTask(void *pvParameters);

extern int VCOM_getchar(void);

int VCOM_putchar(int c);

inline int32_t encoder()
{
	return (int32_t)QEI->QEIPOS;
}

static portTASK_FUNCTION(vPrintPeriodic, pvParameters)
{
	portTickType xLastFlashTime;

	/* We need to initialise xLastFlashTime prior to the first call to
	vTaskDelayUntil(). */
	xLastFlashTime = xTaskGetTickCount();

	analog_init();

	encoder_init();

	// Wait 100 ms for it to boot.
	vTaskDelayUntil(&xLastFlashTime, 100 / portTICK_RATE_MS);
	spi_init();

	// Enable USB.  The PC has probably disconnected it now.
	USBHwAllowConnect();

	// TODO(aschuh): Write this into a gyro calibration function, and check all the outputs.
	vTaskDelayUntil(&xLastFlashTime, 50 / portTICK_RATE_MS);
	enable_gyro_csel();
	printf("SPI Gyro Second Response 0x%x %x\n", transfer_spi_bytes(0x2000), transfer_spi_bytes(0x0000));
	disable_gyro_csel();

	vTaskDelayUntil(&xLastFlashTime, 50 / portTICK_RATE_MS);
	enable_gyro_csel();
	printf("SPI Gyro Third Response 0x%x %x\n", transfer_spi_bytes(0x2000), transfer_spi_bytes(0x0000));
	disable_gyro_csel();

	vTaskDelayUntil(&xLastFlashTime, 10 / portTICK_RATE_MS);
	enable_gyro_csel();
	printf("SPI Gyro Fourth Response 0x%x %x\n", transfer_spi_bytes(0x2000), transfer_spi_bytes(0x0000));
	disable_gyro_csel();
	const int hz = 200;
	const int flash_hz = 10;
	const int startup_cycles = hz * 2;
	const int zeroing_cycles = hz * 6;
	int32_t zero_bias = 0;
	int32_t startup_cycles_left = startup_cycles;
	int32_t zeroing_cycles_left = zeroing_cycles;
	int32_t full_units_offset = 0;
	int32_t remainder_offset = 0;
	int32_t remainder_sum = 0;
	int32_t led_flash = 0;
	vParTestSetLED(0, 0);

	for (;;) {
		led_flash ++;
		if (led_flash < hz / flash_hz / 2) {
			vParTestSetLED(1, 0);
		} else {
			vParTestSetLED(1, 1);
		}
		if (led_flash >= hz / flash_hz) {
			led_flash = 0;
		}
		/* Delay for half the flash period then turn the LED on. */
		vTaskDelayUntil(&xLastFlashTime, 1000 / hz / portTICK_RATE_MS);
		enable_gyro_csel();
		uint16_t high_value = transfer_spi_bytes(0x2000);
		uint16_t low_value = transfer_spi_bytes(0x0000);
		disable_gyro_csel();
		int16_t gyro_value = -((int16_t)((((uint32_t)high_value << 16) | (uint32_t)low_value) >> 10));

		if (startup_cycles_left) {
			vParTestSetLED(2, 0);
			--startup_cycles_left;
		} else if (zeroing_cycles_left) {
			vParTestSetLED(2, 1);
			//printf("Zeroing ");
			--zeroing_cycles_left;
			zero_bias -= gyro_value;
			if (zeroing_cycles_left == 0) {
				// Do all the nice math
				full_units_offset = zero_bias / zeroing_cycles;
				remainder_offset = zero_bias % zeroing_cycles;
				if (remainder_offset < 0) {
					remainder_offset += zeroing_cycles;
					--full_units_offset;
				}
			}
		} else {
			vParTestSetLED(2, 0);
			int64_t new_angle = gyro_angle + gyro_value + full_units_offset;
			if (remainder_sum >= zeroing_cycles) {
				remainder_sum -= zeroing_cycles;
				new_angle += 1;
			}
			NVIC_DisableIRQ(USB_IRQn);
			gyro_angle = new_angle;
			NVIC_EnableIRQ(USB_IRQn);
			remainder_sum += remainder_offset;
		}
		//printf("Angle %d Rate %d\n", (int)(gyro_angle / 16), (int)(gyro_value + full_units_offset));

		//printf("time: %d analog %d encoder %d goal %d\n", (int)i, (int)analog(5), (int)encoder(), (int)goal);

		//printf("time: %d encoder %d goal %d\n", (int)i, (int)encoder(), (int)goal);
		/*
		for(i = 0; i < 4; i++){
			printf("analog(%d) => %d\n",i,analog(i));
		}
		for(i = 1; i < 13; i++){
			printf("digital(%d) => %d\n",i,digital(i));
		}
		for(i = 0; i < 4; i++){
			printf("dip(%d) => %d\n",i,dip(i));
		}
		for(i = 0; i < 4; i++){
			printf("encoder(%d) => %d\n",i,encoder_bits(i));
		}
		for(i = 0; i < 4; i++){
			printf("encoder_val(%d) => %d\n",i,(int)encoder_val(i));
		}*/
	}
}

#include "CAN.h"



void motor(int32_t speed)
{
	if (speed > 2047) speed = 2047;
	if (speed < -2047) speed = -2047;
	return;
	if (speed > 0) {
		MCPWM->MCMAT1 = 2047 - speed;
		MCPWM->MCMAT0 = 2048;
	} else {
		MCPWM->MCMAT1 = 2048;
		MCPWM->MCMAT0 = speed + 2047;
	}
}



/*-----------------------------------------------------------*/

int main(void)
{
        // Configure the hardware
        prvSetupHardware();

        /* Start the standard demo tasks.  These are just here to exercise the
        kernel port and provide examples of how the FreeRTOS API can be used. */
        //vStartLEDFlashTasks(mainFLASH_TASK_PRIORITY);

        /* Create the USB task. */
        xTaskCreate(vUSBTask, (signed char *) "USB", configMINIMAL_STACK_SIZE + 1020, (void *) NULL, tskIDLE_PRIORITY + 3, NULL);

	xTaskCreate(vPrintPeriodic, (signed char *) "PRINTx", configMINIMAL_STACK_SIZE + 100, NULL, tskIDLE_PRIORITY + 2, NULL);

	initCAN();

        // Start the scheduler.
        vTaskStartScheduler();

        /* Will only get here if there was insufficient memory to create the idle
           task.  The idle task is created within vTaskStartScheduler(). */
        for (;;);
}
/*-----------------------------------------------------------*/


void vApplicationTickHook(void)
{
        static unsigned long ulTicksSinceLastDisplay = 0;

        /* Called from every tick interrupt as described in the comments at the top
        of this file.

        Have enough ticks passed to make it	time to perform our health status
        check again? */

        ulTicksSinceLastDisplay++;

        if (ulTicksSinceLastDisplay >= mainCHECK_DELAY) {
                /* Reset the counter so these checks run again in mainCHECK_DELAY
                ticks time. */
                ulTicksSinceLastDisplay = 0;
        }
}
/*-----------------------------------------------------------*/

void prvSetupHardware(void)
{
	// Setup the peripherals.
	// The CPU will be running at 100 MHz with a 12 MHz clock input.

        // Setup GPIO power.
        SC->PCONP = PCONP_PCGPIO;
        // Disable TPIU.
        PINCON->PINSEL10 = 0;

	// Setup PLL0 so that the CPU runs at 100 MHz.
        if (SC->PLL0STAT & (1 << 25)) {
                /* Enable PLL, disconnected. */
                SC->PLL0CON = 1;
                SC->PLL0FEED = PLLFEED_FEED1;
                SC->PLL0FEED = PLLFEED_FEED2;
        }

        // Disable PLL, disconnected.
        SC->PLL0CON = 0;
        SC->PLL0FEED = PLLFEED_FEED1;
        SC->PLL0FEED = PLLFEED_FEED2;

        // Enable main OSC and wait until it's ready.
        SC->SCS |= 0x20;
        while (!(SC->SCS & 0x40));

        // select main OSC, 12MHz, as the PLL clock source.
        SC->CLKSRCSEL = 0x1;

        SC->PLL0CFG = 0x20031;
        SC->PLL0FEED = PLLFEED_FEED1;
        SC->PLL0FEED = PLLFEED_FEED2;

        // Enable PLL, disconnected.
        SC->PLL0CON = 1;
        SC->PLL0FEED = PLLFEED_FEED1;
        SC->PLL0FEED = PLLFEED_FEED2;

        // Set clock divider.
        SC->CCLKCFG = 0x03;

        // Configure flash accelerator.
        SC->FLASHCFG = 0x403a;

        // Check lock bit status.
        while (((SC->PLL0STAT & (1 << 26)) == 0));

        // Enable and connect.
        SC->PLL0CON = 3;
        SC->PLL0FEED = PLLFEED_FEED1;
        SC->PLL0FEED = PLLFEED_FEED2;

        while (((SC->PLL0STAT & (1 << 25)) == 0));

        // Configure the clock for the USB.
        if (SC->PLL1STAT & (1 << 9)) {
                // Enable PLL, disconnected.
                SC->PLL1CON = 1;
                SC->PLL1FEED = PLLFEED_FEED1;
                SC->PLL1FEED = PLLFEED_FEED2;
        }

        // Disable PLL, disconnected.
        SC->PLL1CON = 0;
        SC->PLL1FEED = PLLFEED_FEED1;
        SC->PLL1FEED = PLLFEED_FEED2;

        SC->PLL1CFG = 0x23;
        SC->PLL1FEED = PLLFEED_FEED1;
        SC->PLL1FEED = PLLFEED_FEED2;

        /* Enable PLL, disconnected. */
        SC->PLL1CON = 1;
        SC->PLL1FEED = PLLFEED_FEED1;
        SC->PLL1FEED = PLLFEED_FEED2;
        while (((SC->PLL1STAT & (1 << 10)) == 0));

        /* Enable and connect. */
        SC->PLL1CON = 3;
        SC->PLL1FEED = PLLFEED_FEED1;
        SC->PLL1FEED = PLLFEED_FEED2;
        while (((SC->PLL1STAT & (1 << 9)) == 0));

        // Setup the peripheral bus to be the same as the CCLK, 100 MHz.
	// Set CAN to run at CCLK/6, which should have it running about 1 Mbit (1.042)
        SC->PCLKSEL0 = 0xff555555;

        /* Configure the LEDs. */
        vParTestInitialise();
}
/*-----------------------------------------------------------*/

void vApplicationStackOverflowHook(xTaskHandle *pxTask, signed char *pcTaskName)
{
        /* This function will get called if a task overflows its stack. */

        (void) pxTask;
        (void) pcTaskName;

        for (;;);
}
/*-----------------------------------------------------------*/

void vConfigureTimerForRunTimeStats(void)
{
        const unsigned long TCR_COUNT_RESET = 2, CTCR_CTM_TIMER = 0x00, TCR_COUNT_ENABLE = 0x01;

        /* This function configures a timer that is used as the time base when
        collecting run time statistical information - basically the percentage
        of CPU time that each task is utilising.  It is called automatically when
        the scheduler is started (assuming configGENERATE_RUN_TIME_STATS is set
        to 1). */

        /* Power up and feed the timer. */
        SC->PCONP |= 0x02UL;
        SC->PCLKSEL0 = (SC->PCLKSEL0 & (~(0x3 << 2))) | (0x01 << 2);

        /* Reset Timer 0 */
        TIM0->TCR = TCR_COUNT_RESET;

        /* Just count up. */
        TIM0->CTCR = CTCR_CTM_TIMER;

        /* Prescale to a frequency that is good enough to get a decent resolution,
        but not too fast so as to overflow all the time. */
        TIM0->PR = (configCPU_CLOCK_HZ / 10000UL) - 1UL;

        /* Start the counter. */
        TIM0->TCR = TCR_COUNT_ENABLE;
}