Bluetooth communication for flocking.
Fork of BeautifulMemeProject by
PsiSwarm/i2c.cpp@7:ef9ab01b9e26, 2015-10-22 (annotated)
- Committer:
- jah128
- Date:
- Thu Oct 22 13:28:17 2015 +0000
- Revision:
- 7:ef9ab01b9e26
- Parent:
- 6:ff3c66f7372b
- Child:
- 10:1b09d4bb847b
Bug fix in bearing measurement
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jah128 | 0:8a5497a2e366 | 1 | /* University of York Robotics Laboratory PsiSwarm Library: I2C Source File |
jah128 | 0:8a5497a2e366 | 2 | * |
jah128 | 0:8a5497a2e366 | 3 | * File: i2c.cpp |
jah128 | 0:8a5497a2e366 | 4 | * |
jah128 | 6:ff3c66f7372b | 5 | * (C) Dept. Electronics & Computer Science, University of York |
jah128 | 6:ff3c66f7372b | 6 | * James Hilder, Alan Millard, Homero Elizondo, Jon Timmis |
jah128 | 0:8a5497a2e366 | 7 | * |
jah128 | 6:ff3c66f7372b | 8 | * PsiSwarm Library Version: 0.3 |
jah128 | 0:8a5497a2e366 | 9 | * |
jah128 | 6:ff3c66f7372b | 10 | * October 2015 |
jah128 | 0:8a5497a2e366 | 11 | * |
jah128 | 0:8a5497a2e366 | 12 | */ |
jah128 | 0:8a5497a2e366 | 13 | |
jah128 | 0:8a5497a2e366 | 14 | #include "psiswarm.h" |
jah128 | 0:8a5497a2e366 | 15 | |
jah128 | 0:8a5497a2e366 | 16 | char gpio_byte0; |
jah128 | 0:8a5497a2e366 | 17 | char gpio_byte1; |
jah128 | 0:8a5497a2e366 | 18 | char user_id_set = 0; |
jah128 | 0:8a5497a2e366 | 19 | char wheel_enc_set = 0; |
jah128 | 0:8a5497a2e366 | 20 | char switch_set = 0; |
jah128 | 0:8a5497a2e366 | 21 | |
jah128 | 0:8a5497a2e366 | 22 | char emitter_byte = 0x00; |
jah128 | 0:8a5497a2e366 | 23 | |
jah128 | 0:8a5497a2e366 | 24 | Timeout update_timeout; |
jah128 | 0:8a5497a2e366 | 25 | |
jah128 | 0:8a5497a2e366 | 26 | char test; |
jah128 | 0:8a5497a2e366 | 27 | |
jah128 | 0:8a5497a2e366 | 28 | void IF_set_IR_emitter_output(char emitter, char state) |
jah128 | 0:8a5497a2e366 | 29 | { |
jah128 | 0:8a5497a2e366 | 30 | if(emitter <3) { |
jah128 | 0:8a5497a2e366 | 31 | if(state == 0) { |
jah128 | 0:8a5497a2e366 | 32 | char shift = 1 << emitter; |
jah128 | 0:8a5497a2e366 | 33 | emitter_byte &= (0xFF - shift); |
jah128 | 0:8a5497a2e366 | 34 | } |
jah128 | 0:8a5497a2e366 | 35 | if(state == 1) { |
jah128 | 0:8a5497a2e366 | 36 | char shift = 1 << emitter; |
jah128 | 0:8a5497a2e366 | 37 | emitter_byte |= shift; |
jah128 | 0:8a5497a2e366 | 38 | } |
jah128 | 0:8a5497a2e366 | 39 | char data[2]; |
jah128 | 0:8a5497a2e366 | 40 | data [0] = 0x0A; //Write to OLAT register |
jah128 | 0:8a5497a2e366 | 41 | data [1] = emitter_byte; //GP0-3 are outputs on aux expansion IC |
jah128 | 0:8a5497a2e366 | 42 | //pc.printf("%c\n", emitter_byte); |
jah128 | 0:8a5497a2e366 | 43 | primary_i2c.write(AUX_IC_ADDRESS,data,2,false); |
jah128 | 0:8a5497a2e366 | 44 | } |
jah128 | 0:8a5497a2e366 | 45 | } |
jah128 | 0:8a5497a2e366 | 46 | |
jah128 | 0:8a5497a2e366 | 47 | void IF_set_base_LED(char state) |
jah128 | 0:8a5497a2e366 | 48 | { |
jah128 | 0:8a5497a2e366 | 49 | if(state == 0) { |
jah128 | 0:8a5497a2e366 | 50 | emitter_byte &= 0xF7; |
jah128 | 0:8a5497a2e366 | 51 | } else emitter_byte |= 0x08; |
jah128 | 0:8a5497a2e366 | 52 | char data[2]; |
jah128 | 0:8a5497a2e366 | 53 | data [0] = 0x0A; //Write to OLAT register |
jah128 | 0:8a5497a2e366 | 54 | data [1] = emitter_byte; //GP0-3 are outputs on aux expansion IC |
jah128 | 0:8a5497a2e366 | 55 | primary_i2c.write(AUX_IC_ADDRESS,data,2,false); |
jah128 | 0:8a5497a2e366 | 56 | |
jah128 | 0:8a5497a2e366 | 57 | } |
jah128 | 0:8a5497a2e366 | 58 | |
jah128 | 0:8a5497a2e366 | 59 | unsigned short IF_read_IR_adc_value(char adc, char index) |
jah128 | 0:8a5497a2e366 | 60 | { |
jah128 | 0:8a5497a2e366 | 61 | char address = ADC1_ADDRESS; |
jah128 | 0:8a5497a2e366 | 62 | if(adc == 2) address=ADC2_ADDRESS; |
jah128 | 0:8a5497a2e366 | 63 | // Returns the raw sensor value for the IR sensor defined by index (range 0-7). |
jah128 | 0:8a5497a2e366 | 64 | short value = 0; |
jah128 | 0:8a5497a2e366 | 65 | // Read a single value from the ADC |
jah128 | 0:8a5497a2e366 | 66 | if(index<8) { |
jah128 | 0:8a5497a2e366 | 67 | char apb[1]; |
jah128 | 0:8a5497a2e366 | 68 | char data[2]; |
jah128 | 0:8a5497a2e366 | 69 | switch(index) { |
jah128 | 0:8a5497a2e366 | 70 | case 0: |
jah128 | 0:8a5497a2e366 | 71 | apb[0]=0x80; |
jah128 | 0:8a5497a2e366 | 72 | break; |
jah128 | 0:8a5497a2e366 | 73 | case 1: |
jah128 | 0:8a5497a2e366 | 74 | apb[0]=0x90; |
jah128 | 0:8a5497a2e366 | 75 | break; |
jah128 | 0:8a5497a2e366 | 76 | case 2: |
jah128 | 0:8a5497a2e366 | 77 | apb[0]=0xA0; |
jah128 | 0:8a5497a2e366 | 78 | break; |
jah128 | 0:8a5497a2e366 | 79 | case 3: |
jah128 | 0:8a5497a2e366 | 80 | apb[0]=0xB0; |
jah128 | 0:8a5497a2e366 | 81 | break; |
jah128 | 0:8a5497a2e366 | 82 | case 4: |
jah128 | 0:8a5497a2e366 | 83 | apb[0]=0xC0; |
jah128 | 0:8a5497a2e366 | 84 | break; |
jah128 | 0:8a5497a2e366 | 85 | case 5: |
jah128 | 0:8a5497a2e366 | 86 | apb[0]=0xD0; |
jah128 | 0:8a5497a2e366 | 87 | break; |
jah128 | 0:8a5497a2e366 | 88 | case 6: |
jah128 | 0:8a5497a2e366 | 89 | apb[0]=0xE0; |
jah128 | 0:8a5497a2e366 | 90 | break; |
jah128 | 0:8a5497a2e366 | 91 | case 7: |
jah128 | 0:8a5497a2e366 | 92 | apb[0]=0xF0; |
jah128 | 0:8a5497a2e366 | 93 | break; |
jah128 | 0:8a5497a2e366 | 94 | } |
jah128 | 0:8a5497a2e366 | 95 | primary_i2c.write(address,apb,1,false); |
jah128 | 0:8a5497a2e366 | 96 | primary_i2c.read(address,data,2,false); |
jah128 | 0:8a5497a2e366 | 97 | value=((data[0] % 16)<<8)+data[1]; |
jah128 | 0:8a5497a2e366 | 98 | if(value > 4096) value=4096; |
jah128 | 0:8a5497a2e366 | 99 | value=4096-value; |
jah128 | 0:8a5497a2e366 | 100 | } |
jah128 | 0:8a5497a2e366 | 101 | return value; |
jah128 | 0:8a5497a2e366 | 102 | } |
jah128 | 0:8a5497a2e366 | 103 | |
jah128 | 0:8a5497a2e366 | 104 | char IF_setup_led_expansion_ic(void) |
jah128 | 0:8a5497a2e366 | 105 | { |
jah128 | 0:8a5497a2e366 | 106 | //LED expansion IC is PCA9555 |
jah128 | 0:8a5497a2e366 | 107 | //Address is 0100 001x (0x42) {defined by LED_IC_ADDRESS} |
jah128 | 0:8a5497a2e366 | 108 | //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) |
jah128 | 0:8a5497a2e366 | 109 | //Message structure: {Address-RW}{Command}{Port 0}{Port 1} |
jah128 | 0:8a5497a2e366 | 110 | //Command bytes: 00000010 (0x02) = Write to output port |
jah128 | 0:8a5497a2e366 | 111 | //Command bytes: 00000110 (0x06) = Write to config registers |
jah128 | 0:8a5497a2e366 | 112 | //Note that for the LEDs, 0 = on, 1 = off |
jah128 | 0:8a5497a2e366 | 113 | //Port 0 = LED 1:4 Red:Green |
jah128 | 0:8a5497a2e366 | 114 | //Port 1 = LED 5:8 Red:Green |
jah128 | 0:8a5497a2e366 | 115 | char data [3]; |
jah128 | 0:8a5497a2e366 | 116 | data [0] = 0x06; //Write config registers |
jah128 | 0:8a5497a2e366 | 117 | data [1] = 0x00; //All 8 pins in port 0 are outputs (0) |
jah128 | 0:8a5497a2e366 | 118 | data [2] = 0x00; //All 8 pins in port 1 are outputs (0) |
jah128 | 0:8a5497a2e366 | 119 | primary_i2c.write(LED_IC_ADDRESS,data,3,false); |
jah128 | 0:8a5497a2e366 | 120 | |
jah128 | 0:8a5497a2e366 | 121 | //Turn all LEDs on |
jah128 | 0:8a5497a2e366 | 122 | data [0] = 0x02; //Write to output port |
jah128 | 0:8a5497a2e366 | 123 | data [1] = 0x00; //Enable LED1-4 (both colours) |
jah128 | 0:8a5497a2e366 | 124 | data [2] = 0x00; //Enable LED5-8 (both colours) |
jah128 | 0:8a5497a2e366 | 125 | primary_i2c.write(LED_IC_ADDRESS,data,3,false); |
jah128 | 0:8a5497a2e366 | 126 | |
jah128 | 0:8a5497a2e366 | 127 | wait(0.05); |
jah128 | 0:8a5497a2e366 | 128 | //Turn all LEDs off |
jah128 | 0:8a5497a2e366 | 129 | data [0] = 0x02; //Write to output port |
jah128 | 0:8a5497a2e366 | 130 | data [1] = 0xFF; //Enable LED1-4 (both colours) |
jah128 | 0:8a5497a2e366 | 131 | data [2] = 0xFF; //Enable LED5-8 (both colours) |
jah128 | 0:8a5497a2e366 | 132 | return primary_i2c.write(LED_IC_ADDRESS,data,3,false); |
jah128 | 0:8a5497a2e366 | 133 | } |
jah128 | 0:8a5497a2e366 | 134 | |
jah128 | 0:8a5497a2e366 | 135 | //Returns 0 if successful, 1 if test mode button pressed |
jah128 | 0:8a5497a2e366 | 136 | void IF_setup_gpio_expansion_ic(void) |
jah128 | 0:8a5497a2e366 | 137 | { |
jah128 | 0:8a5497a2e366 | 138 | //Main GPIO expansion IC is PCA9555 |
jah128 | 0:8a5497a2e366 | 139 | //Address is 0100 000x (0x40) {defined by GPIO_IC_ADDRESS} |
jah128 | 0:8a5497a2e366 | 140 | //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) |
jah128 | 0:8a5497a2e366 | 141 | //Message structure: {Address-RW}{Command}{Port 0}{Port 1} |
jah128 | 0:8a5497a2e366 | 142 | //Command bytes: 00000010 (0x02) = Write to output port |
jah128 | 0:8a5497a2e366 | 143 | //Command bytes: 00000110 (0x06) = Write to config registers |
jah128 | 0:8a5497a2e366 | 144 | //Note that for the LEDs, 0 = on, 1 = off |
jah128 | 0:8a5497a2e366 | 145 | //Port 0 = PGDL; PGDR; PGDIR; UP; DOWN; LEFT; RIGHT; CENTER |
jah128 | 0:8a5497a2e366 | 146 | //Port 1 = ENC_LA; ENC_LB; ENC_RA; ENC_RB; ID0; ID1; ID2; ID3 |
jah128 | 0:8a5497a2e366 | 147 | char data [3]; |
jah128 | 5:598298aa4900 | 148 | char okay = 1; |
jah128 | 0:8a5497a2e366 | 149 | data [0] = 0x06; //Write config registers |
jah128 | 0:8a5497a2e366 | 150 | data [1] = 0xFF; //All 8 pins in port 0 are inputs (1) |
jah128 | 0:8a5497a2e366 | 151 | data [2] = 0xFF; //All 8 pins in port 1 are inputs (1) |
jah128 | 0:8a5497a2e366 | 152 | if(primary_i2c.write(GPIO_IC_ADDRESS,data,3,false) != 0) { |
jah128 | 0:8a5497a2e366 | 153 | system_warnings += 2; |
jah128 | 5:598298aa4900 | 154 | okay = 0; |
jah128 | 0:8a5497a2e366 | 155 | debug("- WARNING: No I2C acknowledge for main GPIO IC\n"); |
jah128 | 6:ff3c66f7372b | 156 | if(HALT_ON_GPIO_ERROR){ |
jah128 | 6:ff3c66f7372b | 157 | debug("- PROGRAM HALTED. Check that robot is switched on!\n"); |
jah128 | 6:ff3c66f7372b | 158 | while(1){ |
jah128 | 6:ff3c66f7372b | 159 | mbed_led1=1; |
jah128 | 6:ff3c66f7372b | 160 | mbed_led2=1; |
jah128 | 6:ff3c66f7372b | 161 | mbed_led3=0; |
jah128 | 6:ff3c66f7372b | 162 | mbed_led4=0; |
jah128 | 6:ff3c66f7372b | 163 | wait(0.25); |
jah128 | 6:ff3c66f7372b | 164 | mbed_led1=0; |
jah128 | 6:ff3c66f7372b | 165 | mbed_led2=0; |
jah128 | 6:ff3c66f7372b | 166 | mbed_led3=1; |
jah128 | 6:ff3c66f7372b | 167 | mbed_led4=1; |
jah128 | 6:ff3c66f7372b | 168 | wait(0.25); |
jah128 | 6:ff3c66f7372b | 169 | } |
jah128 | 6:ff3c66f7372b | 170 | } |
jah128 | 0:8a5497a2e366 | 171 | } |
jah128 | 0:8a5497a2e366 | 172 | //Set all inputs to polarity-inverted (so a logic low = 1) |
jah128 | 0:8a5497a2e366 | 173 | data [0] = 0x04; //Write to polarity inversion ports |
jah128 | 5:598298aa4900 | 174 | data [1] = 0xF8; //Invert polarity of all switch input bits in input port 0 [but not power-good inputs] |
jah128 | 0:8a5497a2e366 | 175 | data [2] = 0xFF; //Invert polarity of all bits in input port 1 |
jah128 | 0:8a5497a2e366 | 176 | primary_i2c.write(GPIO_IC_ADDRESS,data,3,false); |
jah128 | 0:8a5497a2e366 | 177 | |
jah128 | 0:8a5497a2e366 | 178 | wait(0.01); |
jah128 | 0:8a5497a2e366 | 179 | |
jah128 | 0:8a5497a2e366 | 180 | //Read data |
jah128 | 0:8a5497a2e366 | 181 | char read_data[2]; |
jah128 | 0:8a5497a2e366 | 182 | char command[1]; //Command to read from input port 0 |
jah128 | 0:8a5497a2e366 | 183 | command[0]=0; |
jah128 | 0:8a5497a2e366 | 184 | primary_i2c.write(GPIO_IC_ADDRESS,command,1,false); |
jah128 | 0:8a5497a2e366 | 185 | primary_i2c.read(GPIO_IC_ADDRESS,read_data,2,false); |
jah128 | 0:8a5497a2e366 | 186 | gpio_byte0 = read_data[0]; |
jah128 | 0:8a5497a2e366 | 187 | //char ret_val = (gpio_byte0 & 0xF8) >> 3; //Returns a >0 value if a button is being pushed |
jah128 | 0:8a5497a2e366 | 188 | gpio_byte1 = read_data[1]; |
jah128 | 5:598298aa4900 | 189 | if(okay && testing_voltage_regulators_flag)debug("- Checking 3.3V voltage regulators\n"); |
jah128 | 0:8a5497a2e366 | 190 | IF_parse_gpio_byte0(gpio_byte0); |
jah128 | 0:8a5497a2e366 | 191 | IF_parse_gpio_byte1(gpio_byte1); |
jah128 | 5:598298aa4900 | 192 | testing_voltage_regulators_flag = 0; |
jah128 | 0:8a5497a2e366 | 193 | //Setup interrupt handler for GPIO interrupts |
jah128 | 0:8a5497a2e366 | 194 | gpio_interrupt.mode(PullUp); |
jah128 | 0:8a5497a2e366 | 195 | gpio_interrupt.rise(&IF_handle_gpio_interrupt); |
jah128 | 0:8a5497a2e366 | 196 | //pc.printf("%c %c",gpio_byte0,gpio_byte1); |
jah128 | 0:8a5497a2e366 | 197 | |
jah128 | 0:8a5497a2e366 | 198 | //Secondary GPIO expansion IC is MCP23009 |
jah128 | 0:8a5497a2e366 | 199 | //Address is 0100 111 (0x4E) {defined by AUX_IC_ADDRESS} |
jah128 | 0:8a5497a2e366 | 200 | //GP0,1,2,3 are outputs for driving infrared emitters and the base LED |
jah128 | 0:8a5497a2e366 | 201 | //IODIR register wants to be 0xF0 (1=input, 0=output) |
jah128 | 0:8a5497a2e366 | 202 | data [0] = 0x00; //Write to IODIR register |
jah128 | 0:8a5497a2e366 | 203 | data [1] = 0xF0; //Set GP0-3 as outputs |
jah128 | 0:8a5497a2e366 | 204 | primary_i2c.write(AUX_IC_ADDRESS,data,2,false); |
jah128 | 0:8a5497a2e366 | 205 | |
jah128 | 0:8a5497a2e366 | 206 | if(primary_i2c.write(AUX_IC_ADDRESS,data,2,false) != 0) { |
jah128 | 0:8a5497a2e366 | 207 | system_warnings += 4; |
jah128 | 0:8a5497a2e366 | 208 | debug("- WARNING: No I2C acknowledge for aux GPIO IC\n"); |
jah128 | 0:8a5497a2e366 | 209 | } |
jah128 | 0:8a5497a2e366 | 210 | data [0] = 0x06; //Write to GPPU register |
jah128 | 6:ff3c66f7372b | 211 | data [1] = 0x3F; //Set GP0-3 as active pull-up outputs and P4,P5 as pull-up inputs |
jah128 | 0:8a5497a2e366 | 212 | primary_i2c.write(AUX_IC_ADDRESS,data,2,false); |
jah128 | 0:8a5497a2e366 | 213 | |
jah128 | 0:8a5497a2e366 | 214 | //My interrupt is not so reliable: poll with a 50ms timeout in case interrupts aren't handled |
jah128 | 0:8a5497a2e366 | 215 | update_timeout.attach_us(&IF_update_gpio_inputs,50000); |
jah128 | 0:8a5497a2e366 | 216 | //return ret_val; |
jah128 | 0:8a5497a2e366 | 217 | } |
jah128 | 0:8a5497a2e366 | 218 | |
jah128 | 6:ff3c66f7372b | 219 | void IF_read_aux_ic_data() |
jah128 | 6:ff3c66f7372b | 220 | { |
jah128 | 6:ff3c66f7372b | 221 | //Read the values of the input pins on the auxilliary GPIO expander |
jah128 | 6:ff3c66f7372b | 222 | char write_data [1]; |
jah128 | 6:ff3c66f7372b | 223 | char read_data [1]; |
jah128 | 6:ff3c66f7372b | 224 | write_data[0] = 0x09; |
jah128 | 6:ff3c66f7372b | 225 | primary_i2c.write(AUX_IC_ADDRESS,write_data,1,false); |
jah128 | 6:ff3c66f7372b | 226 | primary_i2c.read(AUX_IC_ADDRESS,read_data,1,false); |
jah128 | 6:ff3c66f7372b | 227 | |
jah128 | 6:ff3c66f7372b | 228 | pc.printf("Aux IC Data:%X\n",read_data[0]); |
jah128 | 6:ff3c66f7372b | 229 | } |
jah128 | 6:ff3c66f7372b | 230 | |
jah128 | 0:8a5497a2e366 | 231 | void IF_parse_gpio_byte0(char byte) |
jah128 | 0:8a5497a2e366 | 232 | { |
jah128 | 0:8a5497a2e366 | 233 | gpio_byte0 = byte; |
jah128 | 0:8a5497a2e366 | 234 | //GPIO byte zero contains the power line traces and the switch states |
jah128 | 0:8a5497a2e366 | 235 | char current_switch = ((gpio_byte0 & 0xF8) >> 3); |
jah128 | 0:8a5497a2e366 | 236 | if(switch_set == 1) { |
jah128 | 0:8a5497a2e366 | 237 | if(current_switch != switch_byte) { |
jah128 | 0:8a5497a2e366 | 238 | previous_switch_byte = switch_byte; |
jah128 | 0:8a5497a2e366 | 239 | switch_byte = current_switch; |
jah128 | 0:8a5497a2e366 | 240 | event++; |
jah128 | 0:8a5497a2e366 | 241 | switch_event = 1; |
jah128 | 0:8a5497a2e366 | 242 | } |
jah128 | 0:8a5497a2e366 | 243 | } else { |
jah128 | 0:8a5497a2e366 | 244 | switch_byte = current_switch; |
jah128 | 0:8a5497a2e366 | 245 | switch_set = 1; |
jah128 | 0:8a5497a2e366 | 246 | } |
jah128 | 5:598298aa4900 | 247 | if(((gpio_byte0 & 0x01)) != power_good_motor_left){ |
jah128 | 5:598298aa4900 | 248 | power_good_motor_left = (gpio_byte0 & 0x01); |
jah128 | 5:598298aa4900 | 249 | if(!power_good_motor_left){ |
jah128 | 5:598298aa4900 | 250 | if(testing_voltage_regulators_flag || SHOW_VR_WARNINGS)debug("- WARNING: Voltage regulator left motor low\n"); |
jah128 | 5:598298aa4900 | 251 | } |
jah128 | 5:598298aa4900 | 252 | else if(testing_voltage_regulators_flag)debug("- Power good left motor v.reg\n"); |
jah128 | 5:598298aa4900 | 253 | } |
jah128 | 5:598298aa4900 | 254 | if(((gpio_byte0 & 0x02) >> 1) != power_good_motor_right){ |
jah128 | 5:598298aa4900 | 255 | power_good_motor_right = (gpio_byte0 & 0x02) >> 1; |
jah128 | 5:598298aa4900 | 256 | if(!power_good_motor_right){ |
jah128 | 5:598298aa4900 | 257 | if(testing_voltage_regulators_flag || SHOW_VR_WARNINGS)debug("- WARNING: Voltage regulator right motor low\n"); |
jah128 | 5:598298aa4900 | 258 | } |
jah128 | 5:598298aa4900 | 259 | else if(testing_voltage_regulators_flag)debug("- Power good right motor v.reg\n"); |
jah128 | 5:598298aa4900 | 260 | } |
jah128 | 5:598298aa4900 | 261 | if(((gpio_byte0 & 0x04) >> 2) != power_good_infrared){ |
jah128 | 5:598298aa4900 | 262 | power_good_infrared = (gpio_byte0 & 0x04) >> 2; |
jah128 | 5:598298aa4900 | 263 | if(!power_good_infrared){ |
jah128 | 5:598298aa4900 | 264 | if(testing_voltage_regulators_flag || SHOW_VR_WARNINGS)debug("- WARNING: Voltage regulator infrared low\n"); |
jah128 | 5:598298aa4900 | 265 | } |
jah128 | 5:598298aa4900 | 266 | else if(testing_voltage_regulators_flag)debug("- Power good infrared and aux v.reg\n"); |
jah128 | 5:598298aa4900 | 267 | } |
jah128 | 5:598298aa4900 | 268 | if(USE_LED4_FOR_VR_WARNINGS){ |
jah128 | 5:598298aa4900 | 269 | mbed_led4 = (!power_good_motor_left || !power_good_motor_right || !power_good_infrared); |
jah128 | 5:598298aa4900 | 270 | } |
jah128 | 6:ff3c66f7372b | 271 | //Halt the system if settings flag is set and all v-regs are bad [usually this means robot is switched off!] |
jah128 | 6:ff3c66f7372b | 272 | if(HALT_ON_ALL_VREGS_LOW && !power_good_motor_left && !power_good_motor_right && !power_good_infrared){ |
jah128 | 7:ef9ab01b9e26 | 273 | debug("- PROGRAM HALTED. Check that robot is switched on!\n"); |
jah128 | 6:ff3c66f7372b | 274 | while(1){ |
jah128 | 6:ff3c66f7372b | 275 | mbed_led1=1; |
jah128 | 6:ff3c66f7372b | 276 | mbed_led2=0; |
jah128 | 6:ff3c66f7372b | 277 | mbed_led3=1; |
jah128 | 6:ff3c66f7372b | 278 | mbed_led4=0; |
jah128 | 6:ff3c66f7372b | 279 | wait(0.25); |
jah128 | 6:ff3c66f7372b | 280 | mbed_led1=0; |
jah128 | 6:ff3c66f7372b | 281 | mbed_led2=1; |
jah128 | 6:ff3c66f7372b | 282 | mbed_led3=0; |
jah128 | 6:ff3c66f7372b | 283 | mbed_led4=1; |
jah128 | 6:ff3c66f7372b | 284 | wait(0.25); |
jah128 | 6:ff3c66f7372b | 285 | } |
jah128 | 6:ff3c66f7372b | 286 | } |
jah128 | 0:8a5497a2e366 | 287 | } |
jah128 | 0:8a5497a2e366 | 288 | |
jah128 | 0:8a5497a2e366 | 289 | void IF_parse_gpio_byte1(char byte) |
jah128 | 0:8a5497a2e366 | 290 | { |
jah128 | 0:8a5497a2e366 | 291 | gpio_byte1 = byte; |
jah128 | 0:8a5497a2e366 | 292 | //GPIO byte one contains the wheel encoders and the ID switch |
jah128 | 0:8a5497a2e366 | 293 | char current_id = ((gpio_byte1 & 0xF0)>> 4); |
jah128 | 0:8a5497a2e366 | 294 | if(user_id_set == 1) { |
jah128 | 0:8a5497a2e366 | 295 | if(robot_id != current_id) { |
jah128 | 0:8a5497a2e366 | 296 | previous_robot_id = robot_id; |
jah128 | 0:8a5497a2e366 | 297 | robot_id = current_id; |
jah128 | 0:8a5497a2e366 | 298 | event++; |
jah128 | 0:8a5497a2e366 | 299 | change_id_event = 1; |
jah128 | 0:8a5497a2e366 | 300 | } |
jah128 | 0:8a5497a2e366 | 301 | } else { |
jah128 | 0:8a5497a2e366 | 302 | robot_id = current_id; |
jah128 | 0:8a5497a2e366 | 303 | user_id_set = 1; |
jah128 | 0:8a5497a2e366 | 304 | } |
jah128 | 0:8a5497a2e366 | 305 | char current_encoder = (gpio_byte1 & 0x0F); |
jah128 | 0:8a5497a2e366 | 306 | if(wheel_enc_set == 1) { |
jah128 | 0:8a5497a2e366 | 307 | if(wheel_encoder_byte != current_encoder) { |
jah128 | 0:8a5497a2e366 | 308 | previous_wheel_encoder_byte = wheel_encoder_byte; |
jah128 | 0:8a5497a2e366 | 309 | wheel_encoder_byte = current_encoder; |
jah128 | 0:8a5497a2e366 | 310 | event++; |
jah128 | 0:8a5497a2e366 | 311 | encoder_event = 1; |
jah128 | 0:8a5497a2e366 | 312 | } |
jah128 | 0:8a5497a2e366 | 313 | } else { |
jah128 | 0:8a5497a2e366 | 314 | wheel_encoder_byte = current_encoder; |
jah128 | 0:8a5497a2e366 | 315 | wheel_enc_set = 1; |
jah128 | 0:8a5497a2e366 | 316 | } |
jah128 | 0:8a5497a2e366 | 317 | } |
jah128 | 0:8a5497a2e366 | 318 | |
jah128 | 0:8a5497a2e366 | 319 | void IF_handle_gpio_interrupt() |
jah128 | 0:8a5497a2e366 | 320 | { |
jah128 | 0:8a5497a2e366 | 321 | test = 1-test; |
jah128 | 5:598298aa4900 | 322 | if(USE_LED3_FOR_INTERRUPTS) mbed_led3 = test; |
jah128 | 0:8a5497a2e366 | 323 | IF_update_gpio_inputs(); |
jah128 | 0:8a5497a2e366 | 324 | } |
jah128 | 0:8a5497a2e366 | 325 | |
jah128 | 0:8a5497a2e366 | 326 | char IF_is_switch_pressed() |
jah128 | 0:8a5497a2e366 | 327 | { |
jah128 | 0:8a5497a2e366 | 328 | //Read data |
jah128 | 0:8a5497a2e366 | 329 | char data[1]; |
jah128 | 0:8a5497a2e366 | 330 | char command[1] = {0}; //Command to read from input port 0 |
jah128 | 0:8a5497a2e366 | 331 | primary_i2c.write(GPIO_IC_ADDRESS,command,1,false); |
jah128 | 0:8a5497a2e366 | 332 | primary_i2c.read(GPIO_IC_ADDRESS,data,1,false); |
jah128 | 0:8a5497a2e366 | 333 | return (data[0] & 0x80); //Returns a 1 if the center button is being pushed |
jah128 | 0:8a5497a2e366 | 334 | } |
jah128 | 0:8a5497a2e366 | 335 | |
jah128 | 0:8a5497a2e366 | 336 | |
jah128 | 0:8a5497a2e366 | 337 | char IF_get_switch_state() |
jah128 | 0:8a5497a2e366 | 338 | { |
jah128 | 0:8a5497a2e366 | 339 | //Read data |
jah128 | 0:8a5497a2e366 | 340 | char data[1]; |
jah128 | 0:8a5497a2e366 | 341 | char command[1] = {0}; //Command to read from input port 0 |
jah128 | 0:8a5497a2e366 | 342 | primary_i2c.write(GPIO_IC_ADDRESS,command,1,false); |
jah128 | 0:8a5497a2e366 | 343 | primary_i2c.read(GPIO_IC_ADDRESS,data,1,false); |
jah128 | 0:8a5497a2e366 | 344 | return (data[0] & 0xF8) >> 3; //Returns the current switch state |
jah128 | 0:8a5497a2e366 | 345 | } |
jah128 | 0:8a5497a2e366 | 346 | |
jah128 | 0:8a5497a2e366 | 347 | void IF_update_gpio_inputs() |
jah128 | 0:8a5497a2e366 | 348 | { |
jah128 | 0:8a5497a2e366 | 349 | update_timeout.detach(); |
jah128 | 0:8a5497a2e366 | 350 | //Read data |
jah128 | 0:8a5497a2e366 | 351 | char data[2]; |
jah128 | 0:8a5497a2e366 | 352 | char command[1] = {0}; //Command to read from input port 0 |
jah128 | 0:8a5497a2e366 | 353 | primary_i2c.write(GPIO_IC_ADDRESS,command,1,false); |
jah128 | 0:8a5497a2e366 | 354 | primary_i2c.read(GPIO_IC_ADDRESS,data,2,false); |
jah128 | 0:8a5497a2e366 | 355 | if(data[0]!=gpio_byte0) { |
jah128 | 0:8a5497a2e366 | 356 | IF_parse_gpio_byte0(data[0]); |
jah128 | 0:8a5497a2e366 | 357 | } |
jah128 | 0:8a5497a2e366 | 358 | if(data[1]!=gpio_byte1) { |
jah128 | 0:8a5497a2e366 | 359 | IF_parse_gpio_byte1(data[1]); |
jah128 | 0:8a5497a2e366 | 360 | } |
jah128 | 0:8a5497a2e366 | 361 | update_timeout.attach_us(&IF_update_gpio_inputs,50000); |
jah128 | 0:8a5497a2e366 | 362 | } |
jah128 | 0:8a5497a2e366 | 363 | |
jah128 | 0:8a5497a2e366 | 364 | |
jah128 | 0:8a5497a2e366 | 365 | void IF_write_to_led_ic(char byte_0, char byte_1) |
jah128 | 0:8a5497a2e366 | 366 | { |
jah128 | 0:8a5497a2e366 | 367 | //Set LEDs |
jah128 | 0:8a5497a2e366 | 368 | char data[3]; |
jah128 | 0:8a5497a2e366 | 369 | data [0] = 0x02; //Write to output port |
jah128 | 0:8a5497a2e366 | 370 | data [1] = byte_0; |
jah128 | 0:8a5497a2e366 | 371 | data [2] = byte_1; |
jah128 | 0:8a5497a2e366 | 372 | primary_i2c.write(LED_IC_ADDRESS,data,3,false); |
jah128 | 0:8a5497a2e366 | 373 | } |
jah128 | 0:8a5497a2e366 | 374 | |
jah128 | 0:8a5497a2e366 | 375 | |
jah128 | 0:8a5497a2e366 | 376 | void IF_setup_temperature_sensor() |
jah128 | 0:8a5497a2e366 | 377 | { |
jah128 | 0:8a5497a2e366 | 378 | char data[3]; |
jah128 | 0:8a5497a2e366 | 379 | data[0] = 0x04; //Set critical temp limit |
jah128 | 0:8a5497a2e366 | 380 | data[1] = TEMPERATURE_CRITICAL_HI; |
jah128 | 0:8a5497a2e366 | 381 | data[2] = TEMPEARTURE_CRITICAL_LO; |
jah128 | 0:8a5497a2e366 | 382 | primary_i2c.write(TEMPERATURE_ADDRESS,data,3,false); |
jah128 | 0:8a5497a2e366 | 383 | data[0] = 0x02; //Set high temp limit |
jah128 | 0:8a5497a2e366 | 384 | data[1] = TEMPERATURE_HIGH_HI; |
jah128 | 0:8a5497a2e366 | 385 | data[2] = TEMPEARTURE_HIGH_LO; |
jah128 | 0:8a5497a2e366 | 386 | primary_i2c.write(TEMPERATURE_ADDRESS,data,3,false); |
jah128 | 0:8a5497a2e366 | 387 | data[0] = 0x03; //Set low temp limit |
jah128 | 0:8a5497a2e366 | 388 | data[1] = TEMPERATURE_LOW_HI; |
jah128 | 0:8a5497a2e366 | 389 | data[2] = TEMPEARTURE_LOW_LO; |
jah128 | 0:8a5497a2e366 | 390 | primary_i2c.write(TEMPERATURE_ADDRESS,data,3,false); |
jah128 | 0:8a5497a2e366 | 391 | } |
jah128 | 0:8a5497a2e366 | 392 | |
jah128 | 0:8a5497a2e366 | 393 | float IF_read_from_temperature_sensor() |
jah128 | 0:8a5497a2e366 | 394 | { |
jah128 | 0:8a5497a2e366 | 395 | char command[1] = {0x05}; //Write to Ta Register |
jah128 | 0:8a5497a2e366 | 396 | char data[3]; |
jah128 | 0:8a5497a2e366 | 397 | signed int temp; |
jah128 | 0:8a5497a2e366 | 398 | float temperature; |
jah128 | 0:8a5497a2e366 | 399 | primary_i2c.write(TEMPERATURE_ADDRESS,command,1,false); |
jah128 | 0:8a5497a2e366 | 400 | primary_i2c.read(TEMPERATURE_ADDRESS,data,2,false); |
jah128 | 0:8a5497a2e366 | 401 | |
jah128 | 0:8a5497a2e366 | 402 | //Convert the temperature data |
jah128 | 0:8a5497a2e366 | 403 | //First Check flag bits |
jah128 | 0:8a5497a2e366 | 404 | char UpperByte = data[0]; |
jah128 | 0:8a5497a2e366 | 405 | char LowerByte = data[1]; |
jah128 | 0:8a5497a2e366 | 406 | if ((UpperByte & 0x80) == 0x80) { |
jah128 | 0:8a5497a2e366 | 407 | debug("- WARNING: Temperature sensor reports critical temperature\n"); |
jah128 | 0:8a5497a2e366 | 408 | } |
jah128 | 0:8a5497a2e366 | 409 | if ((UpperByte & 0x40) == 0x40) { |
jah128 | 0:8a5497a2e366 | 410 | debug("- WARNING: Temperature sensor reports above upper limit\n"); |
jah128 | 0:8a5497a2e366 | 411 | } |
jah128 | 0:8a5497a2e366 | 412 | if ((UpperByte & 0x20) == 0x20) { |
jah128 | 0:8a5497a2e366 | 413 | debug("- WARNING: Temperature sensor reports below lower limit\n"); |
jah128 | 0:8a5497a2e366 | 414 | } |
jah128 | 0:8a5497a2e366 | 415 | UpperByte = UpperByte & 0x1F; //Clear flag bits |
jah128 | 0:8a5497a2e366 | 416 | if ((UpperByte & 0x10) == 0x10) { |
jah128 | 0:8a5497a2e366 | 417 | UpperByte = UpperByte & 0x0F; //Clear SIGN |
jah128 | 0:8a5497a2e366 | 418 | temp = (UpperByte * 256) + LowerByte; |
jah128 | 0:8a5497a2e366 | 419 | temperature = - (temp / 16.0f); |
jah128 | 0:8a5497a2e366 | 420 | } else { |
jah128 | 0:8a5497a2e366 | 421 | temp = (UpperByte * 256) + LowerByte; |
jah128 | 0:8a5497a2e366 | 422 | temperature = (temp / 16.0f); |
jah128 | 0:8a5497a2e366 | 423 | } |
jah128 | 0:8a5497a2e366 | 424 | return temperature; |
jah128 | 0:8a5497a2e366 | 425 | } |