void FlyportTask2()
{
const int maxBright = 100; //here we set max % of brightness
const int minBright = 0; //and here the min %
float bright = (float)maxBright;
int static initialized=0;
if(!initialized) {
initialized=1;
PWMInit(1,50,maxBright);
PWMOn(p19, 1);//Assign pin 9 as PWM1 and turns it on
}
//while(1)
{
for (bright = maxBright; bright > minBright; bright--)
{
PWMDuty(bright, 1);
vTaskDelay(1); //used to slow down the effect
}
for (bright = minBright; bright < maxBright; bright ++)
{
PWMDuty(bright, 1);
vTaskDelay(1); //used to slow down the effect
}
UARTWrite(1,"changed");
}
}
/****************************************************************************
Function
PulseInit
Parameters
none
Returns
none
Description
Initializes the pulse output pin
Notes
Authors
Jordan A. Miller, 11/08/15, 14:52
****************************************************************************/
void PulseInit(void) {
PWMInit();
currentIRState = false;
PWM8_TIVA_SetPeriod(PERIOD,2); // Group 2 includes PE4 and PE5
}
void CPUInit(void)
{
//Initialise GPIO as alternate function: PWM function
GPIOInit();
//Setting PWM
PWMInit();
}
//Function to configure ADC Channels, PWM Channels and GPIO pins for DOF1 Control
void robot_setconfig()
{
//UART_Init(0,9600);
ADCInit(ADC_CLK);
PWMInit();
GPIOInit();
}
int main(void) {
WDTCTL = WDTPW | WDTHOLD; // Stop watchdog timer
LedInit();
PWMInit();
while(1){
}
}
/**
* @brief Sets up the hardware.
*
* - Starts the clock at 48MHz with the PLL
* - Starts the MAM (Memory accelerator module) for faster reads and writes
* - Initialize the LED outputs
* - Initialize the PWM outputs
* - Initialize the ADC inputs
* - Initialize the LCD outputs
* - Start the ROS and Debug serial ports
*/
static void prvSetupHardware( void )
{
#ifdef RUN_FROM_RAM
/* Remap the interrupt vectors to RAM if we are are running from RAM. */
SCB_MEMMAP = 2;
#endif
/* Disable the PLL. */
PLLCON = 0;
PLLFEED = mainPLL_FEED_BYTE1;
PLLFEED = mainPLL_FEED_BYTE2;
/* Configure clock source. */
SCS |= mainOSC_ENABLE;
while( !( SCS & mainOSC_STAT ) );
CLKSRCSEL = mainOSC_SELECT;
/* Setup the PLL to multiply the XTAL input by 4. */
PLLCFG = ( mainPLL_MUL | mainPLL_DIV );
PLLFEED = mainPLL_FEED_BYTE1;
PLLFEED = mainPLL_FEED_BYTE2;
/* Turn on and wait for the PLL to lock... */
PLLCON = mainPLL_ENABLE;
PLLFEED = mainPLL_FEED_BYTE1;
PLLFEED = mainPLL_FEED_BYTE2;
CCLKCFG = mainCPU_CLK_DIV;
while( !( PLLSTAT & mainPLL_LOCK ) );
/* Connecting the clock. */
PLLCON = mainPLL_CONNECT;
PLLFEED = mainPLL_FEED_BYTE1;
PLLFEED = mainPLL_FEED_BYTE2;
while( !( PLLSTAT & mainPLL_CONNECTED ) );
/*
This code is commented out as the MAM does not work on the original revision
LPC2368 chips. If using Rev B chips then you can increase the speed though
the use of the MAM.
Setup and turn on the MAM. Three cycle access is used due to the fast
PLL used. It is possible faster overall performance could be obtained by
tuning the MAM and PLL settings.
*/
MAMCR = 0;
MAMTIM = mainMAM_TIM_3;
MAMCR = mainMAM_MODE_FULL;
/* Setup the led's on the MCB2300 board */
vParTestInitialise();
PWMInit();
ADCInit();
LcdInit();
IOInit();
rosPortHandle = xSerialPortInit( serCOM1, ser57600, serNO_PARITY, serBITS_8,
serSTOP_1, 1000 );
//debugPortHandle = xSerialPortInit( serCOM2, ser57600, serNO_PARITY, serBITS_8,
// serSTOP_1, 250 );
}
/*********************************************************************************************************
** Function name: FanInit
** Descriptions:
** input parameters:
**
** Output parameters:: 无
** Returned value:
**
** Created by: zhangwen
** Created Date:
**--------------------------------------------------------------------------------------------------------
** Modified by:
** Modified date:
**--------------------------------------------------------------------------------------------------------
*********************************************************************************************************/
void FanInit()
{
/// ConfigureFanFb();
PWMInit(FAN_DEFAULT_PWM);
GPIOPinWrite(GPIO_PORTF_BASE, GPIO_PIN_3, GPIO_PIN_3);
SetFanOnOff(1);
//g_u8FanSpeed = (FAN_DEFAULT_PWM-55)*3 + 1;
LCDFanSpeedShow(g_u8FanSpeed);
}
void init()
{
PWMInit();
init_1602();
initTimer0();
init_serial();
timePrint();
car.current = TG_START;
}
int main (void)
{
PWMInit();
while(1)
{
}
return 0;
}
/***************************************************************
** 作 者: Songyimiao
** 官 网：http://www.miaowlabs.com
** 淘 宝：http://miaowlabs.taobao.com
** 日 期: 2015年11月29日
** 函数名称: DriversInit
** 功能描述: 底层驱动初始化
** 输 入:
** 输 出:
** 备 注:
********************喵呜实验室版权所有**************************
***************************************************************/
void DriversInit(void)
{
GPIOInit();
Timer1Init();
PWMInit();
Uart1Init();
Uart2Init();
Timer3Timer4Init();
}
uint32_t CmdHandler(uint8_t * data, uint32_t size)
{
uint8_t index;
uint8_t val;
index = data[1] - '0';
switch (data[0]) {
case 'P':
desc.ioPin[index].port = data[2] - '0';
desc.ioPin[index].pin = data[3] - '0';
desc.ioPin[index].mode = data[4] - '0';
GPIOPinTypeGPIOOutput(MapPort(desc.ioPin[index].port),
MapPin(desc.ioPin[index].pin));
SaveConfig();
break;
case 'I':
GPIOPinWrite(GPIO_PORTF_BASE, MapPin(desc.ioPin[index].pin),
MapPin(desc.ioPin[index].pin));
break;
case 'i':
GPIOPinWrite(GPIO_PORTF_BASE, MapPin(desc.ioPin[index].pin),
0x00);
break;
case 'T':
val = ADCTempRead(data[2]);
usprintf(data, "%d", val);
return sizeof(data);
case 'L':
usprintf(data, "%d%d%d", desc.ioPin[index].port,
desc.ioPin[index].pin, desc.ioPin[index].mode);
return sizeof(data);
case 'A':
desc.ioPin[0].port = 0;
SaveConfig();
PWMInit();
return sizeof(data);
case 'W':
PWMWidthSet(1);
return sizeof(data);
case 'w':
PWMWidthSet(0);
return sizeof(data);
default:
desc.ioPin[0].pin = data[3] - '0';
}
return size;
}
//////////////////////////////////////////////////////////////////////////////////////////
//
// TransducerInit - Initialize the driver interface
//
// Inputs: None.
//
// Outputs: None.
//
void TransducerInit(void) {
//
// Initialize sub components to this module
//
SPIInit;
SG3525_INIT;
AD9833Init();
ACS712Init();
PWMPotInit;
PWMInit();
InputsInit();
OutputsInit();
//
// See initialization struct above
//
// TransducerSet.Freq = SG3525_DEF_FREQ;
// TransducerSet.Power = SG3525_MIN_POWER;
// TransducerSet.RunMode = RUN_CONTINUOUS;
// TransducerSet.RunTimer = 0;
// TransducerSet.CtlMode = PWR_CONST_FREQ;
//
memcpy_P(&TransducerSet,&TransducerDefaults,sizeof(TransducerSet));
//
// TransducerCurr.Freq = 0;
// TransducerCurr.Power = 0;
// TransducerCurr.RunTimer = 0;
// TransducerCurr.Current = 0;
// TransducerCurr.PWM = 0;
// TransducerCurr.PWMWiper = 0;
// TransducerCurr.EStop = false;
// TransducerCurr.On = false;
//
memset(&TransducerCurr,0,sizeof(TransducerCurr));
TransducerSetup();
TransducerOn(false);
TransducerEStop(true);
//
// The default power wiper aetting was determined experimentally, and
// represents a reasonably small initial PWM (24%) as a starting point.
//
TransducerCurr.PWMWiper = 30;
PWMPotSetWiper(TransducerCurr.PWMWiper);
}
void SoftReset() {
BYTE ipl_backup = SRbits.IPL;
SRbits.IPL = 7; // disable interrupts
log_printf("SoftReset()");
TimersInit();
PinsInit();
PWMInit();
ADCInit();
UARTInit();
SPIInit();
I2CInit();
InCapInit();
// TODO: reset all peripherals!
SRbits.IPL = ipl_backup; // enable interrupts
}
void Init_All(void){
// SetBusClock_24MHz();
//initPLL();
//eOSC_SetBusCLK_2nM(24);
initPLL();
initAPI();
PWMInit();
ADC_Init();
select_plan();
// OutputCompare_Init();
// PITInit();
// Init_MMA();
//SCI0_BR(38400);
//SCI0_Init();
//SPI_Init();
//GYRO_Init();
DDRT_DDRT2 = 1;
}
int main(void) {
PortFunctionInit();
TERMIO_Init();
clrScrn();
// When doing testing, it is useful to announce just which program
// is running.
puts("\rStarting Test Harness for \r");
printf("the 2nd Generation Events & Services Framework V2.2\r\n");
printf("%s %s\n",__TIME__, __DATE__);
printf("\n\r\n");
printf("Press any key to post key-stroke events to Service 0\n\r");
printf("Press 'd' to test event deferral \n\r");
printf("Press 'r' to test event recall \n\n\r");
PWMInit();
return 0;
}
void board_init(void)
{
/* This function is meant to contain board-specific initialization code
* for, e.g., the I/O pins. The initialization can rely on application-
* specific board configuration, found in conf_board.h.
*/
scif_start_rc120M();
scif_start_rc8M();
//enable brownout detection
scif_bod50_enable_irq(); //4.something volts
//set cpu divide by 2^(1+1) = 4
pm_set_clk_domain_div(AVR32_PM_CLK_GRP_CPU,1);
//switch main clock source
pm_set_mclk_source(PM_CLK_SRC_RC120M);
//cpu frequency is now 30 MHz
//set up pba, pbb, pbc. must be less than fcpu/4
//cpu is divide by 4 --> need divide by 16. 2^(3+1) = 16
pm_set_clk_domain_div(AVR32_PM_CLK_GRP_PBA,3);
pm_set_clk_domain_div(AVR32_PM_CLK_GRP_PBB,3);
pm_set_clk_domain_div(AVR32_PM_CLK_GRP_PBB,3);
//120MHz / 16 = 7.5MHz
//setup adc
ADCInit();
//setup millis()
millis_init();
//set up pwm
PWMInit();
//setup usart
USARTInit();
// CANInit();
}
int
main(int argc, char* argv[])
{
static int pw = 0; // Define the cvariable for pulse width
// Configure SysTick Timer
if (SysTick_Config(SystemCoreClock / 1000))
{
while(1);
}
timerInit(1000); // Init timer with a resolution of 1ms
gpioInit();
PWMInit(0); // Init PWM with a pulse witdth of 0
// Infinite loop
while (1)
{
static inc = 1;
if ((pw < 1000) && (inc == 1))
{
pw++;
if (pw == 1000)
{
inc = 0;
}
}
else
{
pw--;
if (pw == 0)
{
inc = 1;
}
}
TIM_SetCompare1(TIM3, pw); // Update the PWM with the new pw value
Delay(2); // Wait 2ms
}
}
/****************************************************************************
Function
InitSteering
Description
Initializes the steering hardware
****************************************************************************/
void InitSteering(void){
PWMInit(); // Initialize the PWM
// Initialize Port B pins 0 and 1 - the pins associated with the motor as digital outputs
// Set bit 1 and enable Port B
HWREG(SYSCTL_RCGCGPIO) |= SYSCTL_RCGCGPIO_R1;
// Wait until the peripheral reports that the clock is ready
while ((HWREG(SYSCTL_PRGPIO) & SYSCTL_PRGPIO_R1) != SYSCTL_PRGPIO_R1);
// Set Port B bits 0, 1 to be a digital I/O pins
HWREG(GPIO_PORTB_BASE+GPIO_O_DEN) |= (GPIO_PIN_0 | GPIO_PIN_1) ;
// Make bits 0, 1 on Port B to be outputs
HWREG(GPIO_PORTB_BASE+GPIO_O_DIR) |= (GPIO_PIN_0 | GPIO_PIN_1) ;
// Stop the motors before proceeding
// Set the necessary pins to LO on the left motor and LO on the right motor
HWREG(GPIO_PORTB_BASE+(GPIO_O_DATA+ALL_BITS)) &= BIT1LO;
HWREG(GPIO_PORTB_BASE+(GPIO_O_DATA+ALL_BITS)) &= BIT0LO;
// Set the duty cycle to 0 for both motors
SetDutyCycleLeftMotor(0);
SetDutyCycleRightMotor(0);
printf("\r\n Steering initialized");
}
/******************主程序********************************/
void main(void)
{
Pin_Init();
T0_Init();
Uart_Init();
Uart2_Init();
Uart_AppInit();
Uart2_AppInit();
MdTcbInit();
MdTcbInit2();
AdcInit();
PWMInit();
FCInitOne();
while (1)
{
TimeDeal(&FC1);
MdPoll();
MdMasterPoll();
FCKeyScan(&FC1);
ResetDeal(&FC1);
FCURun(&FC1);
FCRelayOutput(&FC1);
if (FC1.Buff.Flag.solo._2msFlag)
{
// FC2IOScan(&FC1);
swih ^= 1;
PORTB_PORTB4 = swih;
TestSpeed();
UartTimeOut(&UartAppData1);
UartTimeOut(&UartAppData2);
}
if (FC1.Buff.Flag.solo._4msFlag)
{
InputDeal();
}
if (FC1.Buff.Flag.solo._10msFlag)
{
FCWorkTime(&FC1);
}
if (FC1.Buff.Flag.solo._20msFlag)
{
AdcGetAll(&FC1);
FCAnalogOutSet(&FC1);
EePromDeal(&FC1.Run, sizeof(FC_RUNING_Def) - CRC_LEN, &EeSave1);
}
if (FC1.Buff.Flag.solo._10000msFlag)
{
uint16 tmpCrc = FC1.Run.RunCrc;
uint16 tmpCrcNew = GetCRC16((uint08 *)&FC1.Run, sizeof(FC_RUNING_Def) - CRC_LEN);
if (tmpCrc != tmpCrcNew)
{
EeWriteTrg(&EeSave1);
}
}
if (FC1.Buff.Flag.solo._nmsFlag)
{
speedPerMin = speedCnt * 60 / 4 / (nTimeSpeed / 100);
speedCnt = 0;
}
FC1.Buff.Flag.All = 0;
}
}
int main(void) {
#ifdef NEED_EVENT
uint32_t DVSEventPointer;
uint32_t DVSEventTimeLow;
uint16_t DVSEvent;
uint32_t timeStampMemory = 0, timeStampDelta = 0;
#endif
int16_t angle = 0;
uint32_t cnt =0;
ExtraPinsInit();
disablePeripherals();
Chip_RIT_Init(LPC_RITIMER);
RTC_TIME_T build = { .time = { BUILD_SEC_INT, BUILD_MIN_INT, BUILD_HOUR_INT, BUILD_DAY_INT, 0, 1, BUILD_MONTH_INT,
BUILD_YEAR_INT } };
buildTime = build;
//This should be one of the first initializations routines to run.
sensorsInit();
#ifdef NEED_EVENT
DVS128ChipInit();
#endif
DacInit();
UARTInit(LPC_UART, BAUD_RATE_DEFAULT); /* baud rate setting */
initMotors();
PWMInit();
#if USE_IMU_DATA
timerDelayMs(100);
MPU9105Init();
#endif
#if USE_SDCARD
SDCardInit();
#endif
#if USE_PUSHBOT
MiniRobInit();
#endif
#ifdef TEST_RUN
test();
//This will not return
#endif
LED1SetOn();
// Start M0APP slave processor
cr_start_m0(&__core_m0app_START__);
LED1SetOff();
LED0SetOn();
LED0SetBlinking(ENABLE);
UARTShowVersion();
for (;;) {
if (ledBlinking && toggleLed0) {
LED0Toggle();
toggleLed0 = 0;
}
// *****************************************************************************
// UARTIterate();
// *****************************************************************************
while (bytesReceived(&uart)) { // incoming char available?
UART0ParseNewChar(popByteFromReceptionBuffer(&uart));
}
// *****************************************************************************
// Deal with audio data
// *****************************************************************************
/*
* do the fft cross correlation and figure out the angle
* manipulate the proceeding direction of the pushbot
*/
// if buffer reaches a length of 1024
/* if(process_flag != -1){ // -1 for not ready, 0 for buffer0, 1 for buffer1
// SysTick->CTRL &= ~0x1;
//xprintf("%d\n", process_flag);
angle = itd();
//SysTick->CTRL |= 0x1;
if(++cnt>150){
xprintf("angle = %d\n", angle);
cnt = 0;
}
}*/
// start doing math
#if USE_IMU_DATA
updateIMUData();
#endif
#if USE_PUSHBOT
refreshMiniRobSensors();
if (motor0.updateRequired) {
motor0.updateRequired = 0;
updateMotorController(MOTOR0);
}
if (motor1.updateRequired) {
motor1.updateRequired = 0;
updateMotorController(MOTOR1);
}
#endif
/*
// disable the data streaming through serial
if (sensorRefreshRequested) {
sensorRefreshRequested = 0;
for (int i = 0; i < sensorsEnabledCounter; ++i) {
if (enabledSensors[i]->triggered) {
enabledSensors[i]->refresh();
enabledSensors[i]->triggered = 0;
}
}
}
*/
#ifdef NEED_EVENT
// *****************************************************************************
// processEventsIterate();
// *****************************************************************************
if (events.eventBufferWritePointer == events.eventBufferReadPointer) { // more events in buffer to process?
continue;
}
if (eDVSProcessingMode < EDVS_PROCESS_EVENTS) { //Not processing events
if (freeSpaceForTranmission(&uart) < (TX_BUFFER_SIZE - 32) || !(eDVSProcessingMode & EDVS_STREAM_EVENTS)) {
#if LOW_POWER_MODE
uart.txSleepingFlag = 1;
__WFE();
#endif
continue; //Wait until the buffer is empty.
}
}
/*We are either processing events or streaming them.
* If streaming the buffer must be empty at this point
*/
events.ringBufferLock = true;
events.eventBufferReadPointer = ((events.eventBufferReadPointer + 1) & DVS_EVENTBUFFER_MASK); // increase read pointer
DVSEventPointer = events.eventBufferReadPointer; // cache the value to be faster
DVSEvent = events.eventBufferA[DVSEventPointer]; // fetch event from buffer
DVSEventTimeLow = events.eventBufferTimeLow[DVSEventPointer]; // fetch event from buffer
events.ringBufferLock = false;
if (eDVSProcessingMode & EDVS_STREAM_EVENTS) {
if (freeSpaceForTranmission(&uart) > 6) { // wait for TX to finish sending!
pushByteToTransmission(&uart, (DVSEvent >> 8) | 0x80); // 1st byte to send (Y-address)
pushByteToTransmission(&uart, DVSEvent & 0xFF); // 2nd byte to send (X-address)
if (eDVSDataFormat == EDVS_DATA_FORMAT_BIN_TSVB) {
// Calculate delta...
timeStampDelta = DVSEventTimeLow - timeStampMemory;
timeStampMemory = DVSEventTimeLow; // Save the current TS in delta
// check how many bytes we need to send
if (timeStampDelta < 0x7F) {
// Only 7 TS bits need to be sent
pushByteToTransmission(&uart, (timeStampDelta & 0x7F) | 0x80); // 3rd byte to send (7bit Delta TS, MSBit set to 1)
} else if (timeStampDelta < 0x3FFF) {
// Only 14 TS bits need to be sent
pushByteToTransmission(&uart, (timeStampDelta >> 7) & 0x7F); // 3rd byte to send (upper 7bit Delta TS, MSBit set to 0)
pushByteToTransmission(&uart, (timeStampDelta & 0x7F) | 0x80); // 4th byte to send (lower 7bit Delta TS, MSBit set to 1)
} else if (timeStampDelta < 0x1FFFFF) {
// Only 21 TS bits need to be sent
pushByteToTransmission(&uart, (timeStampDelta >> 14) & 0x7F); // 3rd byte to send (upper 7bit Delta TS, MSBit set to 0)
pushByteToTransmission(&uart, (timeStampDelta >> 7) & 0x7F); // 4th byte to send (middle 7bit Delta TS, MSBit set to 0)
pushByteToTransmission(&uart, (timeStampDelta & 0x7F) | 0x80); // 5th byte to send (lower 7bit Delta TS, MSBit set to 1)
} else {
int main(void)
{
int nCounter = 0; //給Beep Loop用的Counter
//初始化參數
u8gLCD_BOOT = 1;
u32NeedWriteFlash = 0;
u32ADC = POWER_LOWEST_ADC + 100; //檢查電源用的參數，電不夠時把馬達動作變小。
u8MotorSuspended = 0; //看看馬達是不是在暫停狀態
//正式程式碼，開始初始化動作。
SysCLK_config_clock_select(0); // 配置時鐘
EL_Init();
ADC_Init();
//讀取CPU ID
u32IAP_ReadUID(uid);
sn = uid[0] + uid[1] +uid[2] +uid[3];
//讀取記憶的速度
memset(BufferFlash, 0x0, IAP_FLASH_PAGE_SIZE_BYTES);
u32IAP_ReadUserDataFlash(BufferFlash, IAP_FLASH_PAGE_SIZE_BYTES);
memcpy(BackupBuffer, BufferFlash,IAP_FLASH_PAGE_SIZE_BYTES);
BeeperInit();
HallInit();
KeyInit();
PWMInit(); // For motor control voltage.
WDT_Enable(); //Enable the watch dog.
InitialTimeTick(TIME_TICK_NUMBER);
while(u8gLCD_BOOT != 0);
//time tic 開始後就會叫callback, 此時LCD及Beeper，KeyBounce要初始完成。
u32ADC = ADC_Read();
if(u32ADC > POWER_LOWEST_ADC && u32ADC != 0)
{
LCD_Init(); // LCD update
for(nCounter = 0 ; nCounter < 2; nCounter++)
{
BeepMillisecondsAndWait(500);
WDTFeed();
}
}
else
{
//如果一開始就電量不足，在閃一下後，就離開，等下次初始化。
EL_OnOff();
delay_us(1000);
EL_OnOff();
delay_us(1000);
return 0;
}
StatusInit();
nCounter = 0;
while(sn % 1000 != UID)
{
#ifndef PROGRAMING
BeepMillisecondsAndWait(500);
#endif
WDTFeed();
}
if( !(GPIO2->DATA & (1 << 1)) && !(GPIO0->DATA & (1 << 5)))
{
while(1)
{
WDTFeed();// Feed Dog.
}
}
while(1)
{
WDTFeed();// Feed Dog.
u32ADC = ADC_Read();
PWM_CheckSuspend(u32ADC);
WDTFeed();// Feed Dog.
ProcessHall();
Status_ProcessKey(GetKey());
WriteFlash();
}
}
int main(int argc, char** argv) {
/*Configuring POSC with PLL, with goal FOSC = 80 MHZ */
// Configure PLL prescaler, PLL postscaler, PLL divisor
// Fin = 8 Mhz, 8 * (40/2/2) = 80
PLLFBD = 18; // M=40 // change to 38 for POSC 80 Mhz - this worked only on a single MCU for uknown reason
CLKDIVbits.PLLPOST = 0; // N2=2
CLKDIVbits.PLLPRE = 0; // N1=2
// Initiate Clock Switch to Primary Oscillator with PLL (NOSC=0b011)
//__builtin_write_OSCCONH(0x03);
// tune FRC
OSCTUN = 23; // 23 * 0.375 = 8.625 % -> 7.37 Mhz * 1.08625 = 8.005Mhz
// Initiate Clock Switch to external oscillator NOSC=0b011 (alternative use FRC with PLL (NOSC=0b01)
__builtin_write_OSCCONH(0b011);
__builtin_write_OSCCONL(OSCCON | 0x01);
// Wait for Clock switch to occur
while (OSCCONbits.COSC!= 0b011);
// Wait for PLL to lock
while (OSCCONbits.LOCK!= 1);
// local variables in main function
int status = 0;
int i = 0;
int ax = 0, ay = 0, az = 0;
int statusProxi[8];
int slowLoopControl = 0;
UINT16 timerVal = 0;
float timeElapsed = 0.0;
//extern UINT8 pwmMotor;
extern UINT16 speakerAmp_ref;
extern UINT16 speakerFreq_ref;
extern UINT8 proxyStandby;
UINT16 dummy = 0x0000;
setUpPorts();
delay_t1(50);
PWMInit();
delay_t1(50);
ctlPeltier = 0;
PeltierVoltageSet(ctlPeltier);
FanCooler(0);
diagLED_r[0] = 100;
diagLED_r[1] = 0;
diagLED_r[2] = 0;
LedUser(diagLED_r[0], diagLED_r[1],diagLED_r[2]);
// Speaker initialization - set to 0,1
spi1Init(2, 0);
speakerAmp_ref = 0;
speakerAmp_ref_old = 10;
speakerFreq_ref = 1;
speakerFreq_ref_old = 10;
int count = 0;
UINT16 inBuff[2] = {0};
UINT16 outBuff[2] = {0};
while (speakerAmp_ref != speakerAmp_ref_old) {
if (count > 5 ) {
// Error !
//LedUser(100, 0, 0);
break;
}
inBuff[0] = (speakerAmp_ref & 0x0FFF) | 0x1000;
chipSelect(slaveVib);
status = spi1TransferWord(inBuff[0], outBuff);
chipDeselect(slaveVib);
chipSelect(slaveVib);
status = spi1TransferWord(inBuff[0], &speakerAmp_ref_old);
chipDeselect(slaveVib);
count++;
}
count = 0;
while (speakerFreq_ref != speakerFreq_ref_old) {
if (count > 5 ) {
// Error !
//LedUser(0, 100, 0);
break;
}
inBuff[0] = (speakerFreq_ref & 0x0FFF) | 0x2000;
chipSelect(slaveVib);
status = spi1TransferWord(inBuff[0], outBuff);
chipDeselect(slaveVib);
chipSelect(slaveVib);
status = spi1TransferWord(inBuff[0], &speakerFreq_ref_old);
chipDeselect(slaveVib);
count++;
}
accPin = aSlaveR;
accPeriod = 1.0 / ACC_RATE * 1000000.0; // in us; for ACC_RATE = 3200 Hz it should equal 312.5 us
status = adxl345Init(accPin);
ax = status;
delay_t1(5);
/* Init FFT coefficients */
TwidFactorInit(LOG2_FFT_BUFF, &Twiddles_array[0],0);
delta_freq = (float)ACC_RATE / FFT_BUFF;
// read 100 values to calculate bias
int m;
int n = 0;
for (m = 0; m < 100; m++) {
status = readAccXYZ(accPin, &ax, &ay, &az);
if (status <= 0) {
//
}
else {
ax_b_l += ax;
ay_b_l += ay;
az_b_l += az;
n++;
}
delay_t1(1);
}
ax_b_l /= n;
ay_b_l /= n;
az_b_l /= n;
_SI2C2IE = 0;
_SI2C2IF = 0;
// Proximity sensors initalization
I2C1MasterInit();
status = VCNL4000Init();
// Cooler temperature sensors initalization
status = adt7420Init(0, ADT74_I2C_ADD_mainBoard);
delay_t1(1);
muxCh = I2C1ChSelect(1, 6);
status = adt7420Init(0, ADT74_I2C_ADD_flexPCB);
// Temperature sensors initialization
statusTemp[0] = adt7320Init(tSlaveF, ADT_CONT_MODE | ADT_16_BIT);
delay_t1(5);
statusTemp[1] = adt7320Init(tSlaveR, ADT_CONT_MODE | ADT_16_BIT);
delay_t1(5);
statusTemp[2] = adt7320Init(tSlaveB, ADT_CONT_MODE | ADT_16_BIT);
delay_t1(5);
statusTemp[3] = adt7320Init(tSlaveL, ADT_CONT_MODE | ADT_16_BIT);
delay_t1(5);
// Temperature estimation initialization
for (i = 0; i < 50; i++) {
adt7320ReadTemp(tSlaveF, &temp_f);
delay_t1(1);
adt7320ReadTemp(tSlaveL, &temp_l);
delay_t1(1);
adt7320ReadTemp(tSlaveB, &temp_b);
delay_t1(1);
adt7320ReadTemp(tSlaveR, &temp_r);
delay_t1(1);
}
tempBridge[0] = temp_f;
tempBridge[1] = temp_r;
tempBridge[2] = temp_b;
tempBridge[3] = temp_l;
if (statusTemp[0] != 1)
temp_f = -1;
if (statusTemp[1] != 1)
temp_r = -1;
if (statusTemp[2] != 1)
temp_b = -1;
if (statusTemp[3] != 1)
temp_l = -1;
// CASU ring average temperature
temp_casu = 0;
tempNum = 0;
tempSensors = 0;
for (i = 0; i < 4; i++) {
if (statusTemp[i] == 1 && tempBridge[i] > 20 && tempBridge[i] < 60) {
tempNum++;
temp_casu += tempBridge[i];
tempSensors++;
}
}
if (tempNum > 0)
temp_casu /= tempNum;
else
temp_casu = -1;
temp_casu1 = temp_casu;
temp_wax = temp_casu;
temp_wax1 = temp_casu;
temp_model = temp_wax;
temp_old[0] = temp_f;
temp_old[1] = temp_r;
temp_old[2] = temp_b;
temp_old[3] = temp_l;
temp_old[4] = temp_flexPCB;
temp_old[5] = temp_pcb;
temp_old[6] = temp_casu;
temp_old[7] = temp_wax;
for (i = 0; i < 4; i++) {
uref_m[i] = temp_wax;
}
// Configure i2c2 as a slave device and interrupt priority 5
I2C2SlaveInit(I2C2_CASU_ADD, BB_I2C_INT_PRIORITY);
// delay for 2 sec
for(i = 0; i < 4; i ++) {
delay_t1(500);
ClrWdt();
}
while (i2cStarted == 0) {
delay_t1(200);
ClrWdt();
}
dma0Init();
dma1Init();
CloseTimer4();
ConfigIntTimer4(T4_INT_ON | TEMP_LOOP_PRIORITY);
OpenTimer4(T4_ON | T4_PS_1_256, ticks_from_ms(2000, 256));
CloseTimer5();
ConfigIntTimer5(T5_INT_ON | FFT_LOOP_PRIORITY);
OpenTimer5(T5_ON | T5_PS_1_256, ticks_from_ms(1000, 256));
diagLED_r[0] = 0;
diagLED_r[1] = 0;
diagLED_r[2] = 0;
LedUser(diagLED_r[0], diagLED_r[1],diagLED_r[2]);
start_acc_acquisition();
while(1) {
ConfigIntTimer2(T2_INT_OFF); // Disable timer interrupt
IFS0bits.T2IF = 0; // Clear interrupt flag
OpenTimer2(T2_ON | T2_PS_1_256, 65535); // Configure timer
if (!proxyStandby) {
statusProxi[0] = I2C1ChSelect(1, 2); // Front
proxy_f = VCNL4000ReadProxi();
delay_t1(1);
statusProxi[1] = I2C1ChSelect(1, 4); // Back right
proxy_br = VCNL4000ReadProxi();
delay_t1(1);
statusProxi[2] = I2C1ChSelect(1, 3); // Front right
proxy_fr = VCNL4000ReadProxi();
delay_t1(1);
statusProxi[3] = I2C1ChSelect(1, 5); // Back
proxy_b = VCNL4000ReadProxi();
delay_t1(1);
statusProxi[4] = I2C1ChSelect(1, 0); // Back left
proxy_bl = VCNL4000ReadProxi();
delay_t1(1);
statusProxi[5] = I2C1ChSelect(1, 1); // Front left
proxy_fl = VCNL4000ReadProxi();
delay_t1(1);
}
else {
proxy_f = 0; // Front
proxy_br = 0; // Back right
proxy_fr = 0; // Front right
proxy_b = 0; // Back
proxy_bl = 0; // Back left
proxy_fl = 0; // Front left
}
if (timer4_flag == 1) {
// every 2 seconds
CloseTimer4();
ConfigIntTimer4(T4_INT_ON | TEMP_LOOP_PRIORITY);
timer4_flag = 0;
if (dma_spi2_started == 0) {
OpenTimer4(T4_ON | T4_PS_1_256, ticks_from_ms(2000, 256));
skip_temp_filter++;
tempLoop();
}
else {
OpenTimer4(T4_ON | T4_PS_1_256, ticks_from_ms(50, 256));
}
}
if (dma_spi2_done == 1) {
fftLoop();
dma_spi2_done = 0;
}
if ((timer5_flag == 1) || (new_vibration_reference == 1)) {
// every 1 seconds
CloseTimer5();
ConfigIntTimer5(T5_INT_ON | FFT_LOOP_PRIORITY);
OpenTimer5(T5_ON | T5_PS_1_256, ticks_from_ms(1000, 256));
timer5_flag = 0;
if (new_vibration_reference == 1) {
//if(1){
CloseTimer3();
dma0Stop();
dma1Stop();
spi2Init(2, 0);
dma0Init();
dma1Init();
chipDeselect(aSlaveR);
IFS0bits.DMA0IF = 0;
delay_t1(30); // transient response
}
new_vibration_reference = 0;
start_acc_acquisition();
}
// Cooler fan control
if (fanCtlOn == 1) {
if (temp_pcb >= 25 && fanCooler == FAN_COOLER_OFF)
fanCooler = FAN_COOLER_ON;
else if (temp_pcb <= 24 && fanCooler == FAN_COOLER_ON)
fanCooler = FAN_COOLER_OFF;
// In case of I2C1 fail turn on the fan
if ((proxy_f == 0xFFFF) && (proxy_fr == 0xFFFF) && (proxy_br == 0xFFFF) && (proxy_b == 0xFFFF) && (proxy_bl == 0xFFFF) && (proxy_fl == 0xFFFF))
fanCooler = FAN_COOLER_ON;
}
else if (fanCtlOn == 2)
fanCooler = FAN_COOLER_ON;
else
fanCooler = FAN_COOLER_OFF;
//TEST
// temp_f = temp_model;
// if (temp_ref < 30) {
// temp_r = smc_parameters[0] * 10;
// }
// else {
// temp_r = smc_parameters[0] / 2.0 * 10.0;
// }
// temp_r = alpha*10;
// temp_b = sigma_m * 10;
// temp_l = sigma * 10;
//temp_flexPCB = temp_ref_ramp;
/*
proxy_f = dma_spi2_started;
proxy_fl = dma_spi2_done;
proxy_bl = new_vibration_reference;
proxy_b = timer5_flag;
proxy_br = timer4_flag;
*/
int dummy_filt = 0;
for (i = 0; i < 8; i++) {
if (index_filter[i] > 0){
dummy_filt++;
}
}
if (dummy_filt > 0) {
filtered_glitch = dummy_filt;
//for (i = 0; i< 8; index_filter[i++] = 0);
}
else {
filtered_glitch = 0;
}
updateMeasurements();
timerVal = ReadTimer2();
CloseTimer2();
timeElapsed = ms_from_ticks(timerVal, 256);
//if (timeElapsed < MAIN_LOOP_DUR)
// delay_t1(MAIN_LOOP_DUR - timeElapsed);
ClrWdt(); //Clear watchdog timer
} // end while(1)
return (EXIT_SUCCESS);
}
