Diff: PsiSwarm/psiswarm.cpp

Revision:

0:8a5497a2e366

Child:

1:f6356cf1cefc

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PsiSwarm/psiswarm.cpp	Sat Oct 03 22:48:50 2015 +0000
@@ -0,0 +1,244 @@
+/* University of York Robotics Laboratory PsiSwarm Library: PsiSwarm Core Source File
+ * 
+ * File: psiswarm.cpp
+ *
+ * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York
+ * 
+ * PsiSwarm Library Version: 0.2
+ *
+ * September 2015
+ *
+ */ 
+
+#include "psiswarm.h"
+
+Display display;
+Serial pc(USBTX,USBRX);
+Serial bt(p13, p14);
+
+DigitalOut mbed_led1(LED1);
+DigitalOut mbed_led2(LED2);
+DigitalOut mbed_led3(LED3);
+DigitalOut mbed_led4(LED4);
+AnalogIn vin_current(p15);
+AnalogIn vin_battery(p16);
+AnalogIn vin_dc(p17);
+I2C primary_i2c (p9, p10);
+InterruptIn gpio_interrupt (p12);
+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);
+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 switch_byte;
+char previous_switch_byte;
+
+char debug_mode = DEBUG_MODE;
+char debug_output = DEBUG_OUTPUT_STREAM;
+ 
+char firmware_bytes[20];
+
+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;
+
+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;
+
+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 %d.%d\n",firmware_bytes[9],firmware_bytes[10]);   
+    }else debug("INVALID\n");
+    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--;
+}
+
+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