Bluetooth communication for flocking.
Fork of BeautifulMemeProject by
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