void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)
{
int num_links = getNumLinks();
extern bool gDisableDeactivation;
if (!can_sleep || gDisableDeactivation)
{
awake = true;
sleep_timer = 0;
return;
}
// motion is computed as omega^2 + v^2 + (sum of squares of joint velocities)
btScalar motion = 0;
for (int i = 0; i < 6 + num_links; ++i) {
motion += m_real_buf[i] * m_real_buf[i];
}
if (motion < SLEEP_EPSILON) {
sleep_timer += timestep;
if (sleep_timer > SLEEP_TIMEOUT) {
goToSleep();
}
} else {
sleep_timer = 0;
if (!awake)
wakeUp();
}
}
void loop(void)
{
for (;;) {
msNow = millis();
btn.read();
if (btn.wasReleased()) {
if (++mode > MAX_MODE) mode = 0;
}
else if (btn.pressedFor(LONG_PRESS)) {
setPinsInput();
while (!btn.wasReleased()) btn.read(); //wait for the button to be released
goToSleep();
}
else if (msNow - msWakeUp >= msRunTime) {
setPinsInput();
goToSleep();
}
runLEDs();
}
}
//this function sends temp sensor data and battery state to coordinator
//If in router mode the function will use a timer to send at the set interval
//if in end device mode this function will send data and go to sleep for specified time interval
bool WSNode::sendSensorData() {
bool xMit = true; //this stay true if transmit was good
if(!batShutdown| !batCheckOn) { //check to see if we are in battery shutdown, if so all the module will do is sleep
network.update(); //check to see if there is any network traffic that needs to be passed on, technically an end device does not need this
if(timer) { //If in router mode this used to track when it is time to transmit again
//create packet to transmit. First argument calculates temperature, second gets ADC reading from A2, third gives state of battery
payload_t payload = { getSTTS751Temp(), checkBatteryVolt()};
RF24NetworkHeader header(rXNode); //Create transmit header. This goes in transmit packet to help route it where it needs to go, in this case it is the coordinator
//send data onto network and make sure it gets there1
if (network.write(header,&payload,sizeof(payload))) {
digitalWrite(7,LOW); //transmit was successful so make sure status LED is off
}
else { //transmit failed, try again
if (!network.write(header,&payload,sizeof(payload))) {
PIND |= (1<<PIND7); //this toggles the status LED at pin seven to show transmit failed
xMit = false; //transmit failed so let the user know
}
else digitalWrite(7,LOW); //transmit was successful so make sure status LED is off
}
}
}
if(!mMode) { //if we are in end device mode get ready to go to sleep
timer = true; //wer are not using the timer so make sure variable is set to true
radio.powerDown(); //power down the nRF24L01 before we go to sleep
ADCSRA &= ~(1<<ADEN); //Turn off ADC before going to sleep (set ADEN bit to 0). this saves even more power
goToSleep(); //function for putting the Atmega328 to sleep
ADCSRA |= (1<<ADEN); //Turn the ADC back on
radio.powerUp();
}
else { //we are in router mode
timer = checkTimer(); //update timer and check to see if it is time to transmit
batShutdown = false; //this should never be true when in router mode
}
return xMit; //let user know if transmit was successful
}
int main(void) {
char str_buffer[16];
SystemCoreClockUpdate();
gpioInit();
interruptInit();
adcInit();
Chip_PMU_GetSleepFlags(LPC_PMU);
Chip_PMU_ClearSleepFlags(LPC_PMU, PMU_PCON_DPDFLAG);
Chip_SPI_Init(SPI_PORT);
SysTick_Config(Chip_Clock_GetSystemClockRate() / TICKRATE_HZ);
//StuckI2CHack();
delayms(10);
MoonLander_I2C_Init(SENSOR_I2C, MOONLANDER_I2C_100K);
delayms(100);
// Initialize sensors:
HTU21D_Init(&g_HTU21D, SENSOR_I2C);
delayms(10);
HMC5883L_Init(&g_HMC5883L, SENSOR_I2C);
delayms(10);
HMC5883L_SetRange(&g_HMC5883L, HMC5883L_RANGE_2_5);
eGFX_InitDriver();
C12832A_Init(&g_C12832A, SPI_PORT, LCD_A0_PIN, LCD_RST_PIN, LCD_SSEL);
delayms(10);
Plot_Init(&g_plot_temp, -10, 40, "Temp (C)", 0);
Plot_Init(&g_plot_rh, 0, 100, "RH", 0);
Plot_Init(&g_plot_mag, -400, 300, "uTesla", 1);
Plot_SetSpecialValue(&g_plot_mag, 9000, "OL");
Compass_Init(&g_compass);
g_left_display = DISPLAY_TEMP;
g_right_display = DISPLAY_COMPASS;
// Straight to sleep on boot:
g_go_to_sleep = 1;
// Or not:
//g_go_to_sleep = 0;
//wakeup();
while(1) {
fillScreen(0x0);
if (g_go_to_sleep) {
// Write the empty back buffer to the screen:
eGFX_Dump(&eGFX_BackBuffer, &g_C12832A);
g_ignore_switches = 1;
// Sleep!
goToSleep();
// Processor has been woken up, restart clocks, etc.:
wakeup();
delayms(SW_DEBOUNCE_MS);
g_ignore_switches = 0;
}
switch (g_left_display) {
case DISPLAY_TEMP:
Plot_Draw(&eGFX_BackBuffer, &g_plot_temp, PLOT_LEFT);
break;
case DISPLAY_RH:
Plot_Draw(&eGFX_BackBuffer, &g_plot_rh, PLOT_LEFT);
break;
case DISPLAY_MAG:
Plot_Draw(&eGFX_BackBuffer, &g_plot_mag, PLOT_LEFT);
break;
case DISPLAY_COMPASS:
Compass_Draw(&eGFX_BackBuffer, &g_compass, COMPASS_LEFT);
break;
case DISPLAY_RANGE:
eGFX_DrawString(&eGFX_BackBuffer, "Range", 24, 1, &FONT_3_5_1BPP);
sprintf(str_buffer, "%0.2f cm", getRangeCentimeters());
eGFX_DrawString(&eGFX_BackBuffer, str_buffer, 24, 13, &FONT_3_5_1BPP);
break;
default:
break;
}
switch (g_right_display) {
case DISPLAY_TEMP:
Plot_Draw(&eGFX_BackBuffer, &g_plot_temp, PLOT_RIGHT);
break;
case DISPLAY_RH:
Plot_Draw(&eGFX_BackBuffer, &g_plot_rh, PLOT_RIGHT);
break;
case DISPLAY_MAG:
Plot_Draw(&eGFX_BackBuffer, &g_plot_mag, PLOT_RIGHT);
break;
case DISPLAY_COMPASS:
Compass_Draw(&eGFX_BackBuffer, &g_compass, COMPASS_RIGHT);
break;
case DISPLAY_RANGE:
eGFX_DrawString(&eGFX_BackBuffer, "Range", 88, 1, &FONT_3_5_1BPP);
sprintf(str_buffer, "%0.2f cm", getRangeCentimeters());
eGFX_DrawString(&eGFX_BackBuffer, str_buffer, 88, 13, &FONT_3_5_1BPP);
break;
default:
break;
}
eGFX_Dump(&eGFX_BackBuffer, &g_C12832A);
}
return 0 ;
}
