void EyeRover::calibrate(){
std::cout << "Starting calibration procedure, EyeRover must be static" << endl;
std::cout << "Procedure should take about a second" << endl;
m_calibrated = false;
std::vector<std::vector<float> > _gyros_vals(NUMBER_OF_STEPS_FOR_CALIBRATION);
std::vector<std::vector<float> > _accs_vals(NUMBER_OF_STEPS_FOR_CALIBRATION);
read_gyroscope();
read_accelerometer();
read_magnetometer();
usleep(500000); //Sleep for half a second
for (int i=0; i<NUMBER_OF_STEPS_FOR_CALIBRATION; ++i){
_gyros_vals[i] = read_gyroscope();
_accs_vals[i] = read_accelerometer();
}
m_gyro_avg = get_mean(_gyros_vals);
m_accelerometer_avg = get_mean(_accs_vals);
if (get_std(_gyros_vals, 0, m_gyro_avg[0])> 0.01){
m_calibrated = false;
std::cout << "Calibration failed." << endl;
}
else{
m_calibrated = true;
std::cout << "Calibration succeeded." << endl;
}
}
void hexbright::update() {
unsigned long now;
#if (DEBUG==DEBUG_LOOP)
unsigned long start_time=micros();
#endif
#ifdef STROBE
while (true) {
do {
now = micros();
} while (next_strobe > now && // not ready for strobe
continue_time > now); // not ready for update
if (next_strobe <= now) {
if (now - next_strobe <26) {
digitalWriteFast(DPIN_DRV_EN, HIGH);
delayMicroseconds(strobe_duration);
digitalWriteFast(DPIN_DRV_EN, LOW);
}
next_strobe += strobe_delay;
}
if(continue_time <= now) {
if(strobe_delay>update_delay && // we strobe less than once every 8333 microseconds
next_strobe-continue_time < 4000) // and the next strobe is within 4000 microseconds (may occur before we return)
continue;
else
break;
}
} // do nothing... (will short circuit once every 70 minutes (micros maxint))
#else
do {
now = micros();
} while (continue_time > now); // not ready for update
#endif
// if we're in debug mode, let us know if our loops are too large
#if (DEBUG!=DEBUG_OFF && DEBUG!=DEBUG_PRINT)
static int i=0;
#if (DEBUG==DEBUG_LOOP)
static unsigned long last_time = 0;
if(!i) {
Serial.print("Time used: ");
Serial.print(start_time-last_time);
Serial.println("/8333");
}
last_time = now;
#endif
if(now-continue_time>5000 && !i) {
// This may be caused by too much processing for our update_delay, or by too many print statements)
// If you're triggering this, your button and light will react more slowly, and some accelerometer
// data is being missed.
Serial.println("WARNING: code is too slow");
}
if (!i)
i=1000/update_delay; // display loop output every second
else
i--;
#endif
// power saving modes described here: http://www.atmel.com/Images/2545s.pdf
//run overheat protection, time display, track battery usage
#ifdef LED
// regardless of desired led state, turn it off so we can read the button
_led_off(RLED);
delayMicroseconds(50); // let the light stabilize...
read_button();
// turn on (or off) the leds, if appropriate
adjust_leds();
#ifdef PRINT_NUMBER
update_number();
#endif
#else
read_button();
#endif
read_thermal_sensor(); // takes about .2 ms to execute (fairly long, relative to the other steps)
read_charge_state();
read_avr_voltage();
#ifdef ACCELEROMETER
read_accelerometer();
find_down();
#endif
detect_overheating();
detect_low_battery();
apply_max_light_level();
// change light levels as requested
adjust_light();
// advance time at the same rate as values are changed in the accelerometer.
// advance continue_time here, so the first run through short-circuits,
// meaning we will read hardware immediately after power on.
continue_time = continue_time+(1000*update_delay);
}
void hexbright::update() {
// advance time at the same rate as values are changed in the accelerometer.
continue_time = continue_time+(1000*update_delay);
unsigned long now;
while (true) {
do {
now = micros();
} while (next_strobe > now && // not ready for strobe
continue_time > now); // not ready for update
if (next_strobe <= now) {
if (now - next_strobe <26) {
digitalWrite(DPIN_DRV_EN, HIGH);
delayMicroseconds(strobe_duration);
digitalWrite(DPIN_DRV_EN, LOW);
}
next_strobe += strobe_delay;
}
if(continue_time <= now) {
if(strobe_delay>update_delay && // we strobe less than once every 8333 microseconds
next_strobe-continue_time < 4000) // and the next strobe is within 4000 microseconds (may occur before we return)
continue;
else
break;
}
} // do nothing... (will short circuit once every 70 minutes (micros maxint))
// if we're in debug mode, let us know if our loops are too large
#if (DEBUG!=DEBUG_OFF)
static int i=0;
static float avg_loop_time = 0;
static float last_time = 0;
avg_loop_time = (avg_loop_time*29 + continue_time-last_time)/30;
#if (DEBUG==DEBUG_LOOP)
if(!i) {
Serial.print("Average loop time: ");
Serial.println(avg_loop_time/1000);
}
#endif
if(avg_loop_time/1000>update_delay+1 && !i) {
// This may be caused by too much processing for our update_delay, or by too many print statements (each one takes a few ms)
Serial.print("WARNING: loop time: ");
Serial.println(avg_loop_time/1000);
}
if (!i)
i=1000/update_delay; // display loop output every second
else
i--;
last_time = continue_time;
#endif
// power saving modes described here: http://www.atmel.com/Images/2545s.pdf
//run overheat protection, time display, track battery usage
#ifdef LED
// regardless of desired led state, turn it off so we can read the button
_led_off(RLED);
delayMicroseconds(50); // let the light stabilize...
read_button();
// turn on (or off) the leds, if appropriate
adjust_leds();
#ifdef PRINT_NUMBER
update_number();
#endif
#else
read_button();
#endif
read_thermal_sensor(); // takes about .2 ms to execute (fairly long, relative to the other steps)
#ifdef ACCELEROMETER
read_accelerometer();
find_down();
#endif
overheat_protection();
// change light levels as requested
adjust_light();
}