Diff: PsiSwarm/i2c.cpp

Revision:

0:8a5497a2e366

Child:

5:598298aa4900

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PsiSwarm/i2c.cpp	Sat Oct 03 22:48:50 2015 +0000
@@ -0,0 +1,355 @@
+/* University of York Robotics Laboratory PsiSwarm Library: I2C Source File
+ *
+ * File: i2c.cpp
+ *
+ * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York
+ *
+ * PsiSwarm Library Version: 0.2
+ *
+ * September 2015
+ *
+ */
+
+#include "psiswarm.h"
+
+char gpio_byte0;
+char gpio_byte1;
+char user_id_set = 0;
+char wheel_enc_set = 0;
+char switch_set = 0;
+
+char emitter_byte = 0x00;
+
+Timeout update_timeout;
+
+DigitalOut test_led(LED2);
+
+char test;
+
+void IF_set_IR_emitter_output(char emitter, char state)
+{
+    if(emitter <3) {
+        if(state == 0) {
+            char shift = 1 << emitter;
+            emitter_byte &= (0xFF - shift);
+        }
+        if(state == 1) {
+            char shift = 1 << emitter;
+            emitter_byte |= shift;
+        }
+        char data[2];
+        data [0] = 0x0A;  //Write to OLAT register
+        data [1] = emitter_byte;  //GP0-3 are outputs on aux expansion IC
+        //pc.printf("%c\n", emitter_byte);
+        primary_i2c.write(AUX_IC_ADDRESS,data,2,false);
+    }
+}
+
+void IF_set_base_LED(char state)
+{
+    if(state == 0) {
+        emitter_byte &= 0xF7;
+    } else emitter_byte |= 0x08;
+    char data[2];
+    data [0] = 0x0A;  //Write to OLAT register
+    data [1] = emitter_byte;  //GP0-3 are outputs on aux expansion IC
+    primary_i2c.write(AUX_IC_ADDRESS,data,2,false);
+
+}
+
+unsigned short IF_read_IR_adc_value(char adc, char index)
+{
+    char address = ADC1_ADDRESS;
+    if(adc == 2) address=ADC2_ADDRESS;
+    // Returns the raw sensor value for the IR sensor defined by index (range 0-7).
+    short value = 0;
+    // Read a single value from the ADC
+    if(index<8) {
+        char apb[1];
+        char data[2];
+        switch(index) {
+            case 0:
+                apb[0]=0x80;
+                break;
+            case 1:
+                apb[0]=0x90;
+                break;
+            case 2:
+                apb[0]=0xA0;
+                break;
+            case 3:
+                apb[0]=0xB0;
+                break;
+            case 4:
+                apb[0]=0xC0;
+                break;
+            case 5:
+                apb[0]=0xD0;
+                break;
+            case 6:
+                apb[0]=0xE0;
+                break;
+            case 7:
+                apb[0]=0xF0;
+                break;
+        }
+        primary_i2c.write(address,apb,1,false);
+        primary_i2c.read(address,data,2,false);
+        value=((data[0] % 16)<<8)+data[1];
+        if(value > 4096) value=4096;
+        value=4096-value;
+    }
+    return value;
+}
+
+char IF_setup_led_expansion_ic(void)
+{
+    //LED expansion IC is PCA9555
+    //Address is 0100 001x (0x42) {defined by LED_IC_ADDRESS}
+    //All 16 entries are outputs as they drive LEDs; the relevant registers are 2&3 (output port registers) and 6&7 (config. registers: a 0=output)
+    //Message structure: {Address-RW}{Command}{Port 0}{Port 1}
+    //Command bytes: 00000010 (0x02) = Write to output port
+    //Command bytes: 00000110 (0x06) = Write to config registers
+    //Note that for the LEDs, 0 = on, 1 = off
+    //Port 0 = LED 1:4 Red:Green
+    //Port 1 = LED 5:8 Red:Green
+    char data [3];
+    data [0] = 0x06;    //Write config registers
+    data [1] = 0x00;    //All 8 pins in port 0 are outputs (0)
+    data [2] = 0x00;    //All 8 pins in port 1 are outputs (0)
+    primary_i2c.write(LED_IC_ADDRESS,data,3,false);
+
+    //Turn all LEDs on
+    data [0] = 0x02;    //Write to output port
+    data [1] = 0x00;    //Enable LED1-4 (both colours)
+    data [2] = 0x00;    //Enable LED5-8 (both colours)
+    primary_i2c.write(LED_IC_ADDRESS,data,3,false);
+
+    wait(0.05);
+    //Turn all LEDs off
+    data [0] = 0x02;    //Write to output port
+    data [1] = 0xFF;    //Enable LED1-4 (both colours)
+    data [2] = 0xFF;    //Enable LED5-8 (both colours)
+    return primary_i2c.write(LED_IC_ADDRESS,data,3,false);
+}
+
+//Returns 0 if successful, 1 if test mode button pressed
+void IF_setup_gpio_expansion_ic(void)
+{
+    //Main GPIO expansion IC is PCA9555
+    //Address is 0100 000x (0x40) {defined by GPIO_IC_ADDRESS}
+    //All 16 entries are inputs; the relevant registers are 0&1 (input port registers), 4&5 (polarity inv. registers) and 6&7 (config. registers: a 0=output)
+    //Message structure: {Address-RW}{Command}{Port 0}{Port 1}
+    //Command bytes: 00000010 (0x02) = Write to output port
+    //Command bytes: 00000110 (0x06) = Write to config registers
+    //Note that for the LEDs, 0 = on, 1 = off
+    //Port 0 = PGDL; PGDR; PGDIR; UP; DOWN; LEFT; RIGHT; CENTER
+    //Port 1 = ENC_LA; ENC_LB; ENC_RA; ENC_RB; ID0; ID1; ID2; ID3
+    char data [3];
+    data [0] = 0x06;    //Write config registers
+    data [1] = 0xFF;    //All 8 pins in port 0 are inputs (1)
+    data [2] = 0xFF;    //All 8 pins in port 1 are inputs (1)
+    if(primary_i2c.write(GPIO_IC_ADDRESS,data,3,false) != 0) {
+        system_warnings += 2;
+        debug("- WARNING: No I2C acknowledge for main GPIO IC\n");
+    }
+    //Set all inputs to polarity-inverted (so a logic low = 1)
+    data [0] = 0x04;    //Write to polarity inversion ports
+    data [1] = 0xFF;    //Invert polarity of all bits in input port 0
+    data [2] = 0xFF;    //Invert polarity of all bits in input port 1
+    primary_i2c.write(GPIO_IC_ADDRESS,data,3,false);
+
+    wait(0.01);
+
+    //Read data
+    char read_data[2];
+    char command[1]; //Command to read from input port 0
+    command[0]=0;
+    primary_i2c.write(GPIO_IC_ADDRESS,command,1,false);
+    primary_i2c.read(GPIO_IC_ADDRESS,read_data,2,false);
+    gpio_byte0 = read_data[0];
+    //char ret_val = (gpio_byte0 & 0xF8) >> 3;  //Returns a >0 value if a button is being pushed
+    gpio_byte1 = read_data[1];
+    IF_parse_gpio_byte0(gpio_byte0);
+    IF_parse_gpio_byte1(gpio_byte1);
+    //Setup interrupt handler for GPIO interrupts
+    gpio_interrupt.mode(PullUp);
+    gpio_interrupt.rise(&IF_handle_gpio_interrupt);
+    //pc.printf("%c %c",gpio_byte0,gpio_byte1);
+
+    //Secondary GPIO expansion IC is MCP23009
+    //Address is 0100 111 (0x4E) {defined by AUX_IC_ADDRESS}
+    //GP0,1,2,3 are outputs for driving infrared emitters and the base LED
+    //IODIR register wants to be 0xF0 (1=input, 0=output)
+    data [0] = 0x00;  //Write to IODIR register
+    data [1] = 0xF0;  //Set GP0-3 as outputs
+    primary_i2c.write(AUX_IC_ADDRESS,data,2,false);
+
+    if(primary_i2c.write(AUX_IC_ADDRESS,data,2,false) != 0) {
+        system_warnings += 4;
+        debug("- WARNING: No I2C acknowledge for aux GPIO IC\n");
+    }
+    data [0] = 0x06;  //Write to GPPU register
+    data [1] = 0x0F;  //Set GP0-3 as active pull-up outputs
+    primary_i2c.write(AUX_IC_ADDRESS,data,2,false);
+
+    //My interrupt is not so reliable: poll with a 50ms timeout in case interrupts aren't handled
+    update_timeout.attach_us(&IF_update_gpio_inputs,50000);
+    //return ret_val;
+}
+
+void IF_parse_gpio_byte0(char byte)
+{
+    gpio_byte0 = byte;
+    //GPIO byte zero contains the power line traces and the switch states
+    char current_switch = ((gpio_byte0 & 0xF8) >> 3);
+    if(switch_set == 1) {
+        if(current_switch != switch_byte) {
+            previous_switch_byte = switch_byte;
+            switch_byte = current_switch;
+            event++;
+            switch_event = 1;
+        }
+    } else {
+        switch_byte = current_switch;
+        switch_set = 1;
+    }
+}
+
+void IF_parse_gpio_byte1(char byte)
+{
+    gpio_byte1 = byte;
+    //GPIO byte one contains the wheel encoders and the ID switch
+    char current_id = ((gpio_byte1 & 0xF0)>> 4);
+    if(user_id_set == 1) {
+        if(robot_id != current_id) {
+            previous_robot_id = robot_id;
+            robot_id = current_id;
+            event++;
+            change_id_event = 1;
+        }
+    } else {
+        robot_id = current_id;
+        user_id_set = 1;
+    }
+    char current_encoder = (gpio_byte1 & 0x0F);
+    if(wheel_enc_set == 1) {
+        if(wheel_encoder_byte != current_encoder) {
+            previous_wheel_encoder_byte = wheel_encoder_byte;
+            wheel_encoder_byte = current_encoder;
+            event++;
+            encoder_event = 1;
+        }
+    } else {
+        wheel_encoder_byte = current_encoder;
+        wheel_enc_set = 1;
+    }
+}
+
+void IF_handle_gpio_interrupt()
+{
+    test = 1-test;
+    test_led = test;
+    IF_update_gpio_inputs();
+}
+
+char IF_is_switch_pressed()
+{
+    //Read data
+    char data[1];
+    char command[1] = {0}; //Command to read from input port 0
+    primary_i2c.write(GPIO_IC_ADDRESS,command,1,false);
+    primary_i2c.read(GPIO_IC_ADDRESS,data,1,false);
+    return (data[0] & 0x80);  //Returns a 1 if the center button is being pushed
+}
+
+
+char IF_get_switch_state()
+{
+    //Read data
+    char data[1];
+    char command[1] = {0}; //Command to read from input port 0
+    primary_i2c.write(GPIO_IC_ADDRESS,command,1,false);
+    primary_i2c.read(GPIO_IC_ADDRESS,data,1,false);
+    return (data[0] & 0xF8) >> 3;  //Returns the current switch state
+}
+
+void IF_update_gpio_inputs()
+{
+    update_timeout.detach();
+    //Read data
+    char data[2];
+    char command[1] = {0}; //Command to read from input port 0
+    primary_i2c.write(GPIO_IC_ADDRESS,command,1,false);
+    primary_i2c.read(GPIO_IC_ADDRESS,data,2,false);
+    if(data[0]!=gpio_byte0) {
+        IF_parse_gpio_byte0(data[0]);
+    }
+    if(data[1]!=gpio_byte1) {
+        IF_parse_gpio_byte1(data[1]);
+    }
+    update_timeout.attach_us(&IF_update_gpio_inputs,50000);
+}
+
+
+void IF_write_to_led_ic(char byte_0, char byte_1)
+{
+    //Set LEDs
+    char data[3];
+    data [0] = 0x02;    //Write to output port
+    data [1] = byte_0;
+    data [2] = byte_1;
+    primary_i2c.write(LED_IC_ADDRESS,data,3,false);
+}
+
+
+void IF_setup_temperature_sensor()
+{
+    char data[3];
+    data[0] = 0x04; //Set critical temp limit
+    data[1] = TEMPERATURE_CRITICAL_HI;
+    data[2] = TEMPEARTURE_CRITICAL_LO;
+    primary_i2c.write(TEMPERATURE_ADDRESS,data,3,false);
+    data[0] = 0x02; //Set high temp limit
+    data[1] = TEMPERATURE_HIGH_HI;
+    data[2] = TEMPEARTURE_HIGH_LO;
+    primary_i2c.write(TEMPERATURE_ADDRESS,data,3,false);
+    data[0] = 0x03; //Set low temp limit
+    data[1] = TEMPERATURE_LOW_HI;
+    data[2] = TEMPEARTURE_LOW_LO;
+    primary_i2c.write(TEMPERATURE_ADDRESS,data,3,false);
+}
+
+float IF_read_from_temperature_sensor()
+{
+    char command[1] = {0x05};  //Write to Ta Register
+    char data[3];
+    signed int temp;
+    float temperature;
+    primary_i2c.write(TEMPERATURE_ADDRESS,command,1,false);
+    primary_i2c.read(TEMPERATURE_ADDRESS,data,2,false);
+
+    //Convert the temperature data
+    //First Check flag bits
+    char UpperByte = data[0];
+    char LowerByte = data[1];
+    if ((UpperByte & 0x80) == 0x80) {
+        debug("- WARNING: Temperature sensor reports critical temperature\n");
+    }
+    if ((UpperByte & 0x40) == 0x40) {
+        debug("- WARNING: Temperature sensor reports above upper limit\n");
+    }
+    if ((UpperByte & 0x20) == 0x20) {
+        debug("- WARNING: Temperature sensor reports below lower limit\n");
+    }
+    UpperByte = UpperByte & 0x1F;       //Clear flag bits
+    if ((UpperByte & 0x10) == 0x10) {
+        UpperByte = UpperByte & 0x0F;   //Clear SIGN
+        temp = (UpperByte * 256) + LowerByte;
+        temperature = - (temp / 16.0f);
+    } else   {
+        temp = (UpperByte * 256) + LowerByte;
+        temperature = (temp / 16.0f);
+    }
+    return temperature;
+}
\ No newline at end of file