C++ Library for the PsiSwarm Robot - Version 0.8
Dependents: PsiSwarm_V8_Blank_CPP Autonomia_RndmWlk
Fork of PsiSwarmV7_CPP by
Diff: psiswarm.cpp
- Revision:
- 1:060690a934a9
- Parent:
- 0:d6269d17c8cf
- Child:
- 2:c6986ee3c7c5
--- a/psiswarm.cpp Thu Feb 04 21:48:54 2016 +0000 +++ b/psiswarm.cpp Thu Mar 03 23:21:47 2016 +0000 @@ -1,5 +1,5 @@ /* University of York Robotics Laboratory PsiSwarm Library: PsiSwarm Core Source File - * + * * File: psiswarm.cpp * * (C) Dept. Electronics & Computer Science, University of York @@ -10,7 +10,7 @@ * February 2016 * * - */ + */ #include "psiswarm.h" @@ -47,6 +47,21 @@ Ticker timer_ticker; float firmware_version; +float pcb_version; +float serial_number; + +char has_compass=0; +char has_side_ir=1; +char has_base_ir=1; +char has_base_colour_sensor=0; +char has_top_colour_sensor=0; +char has_wheel_encoders=0; +char has_audio_pic=0; +char has_ultrasonic_sensor=0; +char has_temperature_sensor=0; +char has_recharging_circuit=0; +char has_433_radio=0; + char robot_id; char previous_robot_id; @@ -69,7 +84,7 @@ char debug_mode = DEBUG_MODE; char debug_output = DEBUG_OUTPUT_STREAM; - + char firmware_bytes[21]; int base_colour_sensor_raw_values [4]; @@ -115,6 +130,8 @@ int base_ir_pulse_delay = DEFAULT_BASE_IR_PULSE_DELAY; + + /** * init() * @@ -133,57 +150,69 @@ debug("PsiSwarm Robot Library %1.2f\n\n",SOFTWARE_VERSION_CODE); debug("- Setting up serial interface\n"); debug("- Reading firmware: "); - if(read_firmware() == 1){ - debug("Version %3.2f\n",firmware_version); - }else debug("INVALID\n"); - - if(ENABLE_BASIC == 1){ - read_list_of_file_names(); - if(psi_basic_file_count == 0){ - debug("- No PsiBasic files found\n"); - } - else use_flash_basic = 1; + if(read_firmware() == 1) { + debug("Version %3.2f\n",firmware_version); + IF_get_hardware_description(); + } else { + debug("INVALID\n"); + debug("- WARNING: Check firmware to enable robot features"); + } + if(ENABLE_BASIC == 1) { + read_list_of_file_names(); + if(psi_basic_file_count == 0) { + debug("- No PsiBasic files found\n"); + } else use_flash_basic = 1; } debug("- Setting up PIC microcontroller\n"); // IF_check_pic_firmware(); debug("- Setting up LED drivers\n"); - IF_init_leds(); + IF_init_leds(); if(IF_setup_led_expansion_ic() != 0) { - debug("- WARNING: No I2C acknowledge for LED driver\n"); - system_warnings += 1; + debug("- WARNING: No I2C acknowledge for LED driver\n"); + system_warnings += 1; } debug("- Setting up motor drivers\n"); - IF_init_motors(); + IF_init_motors(); debug("- Setting up GPIO expansion\n"); reset_encoders(); IF_setup_gpio_expansion_ic(); - debug("- Setting up temperature sensor\n"); - IF_setup_temperature_sensor(); - debug("- Setting up base colour sensor\n"); - IF_check_base_colour_sensor(); - debug("- Setting up ultrasonic sensor\n"); - //enable_ultrasonic_ticker(); + if(has_temperature_sensor) { + debug("- Setting up temperature sensor\n"); + IF_setup_temperature_sensor(); + } + if(has_base_colour_sensor) { + debug("- Setting up base colour sensor\n"); + IF_check_base_colour_sensor(); + } + if(has_ultrasonic_sensor) { + debug("- Setting up ultrasonic sensor\n"); + //enable_ultrasonic_ticker(); + } + debug("- Robot ID: %d\n",robot_id); - char switchstate = IF_get_switch_state(); + char switchstate = IF_get_switch_state(); debug("- Switch State : %d\n",switchstate); debug("- Battery Voltage: %1.3fV\n",get_battery_voltage()); debug("- DC Voltage : %1.3fV\n",get_dc_voltage()); debug("- Current Draw : %1.3fA\n",get_current()); + if(has_temperature_sensor){ debug("- Temperature : %1.3fC\n",get_temperature()); + } char demo = 0; if(ENABLE_DEMO == 1 && switchstate > 0) demo=1; display.init_display(demo); event_handler.attach_us(&IF_handle_events, 1000); - if(demo > 0){ + if(demo > 0) { debug("- Demo mode button is pressed\n"); wait(1.0); demo = IF_get_switch_state(); if(demo > 0) demo_mode(); - display.init_display(0); + display.init_display(0); } } -void IF_update_minutes(){ +void IF_update_minutes() +{ uptime.reset(); timer_minute_count += 5; } @@ -192,27 +221,27 @@ { // This is the main 'operating system' thread that handles events from robot stimuli // By default it is run every 1ms and checks if there are events to handle - if(event > 0){ + if(event > 0) { // There are some events to handle. We don't handle all events in every loop to keep the system responsive, instead they are priorised. - if(encoder_event == 1){ + if(encoder_event == 1) { // The encoders have changed; update the encoder values IF_update_encoders(); - encoder_event = 0; - event--; + encoder_event = 0; + event--; } else { - if(switch_event == 1){ - IF_update_switch(); - switch_event = 0; - event--; + if(switch_event == 1) { + IF_update_switch(); + switch_event = 0; + event--; } - if(change_id_event == 1){ + if(change_id_event == 1) { // The user ID for the robot has been changed IF_update_user_id(); change_id_event = 0; event--; } } - } + } } void IF_update_encoders() @@ -246,34 +275,55 @@ void reset_encoders() { left_encoder = 0; - right_encoder = 0; + right_encoder = 0; } -void debug(const char* format, ...) +void debug(const char* format, ...) { - char buffer[256]; - if (debug_mode){ - va_list vl; - va_start(vl, format); - vsprintf(buffer,format,vl); - if(debug_output & 2) bt.printf("%s", buffer); - if(debug_output & 1) pc.printf("%s", buffer); - if(debug_output & 4) display.debug_page(buffer,strlen(buffer)); - va_end(vl); - } + char buffer[256]; + if (debug_mode) { + va_list vl; + va_start(vl, format); + vsprintf(buffer,format,vl); + if(debug_output & 2) bt.printf("%s", buffer); + if(debug_output & 1) pc.printf("%s", buffer); + if(debug_output & 4) display.debug_page(buffer,strlen(buffer)); + va_end(vl); + } } float get_uptime(void) { - return uptime.read() + (timer_minute_count * 60); + return uptime.read() + (timer_minute_count * 60); } -void pause_user_code(float period){ +void pause_user_code(float period) +{ user_code_restore_mode = user_code_running; user_code_running = 0; pause_usercode_timeout.attach(&IF_end_pause_user_code, period); } -void IF_end_pause_user_code(){ - user_code_running = user_code_restore_mode; +void IF_end_pause_user_code() +{ + user_code_running = user_code_restore_mode; +} + +void IF_get_hardware_description() +{ + debug("- Robot serial number %1.2f\n",serial_number); + debug("- PCB version %1.2f\n",pcb_version); + debug("- Hardware: "); + if(has_compass) debug("COMPASS, "); + if(has_side_ir) debug("SIDE IR, "); + if(has_base_ir) debug("BASE IR, "); + if(has_base_colour_sensor) debug("BASE COLOUR, "); + if(has_top_colour_sensor) debug("TOP COLOUR, "); + if(has_wheel_encoders) debug("WHEEL ENC., "); + if(has_audio_pic) debug("AUDIO, "); + if(has_ultrasonic_sensor) debug("ULTRASONIC, "); + if(has_temperature_sensor) debug("TEMPERATURE, "); + if(has_recharging_circuit) debug("RECHARGING, "); + if(has_433_radio) debug("433 RADIO."); + debug("\n"); } \ No newline at end of file