void mainInit()
{
// not used pins as inputs with pull-ups
DDRB &= ~(1 << PB4);
PORTB |= (1 << PB4);
DDRB &= ~(1 << PB5);
PORTB |= (1 << PB5);
s_timerCounter = 0;
s_started = 0;
s_buttonPressed = 0;
s_buttonLock = 0;
s_speed = 40;
s_straightMode = CONTROLLER_LEFT_AND_RIGHT;
sei();
motorsInit();
wheelsInit();
controllerInitWithTimer(&mainTimerTick);
remoteInit();
sensorsInit();
ledInit();
buttonInit();
}
void hardwareInit(void)
{
PLLConfig();
SET_ADPCFG;
buttonsInit();
LED_SET_OUT;
LED_OFF;
sensorsInit();
sensorsOn();
readBatteryVoltage();
servoInit();
motorsInit();
//motorsOff();
profilerInit();
readButtons();
if( key_UP&&key_DN ) //if ICSP is connected
{
displayEnabled = TRUE;
serialInit();
//_TRISB10 = 0;
//_TRISB11 = 0;
}
CC2500_init();
systemTimerInit();
}
// Initialize wifi, sensors and push data
void ICACHE_FLASH_ATTR init_done(void)
{
// Init wifi IP configuration
configInitIP();
// Init sensors
sensorsInit();
if(configGet()->sensor_mode==MODE_NORMAL)
{
// Normal mode
pushSensorData();
} else {
// Configuration mode
statusLed(LED_BLINK);
}
}
void stabilizerInit(void)
{
if(isInit)
return;
sensorsInit();
stateEstimatorInit();
stateControllerInit();
powerDistributionInit();
#if defined(SITAW_ENABLED)
sitAwInit();
#endif
xTaskCreate(stabilizerTask, STABILIZER_TASK_NAME,
STABILIZER_TASK_STACKSIZE, NULL, STABILIZER_TASK_PRI, NULL);
isInit = true;
}
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 {
/* Prints "Hello World" and sleeps for three seconds */
void task1(void* pdata) {
int testmode=11;
INT8U err = OS_NO_ERR;
char data[256];
int *power = (int*) GO_BASE;
double frontL, frontR, right, left;
double distance;
int button;
*buttonChange = 0;
roverInit();
int *button1=(int *)DATA_OUTPUT_BASE;
//set up calls
roverInit();
sensorsInit();
while (1) {
//checks the swtich and whether to run a test progrma or not
if (*power == 1) {
if (testmode == 1) {
printf("Testmode\n");
//RANGEFINDERS 0 COMPASS 1 ROVER 2 ROVER1 3 DRIVE 4
test(COMPASS);
OSTimeDlyHMSM(0, 0, 2, 000);
} else { //gets all sensor data and post htem
printf("...");
getRangeData(&frontL, &frontR, &right, &left);
distance=getDistance();
// sprintf(data, "%lf %lf %lf %lf %lf %f", frontL, frontR, right, left,
//distance,compass);
sprintf(data, "%lf %lf %lf %lf %lf", frontL, frontR, right, left,
distance);
err = OSQPost(NewData, (void *) data);
OSTimeDlyHMSM(0, 0, 0, 300);
}
} else {
// int testButton = IORD(DATA_OUTPUT_BASE, 0x3);
stop();
button = *buttonChange;
printf("%d \n",button);
if (button == 1) {
//IOWR(DATA_OUTPUT_BASE, 0x3, 0x1);
*buttonChange = 0;
printData();
// OSTimeDlyHMSM(0, 0, 1, 100);
}
OSTimeDlyHMSM(0, 0, 1, 100);
}
}
}
int main(void) {
uint8_t payload[ADVLEN];
//Disable JTAG to allow for Standby
AONWUCJtagPowerOff();
//Force AUX on
powerEnableAuxForceOn();
powerEnableRFC();
powerEnableXtalInterface();
// Divide INF clk to save Idle mode power (increases interrupt latency)
powerDivideInfClkDS(PRCM_INFRCLKDIVDS_RATIO_DIV32);
initRTC();
powerEnablePeriph();
powerEnableGPIOClockRunMode();
/* Wait for domains to power on */
while((PRCMPowerDomainStatus(PRCM_DOMAIN_PERIPH) != PRCM_DOMAIN_POWER_ON));
sensorsInit();
ledInit();
//Config IOID4 for external interrupt on rising edge and wake up
//IOCPortConfigureSet(BOARD_IOID_KEY_RIGHT, IOC_PORT_GPIO, IOC_IOMODE_NORMAL | IOC_FALLING_EDGE | IOC_INT_ENABLE | IOC_IOPULL_UP | IOC_INPUT_ENABLE | IOC_WAKE_ON_LOW);
// Config reedSwitch as input
IOCPortConfigureSet(BOARD_IOID_DP0, IOC_PORT_GPIO, IOC_IOMODE_NORMAL | IOC_RISING_EDGE | IOC_INT_ENABLE | IOC_IOPULL_DOWN | IOC_INPUT_ENABLE | IOC_WAKE_ON_HIGH);
//Set device to wake MCU from standby on PIN 4 (BUTTON1)
HWREG(AON_EVENT_BASE + AON_EVENT_O_MCUWUSEL) = AON_EVENT_MCUWUSEL_WU0_EV_PAD; //Does not work with AON_EVENT_MCUWUSEL_WU0_EV_PAD4 --> WHY??
IntEnable(INT_EDGE_DETECT);
powerDisablePeriph();
//Disable clock for GPIO in CPU run mode
HWREGBITW(PRCM_BASE + PRCM_O_GPIOCLKGR, PRCM_GPIOCLKGR_CLK_EN_BITN) = 0;
// Load clock settings
HWREGBITW(PRCM_BASE + PRCM_O_CLKLOADCTL, PRCM_CLKLOADCTL_LOAD_BITN) = 1;
initInterrupts();
initRadio();
// baek: before while
powerEnablePeriph();
powerEnableGPIOClockRunMode();
/* Wait for domains to power on */
while((PRCMPowerDomainStatus(PRCM_DOMAIN_PERIPH) != PRCM_DOMAIN_POWER_ON)); // CRASH !!!!!!!!!!!!!!
sensorsInit();
ledInit();
// end baek:
// Turn off FLASH in idle mode
powerDisableFlashInIdle();
// Cache retention must be enabled in Idle if flash domain is turned off (to avoid cache corruption)
powerEnableCacheRetention();
//AUX - request to power down (takes no effect since force on is set)
powerEnableAUXPdReq();
powerDisableAuxRamRet();
//Clear payload buffer
memset(payload, 0, ADVLEN);
while(1) {
//if((g_count& 0x04)== 1){
rfBootDone = 0;
rfSetupDone = 0;
rfAdvertisingDone = 0;
select_bmp_280(); // activates I2C for bmp-sensor
enable_bmp_280(1); // works
//Wait until RF Core PD is ready before accessing radio
waitUntilRFCReady();
initRadioInts();
runRadio();
//Wait until AUX is ready before configuring oscillators
waitUntilAUXReady();
//Enable 24MHz XTAL
OSCHF_TurnOnXosc();
//IDLE until BOOT_DONE interrupt from RFCore is triggered
while( ! rfBootDone) {
powerDisableCPU();
PRCMDeepSleep();
}
//This code runs after BOOT_DONE interrupt has woken up the CPU again
// ->
//Request radio to keep on system bus
radioCmdBusRequest(true);
//Patch CM0 - no RFE patch needed for TX only
radioPatch();
//Start radio timer
radioCmdStartRAT();
//Enable Flash access while doing radio setup
powerEnableFlashInIdle();
//Switch to XTAL
while( !OSCHF_AttemptToSwitchToXosc())
{}
/* baek: before while
powerEnablePeriph();
powerEnableGPIOClockRunMode();
/* Wait for domains to power on */
/* while((PRCMPowerDomainStatus(PRCM_DOMAIN_PERIPH) != PRCM_DOMAIN_POWER_ON)); // CRASH !!!!!!!!!!!!!!
sensorsInit();
ledInit();
*/
/*****************************************************************************************/
// Read sensor values
uint32_t pressure = 0; // only 3 Bytes used
//uint32_t temp = 0;
select_bmp_280(); // activates I2C for bmp-sensor
enable_bmp_280(1); // works
do{
pressure = value_bmp_280(BMP_280_SENSOR_TYPE_PRESS); // read and converts in pascal (96'000 Pa)
//temp = value_bmp_280(BMP_280_SENSOR_TYPE_TEMP);
}while(pressure == 0x80000000);
if(pressure == 0x80000000){
CPUdelay(100);
pressure = value_bmp_280(BMP_280_SENSOR_TYPE_PRESS); // read and converts in pascal (96'000 Pa)
}
//Start Temp measurement
uint16_t temperature;
enable_tmp_007(1);
//Wait for, read and calc temperature
{
int count = 0;
do{
temperature = value_tmp_007(TMP_007_SENSOR_TYPE_AMBIENT);
//g_count++;
}while( ((temperature == 0x80000000) || (temperature == 0)) && (count <=5) );
count++;
count--;
}
enable_tmp_007(0);
char char_temp[2];
//start hum measurement
configure_hdc_1000();
start_hdc_1000();
// //Wait for, read and calc humidity
while(!read_data_hdc_1000());
int humidity = value_hdc_1000(HDC_1000_SENSOR_TYPE_HUMIDITY);
// char char_hum[5];
//END read sensor values
/*****************************************************************************************/
powerDisablePeriph();
// Disable clock for GPIO in CPU run mode
HWREGBITW(PRCM_BASE + PRCM_O_GPIOCLKGR, PRCM_GPIOCLKGR_CLK_EN_BITN) = 0;
// Load clock settings
HWREGBITW(PRCM_BASE + PRCM_O_CLKLOADCTL, PRCM_CLKLOADCTL_LOAD_BITN) = 1;
/*****************************************************************************************/
// Set payload and transmit
uint8_t p;
p = 0;
/*jedes 5.te mal senden*/
payload[p++] = ADVLEN-1; /* len */
payload[p++] = 0x03;
payload[p++] = 0xde;
payload[p++] = 0xba;
payload[p++] =(sequenceNumber >> 8); // laufnummer
payload[p++] = sequenceNumber;
// Speed
payload[p++] = g_diff >> 24; // higher seconds
payload[p++] = g_diff >> 16; // lower seconds
payload[p++] = g_diff >> 8; // higher subseconds
payload[p++] = g_diff; // lower subseconds
//pressure
payload[p++] = 0;
payload[p++] = 0; //(pressure >> 16);
payload[p++] = 0; //(pressure >> 8);
payload[p++] = 0; //pressure;
//temperature
payload[p++] = 0;
payload[p++] = 0; // char_temp[2];
payload[p++] = 0;//temperature >> 8; // char_temp[1];
payload[p++] = 0; //temperature; //char_temp[0];
// huminity
payload[p++] = 0;
payload[p++] = 0;//char_hum[0];
payload[p++] = 0;//char_hum[1];
payload[p++] = 0;//char_hum[2];
payload[p++] = 0;
payload[p++] = 0;
//Start radio setup and linked advertisment
radioUpdateAdvData(ADVLEN, payload);
//Start radio setup and linked advertisment
radioSetupAndTransmit();
//}
//END: Transmit
/*****************************************************************************************/
//Wait in IDLE for CMD_DONE interrupt after radio setup. ISR will disable radio interrupts
while( ! rfSetupDone) {
powerDisableCPU();
PRCMDeepSleep();
}
//Disable flash in IDLE after CMD_RADIO_SETUP is done (radio setup reads FCFG trim values)
powerDisableFlashInIdle();
//Wait in IDLE for LAST_CMD_DONE after 3 adv packets
while( ! rfAdvertisingDone) {
powerDisableCPU();
PRCMDeepSleep();
}
//Request radio to not force on system bus any more
radioCmdBusRequest(false);
// } // end if
// g_count++;
//
// Standby procedure
//
powerDisableXtal();
// Turn off radio
powerDisableRFC();
// Switch to RCOSC_HF
OSCHfSourceSwitch();
// Allow AUX to turn off again. No longer need oscillator interface
powerDisableAuxForceOn();
// Goto Standby. MCU will now request to be powered down on DeepSleep
powerEnableMcuPdReq();
// Disable cache and retention
powerDisableCache();
powerDisableCacheRetention();
//Calculate next recharge
SysCtrlSetRechargeBeforePowerDown(XOSC_IN_HIGH_POWER_MODE);
// Synchronize transactions to AON domain to ensure AUX has turned off
SysCtrlAonSync();
//
// Enter Standby
//
powerDisableCPU();
PRCMDeepSleep();
SysCtrlAonUpdate();
SysCtrlAdjustRechargeAfterPowerDown();
SysCtrlAonSync();
//
// Wakeup from RTC, code starts execution from here
//
powerEnableRFC();
powerEnableAuxForceOn();
//Re-enable cache and retention
powerEnableCache();
powerEnableCacheRetention();
//MCU will not request to be powered down on DeepSleep -> System goes only to IDLE
powerDisableMcuPdReq();
}
}
