C++ Library for the PsiSwarm Robot - Version 0.8
Dependents: PsiSwarm_V8_Blank_CPP Autonomia_RndmWlk
Fork of PsiSwarmV7_CPP by
Diff: sensors.cpp
- Revision:
- 10:e58323951c08
- Parent:
- 6:b340a527add9
- Child:
- 11:312663037b8c
diff -r dde9e21030eb -r e58323951c08 sensors.cpp --- a/sensors.cpp Sun Oct 16 14:12:49 2016 +0000 +++ b/sensors.cpp Sun Oct 16 16:00:20 2016 +0000 @@ -29,17 +29,17 @@ // The ultrasonic sensor needs a start command to be sent: this is done by calling update_ultrasonic_measure(). // It can be set to automatically refresh at 10Hz by called enable_ultrasonic_ticker and disable with disabled_ultrasonic_ticker -void enable_ultrasonic_ticker() +void Sensors::enable_ultrasonic_ticker() { - ultrasonic_ticker.attach_us(update_ultrasonic_measure,100000); + ultrasonic_ticker.attach_us(this,&Sensors::update_ultrasonic_measure,100000); } -void disable_ultrasonic_ticker() +void Sensors::disable_ultrasonic_ticker() { ultrasonic_ticker.detach(); } -void update_ultrasonic_measure() +void Sensors::update_ultrasonic_measure() { if(waiting_for_ultrasonic == 0) { waiting_for_ultrasonic = 1; @@ -49,13 +49,13 @@ command[1] = 0x51; // Get result is centimeters primary_i2c.write(ULTRASONIC_ADDRESS, command, 2); // Send the command to start a ranging burst } - ultrasonic_timeout.attach_us(IF_read_ultrasonic_measure,70000); + ultrasonic_timeout.attach_us(this,&Sensors::IF_read_ultrasonic_measure,70000); } else { debug("WARNING: Ultrasonic sensor called too frequently.\n"); } } -void IF_read_ultrasonic_measure() +void Sensors::IF_read_ultrasonic_measure() { if(has_ultrasonic_sensor) { char command[1]; @@ -74,7 +74,7 @@ // Additional Sensing Functions ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -float get_temperature() +float Sensors::get_temperature() { if(has_temperature_sensor)return IF_read_from_temperature_sensor(); return 0; @@ -84,13 +84,13 @@ // Voltage Sensing Functions ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -float get_battery_voltage () +float Sensors::get_battery_voltage () { float raw_value = vin_battery.read(); return raw_value * 4.95; } -float get_current () +float Sensors::get_current () { // Device uses a INA211 current sense monitor measuring voltage drop across a 2mOhm resistor // Device gain = 500 @@ -98,7 +98,7 @@ return raw_value * 3.3; } -float get_dc_voltage () +float Sensors::get_dc_voltage () { float raw_value = vin_dc.read(); return raw_value * 6.6; @@ -112,7 +112,7 @@ // The value is converted to an approximate distance in millimetres, or 100.0 if no obstacle found. // NB This function is preserved for code-compatability and cases where only a single reading is needed // In many cases it is preferable to call store_reflected_ir_distances() to save all 8 values then read using get_reflected_ir_distance() -float read_reflected_ir_distance ( char index ) +float Sensors::read_reflected_ir_distance ( char index ) { // Sanity check if(index>7) return 0.0; @@ -164,7 +164,7 @@ // Returns the stored value of the reflected obstacle based on last call of either read_reflected_ir_distance or store_reflected_distances -float get_reflected_ir_distance ( char index ) +float Sensors::get_reflected_ir_distance ( char index ) { // Sanity check: check range of index and that values have been stored if (index>7 || ir_values_stored == 0) return 0.0; @@ -172,7 +172,7 @@ } // Returns the stored value of the non-illuminated sensor based on last call of store_background_raw_ir_values -unsigned short get_background_raw_ir_value ( char index ) +unsigned short Sensors::get_background_raw_ir_value ( char index ) { // Sanity check: check range of index and that values have been stored if (index>7 || ir_values_stored == 0) return 0.0; @@ -180,7 +180,7 @@ } // Returns the stored value of the illuminated sensor based on last call of store_illuminated_raw_ir_values -unsigned short get_illuminated_raw_ir_value ( char index ) +unsigned short Sensors::get_illuminated_raw_ir_value ( char index ) { // Sanity check: check range of index and that values have been stored if (index>7 || ir_values_stored == 0) return 0.0; @@ -188,7 +188,7 @@ } // Stores the reflected distances for all 8 IR sensors -void store_reflected_ir_distances ( void ) +void Sensors::store_reflected_ir_distances ( void ) { store_ir_values(); for(int i=0; i<8; i++) { @@ -196,15 +196,15 @@ } } -// Stores the rbackground and illuminated values for all 8 IR sensors -void store_ir_values ( void ) +// Stores the background and illuminated values for all 8 IR sensors +void Sensors::store_ir_values ( void ) { store_background_raw_ir_values(); store_illuminated_raw_ir_values(); } // Stores the raw ADC values for all 8 IR sensors without enabling IR emitters -void store_background_raw_ir_values ( void ) +void Sensors::store_background_raw_ir_values ( void ) { ir_values_stored = 1; for(int i=0; i<8; i++) { @@ -213,7 +213,7 @@ } // Stores the raw ADC values for all 8 IR sensors with a 500us emitter pulse -void store_illuminated_raw_ir_values ( void ) +void Sensors::store_illuminated_raw_ir_values ( void ) { //1. Enable the front IR emitters and store the values IF_set_IR_emitter_output(0, 1); @@ -235,7 +235,7 @@ } // Converts a background and illuminated value into a distance -float calculate_reflected_distance ( unsigned short background_value, unsigned short illuminated_value ) +float Sensors::calculate_reflected_distance ( unsigned short background_value, unsigned short illuminated_value ) { float approximate_distance = 4000 + background_value - illuminated_value; if(approximate_distance < 0) approximate_distance = 0; @@ -253,7 +253,7 @@ } // Returns the illuminated raw sensor value for the IR sensor defined by index (range 0-7); turns on the emitters for a 500us pulse -unsigned short read_illuminated_raw_ir_value ( char index ) +unsigned short Sensors::read_illuminated_raw_ir_value ( char index ) { //This function reads the IR strength when illuminated - used for PC system debugging purposes //1. Enable the relevant IR emitter by turning on its pulse output @@ -296,7 +296,7 @@ // Returns the stored value of the non-illuminated sensor based on last call of store_background_base_ir_values -unsigned short get_background_base_ir_value ( char index ) +unsigned short Sensors::get_background_base_ir_value ( char index ) { // Sanity check: check range of index and that values have been stored if (index>4 || base_ir_values_stored == 0) return 0.0; @@ -304,7 +304,7 @@ } // Returns the stored value of the illuminated sensor based on last call of store_illuminated_base_ir_values -unsigned short get_illuminated_base_ir_value ( char index ) +unsigned short Sensors::get_illuminated_base_ir_value ( char index ) { // Sanity check: check range of index and that values have been stored if (index>4 || base_ir_values_stored == 0) return 0.0; @@ -312,7 +312,7 @@ } // Stores the reflected distances for all 5 base IR sensors -void store_base_ir_values ( void ) +void Sensors::store_base_ir_values ( void ) { store_background_base_ir_values(); store_illuminated_base_ir_values(); @@ -322,7 +322,7 @@ } // Stores the raw ADC values for all 5 base IR sensors without enabling IR emitters -void store_background_base_ir_values ( void ) +void Sensors::store_background_base_ir_values ( void ) { base_ir_values_stored = 1; for(int i=0; i<5; i++) { @@ -331,7 +331,7 @@ } // Stores the raw ADC values for all 5 base IR sensors with a 500us emitter pulse -void store_illuminated_base_ir_values ( void ) +void Sensors::store_illuminated_base_ir_values ( void ) { //1. Enable the base IR emitters and store the values IF_set_IR_emitter_output(2, 1); @@ -344,7 +344,7 @@ } // Routine to store detected line position in a similar format to the used on 3Pi\m3Pi\PiSwarm -void store_line_position ( ) +void Sensors::store_line_position ( ) { // Store background and reflected base IR values store_base_ir_values(); @@ -404,7 +404,7 @@ } // Returns the subtraction of the background base IR value from the reflection based on last call of store_illuminated_base_ir_values -unsigned short calculate_base_ir_value ( char index ){ +unsigned short Sensors::calculate_base_ir_value ( char index ){ // If the index is not in the correct range or the base IR values have not been stored, return zero if (index>4 || base_ir_values_stored == 0) return 0.0; // If the reflection value is greater than the background value, return the subtraction @@ -417,7 +417,7 @@ } // Returns the subtraction of the background side IR value from the reflection based on last call of store_illuminated_base_ir_values -unsigned short calculate_side_ir_value ( char index ){ +unsigned short Sensors::calculate_side_ir_value ( char index ){ // If the index is not in the correct range or the base IR values have not been stored, return zero if (index>7 || ir_values_stored == 0) return 0.0; // If the reflection value is greater than the background value, return the subtraction @@ -429,7 +429,7 @@ } } -void calibrate_base_ir_sensors (void) +void Sensors::calibrate_base_ir_sensors (void) { short white_background[5]; short white_active[5]; @@ -535,7 +535,7 @@ } -int get_bearing_from_ir_array (unsigned short * ir_sensor_readings) +int Sensors::get_bearing_from_ir_array (unsigned short * ir_sensor_readings) { //out("Getting bearing from array: [%d][%d][%d][%d][%d][%d][%d][%d]\n",ir_sensor_readings[0],ir_sensor_readings[1],ir_sensor_readings[2],ir_sensor_readings[3],ir_sensor_readings[4],ir_sensor_readings[5],ir_sensor_readings[6],ir_sensor_readings[7]);