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