Alan Millard / Mbed 2 deprecated BeautifulMemeProjectBT

Bluetooth communication for flocking.

Dependencies:   mbed

Fork of BeautifulMemeProject by James Hilder

PsiSwarm/i2c.cpp@27:7eb032772bc2, 2016-01-31 (annotated)

Committer:
alanmillard
Date:
Sun Jan 31 15:14:54 2016 +0000
Revision:
27:7eb032772bc2
Parent:
10:1b09d4bb847b 
Flocking seems to work better now, however the robot controller sometimes "crashes" (unsure why).

Who changed what in which revision?

UserRevisionLine numberNew 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 }