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