C++ Library for the PsiSwarm Robot - Version 0.7
Fork of PsiSwarmV7 by
Diff: psiswarm.cpp
- Revision:
- 0:d6269d17c8cf
- Child:
- 1:060690a934a9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/psiswarm.cpp Thu Feb 04 21:48:54 2016 +0000 @@ -0,0 +1,279 @@ +/* University of York Robotics Laboratory PsiSwarm Library: PsiSwarm Core Source File + * + * File: psiswarm.cpp + * + * (C) Dept. Electronics & Computer Science, University of York + * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis + * + * PsiSwarm Library Version: 0.4 + * + * February 2016 + * + * + */ + +#include "psiswarm.h" + +//Setup MBED connections to PsiSwarm Robot +Serial pc(USBTX,USBRX); +I2C primary_i2c (p9, p10); +InterruptIn gpio_interrupt (p12); +Serial bt(p13, p14); +AnalogIn vin_current(p15); +AnalogIn vin_battery(p16); +AnalogIn vin_dc(p17); +PwmOut motor_left_f (p21); +PwmOut motor_left_r (p22); +PwmOut motor_right_f(p23); +PwmOut motor_right_r(p24); +PwmOut center_led_red(p25); +PwmOut center_led_green(p26); +Display display; //Connects to i2c(p28,p27), reset(p29), backlight(p30) +DigitalOut mbed_led1(LED1); +DigitalOut mbed_led2(LED2); +DigitalOut mbed_led3(LED3); +DigitalOut mbed_led4(LED4); + + +float center_led_brightness; +float backlight_brightness; + +Ticker event_handler; +Timer uptime; +Timeout pause_usercode_timeout; +Ticker ultrasonic_ticker; +Timeout ultrasonic_timeout; +int timer_minute_count; +Ticker timer_ticker; + +float firmware_version; +char robot_id; +char previous_robot_id; + +char wheel_encoder_byte; +char previous_wheel_encoder_byte; +signed int left_encoder; +signed int right_encoder; + +char time_based_motor_action = 0; + +char testing_voltage_regulators_flag = 1; +char power_good_motor_left = 2; +char power_good_motor_right = 2; +char power_good_infrared = 2; +char status_dc_in = 2; +char status_charging = 2; + +char switch_byte; +char previous_switch_byte; + +char debug_mode = DEBUG_MODE; +char debug_output = DEBUG_OUTPUT_STREAM; + +char firmware_bytes[21]; + +int base_colour_sensor_raw_values [4]; +int top_colour_sensor_raw_values [4]; + +char waiting_for_ultrasonic = 0; +int ultrasonic_distance = 0; +char ultrasonic_distance_updated = 0; + + +float line_position = 0; +char line_found = 0; + +unsigned short background_ir_values [8]; +unsigned short illuminated_ir_values [8]; +float reflected_ir_distances [8]; +char ir_values_stored = 0; +unsigned short background_base_ir_values [5]; +unsigned short illuminated_base_ir_values [5]; +char base_ir_values_stored = 0; + +float motor_left_speed; +float motor_right_speed; +char motor_left_brake; +char motor_right_brake; + +char demo_on = 0; +char event = 0; +char change_id_event = 0; +char encoder_event = 0; +char switch_event = 0; +char user_code_running = 0; +char user_code_restore_mode = 0; +char system_warnings = 0; + + +vector<string> basic_filenames; //filenames are stored in a vector string +char psi_basic_file_count = 0; +char use_flash_basic = 0; +char file_transfer_mode = 0; + +int ir_pulse_delay = DEFAULT_IR_PULSE_DELAY; +int base_ir_pulse_delay = DEFAULT_BASE_IR_PULSE_DELAY; + + +/** + * init() + * + * Main initialisation routine for the PsiSwarm robot + * + * Set up the GPIO expansion ICs, launch demo mode if button is held + */ +void init() +{ + firmware_version=0; + timer_minute_count = 0; + timer_ticker.attach(&IF_update_minutes, 300); + uptime.start(); + primary_i2c.frequency(400000); + IF_setup_serial_interfaces(); + 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; + } + debug("- Setting up PIC microcontroller\n"); + // IF_check_pic_firmware(); + debug("- Setting up LED drivers\n"); + IF_init_leds(); + if(IF_setup_led_expansion_ic() != 0) { + debug("- WARNING: No I2C acknowledge for LED driver\n"); + system_warnings += 1; + } + debug("- Setting up motor drivers\n"); + 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(); + debug("- Robot ID: %d\n",robot_id); + 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()); + 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){ + debug("- Demo mode button is pressed\n"); + wait(1.0); + demo = IF_get_switch_state(); + if(demo > 0) demo_mode(); + display.init_display(0); + } +} + +void IF_update_minutes(){ + uptime.reset(); + timer_minute_count += 5; +} + +void IF_handle_events() +{ + // 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){ + // 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){ + // The encoders have changed; update the encoder values + IF_update_encoders(); + encoder_event = 0; + event--; + } else { + if(switch_event == 1){ + IF_update_switch(); + switch_event = 0; + event--; + } + 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() +{ + char rwep = previous_wheel_encoder_byte >> 2; + char rwe = wheel_encoder_byte >> 2; + char lwep = previous_wheel_encoder_byte % 4; + char lwe = wheel_encoder_byte % 4; + //pc.printf("L:%d P:%d R:%d P:%d \n",lwe,lwep,rwe,rwep); + if(lwe == 0 && lwep==1) left_encoder++; + if(lwe == 0 && lwep==2) left_encoder--; + if(rwe == 0 && rwep==1) right_encoder++; + if(rwe == 0 && rwep==2) right_encoder--; + if(left_encoder % 100 == 0) pc.printf("L:%d\n",left_encoder); +} + +void IF_update_user_id() +{ +} + +void IF_update_switch() +{ + // The user switch has changed state + // In this implementation we will only act on positive changes (rising edges) + // Subtracting new_state from (new_state & old_state) gives the positive changes + char positive_change = switch_byte - (switch_byte & previous_switch_byte); + if(demo_on) demo_handle_switch_event(positive_change); + else handle_switch_event(positive_change); +} + +void reset_encoders() +{ + left_encoder = 0; + right_encoder = 0; +} + +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); + } +} + +float get_uptime(void) +{ + return uptime.read() + (timer_minute_count * 60); +} + +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; +} \ No newline at end of file