i2c_setup.cpp

00001 /* University of York Robotics Laboratory PsiSwarm Library: I2C Setup Source File
00002  * 
00003  * Copyright 2016 University of York
00004  *
00005  * Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. 
00006  * You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0
00007  * Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS
00008  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 
00009  * See the License for the specific language governing permissions and limitations under the License.
00010  *
00011  * File: i2c_setup.cpp
00012  *
00013  * (C) Dept. Electronics & Computer Science, University of York
00014  * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis
00015  *
00016  * PsiSwarm Library Version: 0.8
00017  *
00018  * October 2016
00019  *
00020  *
00021  */
00022 
00023 #include "psiswarm.h"
00024 
00025 char gpio_byte0;
00026 char gpio_byte1;
00027 char user_id_set = 0;
00028 char wheel_enc_set = 0;
00029 char switch_set = 0;
00030 
00031 char emitter_byte = 0x00;
00032 
00033 Timeout update_timeout;
00034 
00035 char test;
00036 
00037 char Setup::get_dc_status()
00038 {
00039     IF_read_aux_ic_data();
00040     return status_dc_in;
00041 }
00042 
00043 void Setup::IF_set_IR_emitter_output(char emitter, char state)
00044 {
00045     if(emitter <3) {
00046         if(state == 0) {
00047             char shift = 1 << emitter;
00048             emitter_byte &= (0xFF - shift);
00049         }
00050         if(state == 1) {
00051             char shift = 1 << emitter;
00052             emitter_byte |= shift;
00053         }
00054         char data[2];
00055         data [0] = 0x0A;  //Write to OLAT register
00056         data [1] = emitter_byte;  //GP0-3 are outputs on aux expansion IC
00057         //pc.printf("%c\n", emitter_byte);
00058         primary_i2c.write(AUX_IC_ADDRESS,data,2,false);
00059     }
00060 }
00061 
00062 void Setup::IF_set_base_LED(char state)
00063 {
00064     if(state == 0) {
00065         emitter_byte &= 0xF7;
00066     } else emitter_byte |= 0x08;
00067     char data[2];
00068     data [0] = 0x0A;  //Write to OLAT register
00069     data [1] = emitter_byte;  //GP0-3 are outputs on aux expansion IC
00070     primary_i2c.write(AUX_IC_ADDRESS,data,2,false);
00071 
00072 }
00073 
00074 unsigned short Setup::IF_read_IR_adc_value(char adc, char index)
00075 {
00076     char address = ADC1_ADDRESS;
00077     if(adc == 2) address=ADC2_ADDRESS;
00078     // Returns the raw sensor value for the IR sensor defined by index (range 0-7).
00079     short value = 0;
00080     // Read a single value from the ADC
00081     if(index<8) {
00082         char apb[1];
00083         char data[2];
00084         switch(index) {
00085             case 0:
00086                 apb[0]=0x80;
00087                 break;
00088             case 1:
00089                 apb[0]=0x90;
00090                 break;
00091             case 2:
00092                 apb[0]=0xA0;
00093                 break;
00094             case 3:
00095                 apb[0]=0xB0;
00096                 break;
00097             case 4:
00098                 apb[0]=0xC0;
00099                 break;
00100             case 5:
00101                 apb[0]=0xD0;
00102                 break;
00103             case 6:
00104                 apb[0]=0xE0;
00105                 break;
00106             case 7:
00107                 apb[0]=0xF0;
00108                 break;
00109         }
00110         primary_i2c.write(address,apb,1,false);
00111         primary_i2c.read(address,data,2,false);
00112         value=((data[0] % 16)<<8)+data[1];
00113         if(value > 4096) value=4096;
00114         value=4096-value;
00115     }
00116     return value;
00117 }
00118 
00119 char Setup::IF_setup_led_expansion_ic(void)
00120 {
00121     //LED expansion IC is PCA9555
00122     //Address is 0100 001x (0x42) {defined by LED_IC_ADDRESS}
00123     //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)
00124     //Message structure: {Address-RW}{Command}{Port 0}{Port 1}
00125     //Command bytes: 00000010 (0x02) = Write to output port
00126     //Command bytes: 00000110 (0x06) = Write to config registers
00127     //Note that for the LEDs, 0 = on, 1 = off
00128     //Port 0 = LED 1:4 Red:Green
00129     //Port 1 = LED 5:8 Red:Green
00130     char data [3];
00131     data [0] = 0x06;    //Write config registers
00132     data [1] = 0x00;    //All 8 pins in port 0 are outputs (0)
00133     data [2] = 0x00;    //All 8 pins in port 1 are outputs (0)
00134     primary_i2c.write(LED_IC_ADDRESS,data,3,false);
00135 
00136     //Turn all LEDs on
00137     data [0] = 0x02;    //Write to output port
00138     data [1] = 0x00;    //Enable LED1-4 (both colours)
00139     data [2] = 0x00;    //Enable LED5-8 (both colours)
00140     primary_i2c.write(LED_IC_ADDRESS,data,3,false);
00141 
00142     wait(0.05);
00143     //Turn all LEDs off
00144     data [0] = 0x02;    //Write to output port
00145     data [1] = 0xFF;    //Enable LED1-4 (both colours)
00146     data [2] = 0xFF;    //Enable LED5-8 (both colours)
00147     return primary_i2c.write(LED_IC_ADDRESS,data,3,false);
00148 }
00149 
00150 //Returns 0 if successful, 1 if test mode button pressed
00151 void Setup::IF_setup_gpio_expansion_ic(void)
00152 {
00153     //Main GPIO expansion IC is PCA9555
00154     //Address is 0100 000x (0x40) {defined by GPIO_IC_ADDRESS}
00155     //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)
00156     //Message structure: {Address-RW}{Command}{Port 0}{Port 1}
00157     //Command bytes: 00000010 (0x02) = Write to output port
00158     //Command bytes: 00000110 (0x06) = Write to config registers
00159     //Note that for the LEDs, 0 = on, 1 = off
00160     //Port 0 = PGDL; PGDR; PGDIR; UP; DOWN; LEFT; RIGHT; CENTER
00161     //Port 1 = ENC_LA; ENC_LB; ENC_RA; ENC_RB; ID0; ID1; ID2; ID3
00162     char data [3];
00163     char okay = 1;
00164     data [0] = 0x06;    //Write config registers
00165     data [1] = 0xFF;    //All 8 pins in port 0 are inputs (1)
00166     data [2] = 0xFF;    //All 8 pins in port 1 are inputs (1)
00167     if(primary_i2c.write(GPIO_IC_ADDRESS,data,3,false) != 0) {
00168         system_warnings += 2;
00169         okay = 0;
00170         psi.debug("- WARNING: No I2C acknowledge for main GPIO IC\n");
00171         if(HALT_ON_GPIO_ERROR){
00172             psi.debug("- PROGRAM HALTED.  Check that robot is switched on!\n");
00173             while(1){
00174                mbed_led1=1;
00175                mbed_led2=1;
00176                mbed_led3=0;
00177                mbed_led4=0;
00178                wait(0.25); 
00179                mbed_led1=0;
00180                mbed_led2=0;
00181                mbed_led3=1;
00182                mbed_led4=1;
00183                wait(0.25);   
00184             }   
00185         }
00186     }
00187     //Set all inputs to polarity-inverted (so a logic low = 1)
00188     data [0] = 0x04;    //Write to polarity inversion ports
00189     data [1] = 0xF8;    //Invert polarity of all switch input bits in input port 0 [but not power-good inputs]
00190     data [2] = 0xFF;    //Invert polarity of all bits in input port 1
00191     primary_i2c.write(GPIO_IC_ADDRESS,data,3,false);
00192 
00193     wait(0.01);
00194 
00195     //Read data
00196     char read_data[2];
00197     char command[1]; //Command to read from input port 0
00198     command[0]=0;
00199     primary_i2c.write(GPIO_IC_ADDRESS,command,1,false);
00200     primary_i2c.read(GPIO_IC_ADDRESS,read_data,2,false);
00201     gpio_byte0 = read_data[0];
00202     //char ret_val = (gpio_byte0 & 0xF8) >> 3;  //Returns a >0 value if a button is being pushed
00203     gpio_byte1 = read_data[1];
00204     if(okay && testing_voltage_regulators_flag)psi.debug("- Checking 3.3V voltage regulators\n");
00205     IF_parse_gpio_byte0(gpio_byte0);
00206     IF_parse_gpio_byte1(gpio_byte1);
00207     testing_voltage_regulators_flag = 0;
00208     //Setup interrupt handler for GPIO interrupts
00209     gpio_interrupt.mode(PullUp);
00210     gpio_interrupt.rise(this,&Setup::IF_handle_gpio_interrupt);
00211     //pc.printf("%c %c",gpio_byte0,gpio_byte1);
00212 
00213     //Secondary GPIO expansion IC is MCP23009
00214     //Address is 0100 111 (0x4E) {defined by AUX_IC_ADDRESS}
00215     //GP0,1,2,3 are outputs for driving infrared emitters and the base LED
00216     //IODIR register wants to be 0xF0 (1=input, 0=output)
00217     data [0] = 0x00;  //Write to IODIR register
00218     data [1] = 0xF0;  //Set GP0-3 as outputs
00219     primary_i2c.write(AUX_IC_ADDRESS,data,2,false);
00220 
00221     if(primary_i2c.write(AUX_IC_ADDRESS,data,2,false) != 0) {
00222         system_warnings += 4;
00223         psi.debug("- WARNING: No I2C acknowledge for aux GPIO IC\n");
00224     }
00225     data [0] = 0x06;  //Write to GPPU register
00226     data [1] = 0x3F;  //Set GP0-3 as active pull-up outputs and P4,P5 as pull-up inputs
00227     primary_i2c.write(AUX_IC_ADDRESS,data,2,false);
00228 
00229     //My interrupt is not so reliable: poll with a 50ms timeout in case interrupts aren't handled
00230     update_timeout.attach_us(this,&Setup::IF_update_gpio_inputs,50000);
00231     //return ret_val;
00232 }
00233 
00234 void Setup::IF_read_aux_ic_data()
00235 {
00236     //Read the values of the input pins on the auxilliary GPIO expander
00237     char write_data [1];
00238     char read_data [1];
00239     write_data[0] = 0x09;
00240     primary_i2c.write(AUX_IC_ADDRESS,write_data,1,false);
00241     primary_i2c.read(AUX_IC_ADDRESS,read_data,1,false);
00242     char old_charging_state = status_dc_in;
00243     status_dc_in = 1-((read_data[0] & 0x10) >> 4);
00244     if(status_dc_in!=old_charging_state){
00245         if(status_dc_in == 0)psi.debug("No DC input\n");
00246         else psi.debug("DC input to charge pins\n");
00247     }
00248     //pc.printf("Aux IC Data:%X Charge:%d\n",read_data[0],charge_in);
00249 }
00250 
00251 void Setup::IF_parse_gpio_byte0(char byte)
00252 {
00253     gpio_byte0 = byte;
00254     //GPIO byte zero contains the power line traces and the switch states
00255     char current_switch = ((gpio_byte0 & 0xF8) >> 3);
00256     if(switch_set == 1) {
00257         if(current_switch != switch_byte) {
00258             previous_switch_byte = switch_byte;
00259             switch_byte = current_switch;
00260             event++;
00261             switch_event = 1;
00262         }
00263     } else {
00264         switch_byte = current_switch;
00265         switch_set = 1;
00266     }
00267     if(((gpio_byte0 & 0x01)) != power_good_motor_left){
00268         power_good_motor_left = (gpio_byte0 & 0x01);
00269         if(!power_good_motor_left){
00270             if(testing_voltage_regulators_flag || SHOW_VR_WARNINGS)psi.debug("- WARNING: Voltage regulator left motor low\n");    
00271         }
00272         else if(testing_voltage_regulators_flag)psi.debug("- Power good left motor v.reg\n");
00273     } 
00274     if(((gpio_byte0 & 0x02) >> 1) != power_good_motor_right){
00275         power_good_motor_right = (gpio_byte0 & 0x02) >> 1;
00276         if(!power_good_motor_right){
00277             if(testing_voltage_regulators_flag || SHOW_VR_WARNINGS)psi.debug("- WARNING: Voltage regulator right motor low\n");
00278         }
00279         else if(testing_voltage_regulators_flag)psi.debug("- Power good right motor v.reg\n");
00280     } 
00281     if(((gpio_byte0 & 0x04) >> 2) != power_good_infrared){
00282         power_good_infrared = (gpio_byte0 & 0x04) >> 2;
00283         if(!power_good_infrared){
00284             if(testing_voltage_regulators_flag || SHOW_VR_WARNINGS)psi.debug("- WARNING: Voltage regulator infrared low\n");
00285         }
00286         else if(testing_voltage_regulators_flag)psi.debug("- Power good infrared and aux v.reg\n");
00287     } 
00288     if(USE_LED4_FOR_VR_WARNINGS){
00289          mbed_led4 = (!power_good_motor_left || !power_good_motor_right || !power_good_infrared);
00290     }
00291     //Halt the system if settings flag is set and all v-regs are bad [usually this means robot is switched off!]
00292     if(HALT_ON_ALL_VREGS_LOW && !power_good_motor_left && !power_good_motor_right && !power_good_infrared){
00293         psi.debug("- PROGRAM HALTED.  Check that robot is switched on!\n");
00294         while(1){
00295              mbed_led1=1;
00296              mbed_led2=0;
00297              mbed_led3=1;
00298              mbed_led4=0;
00299              wait(0.25); 
00300              mbed_led1=0;
00301              mbed_led2=1;
00302              mbed_led3=0;
00303              mbed_led4=1;
00304              wait(0.25); 
00305         }
00306     }
00307 }
00308 
00309 void Setup::IF_parse_gpio_byte1(char byte)
00310 {
00311     gpio_byte1 = byte;
00312     //GPIO byte one contains the wheel encoders and the ID switch
00313     char current_id = ((gpio_byte1 & 0xF0)>> 4);
00314     if(user_id_set == 1) {
00315         if(robot_id != current_id) {
00316             previous_robot_id = robot_id;
00317             robot_id = current_id;
00318             event++;
00319             change_id_event = 1;
00320         }
00321     } else {
00322         robot_id = current_id;
00323         user_id_set = 1;
00324     }
00325     char current_encoder = (gpio_byte1 & 0x0F);
00326     if(wheel_enc_set == 1) {
00327         if(wheel_encoder_byte != current_encoder) {
00328             previous_wheel_encoder_byte = wheel_encoder_byte;
00329             wheel_encoder_byte = current_encoder;
00330             event++;
00331             encoder_event = 1;
00332         }
00333     } else {
00334         wheel_encoder_byte = current_encoder;
00335         wheel_enc_set = 1;
00336     }
00337 }
00338 
00339 void Setup::IF_handle_gpio_interrupt()
00340 {
00341     test = 1-test;
00342     if(USE_LED3_FOR_INTERRUPTS) mbed_led3 = test;
00343     IF_update_gpio_inputs();
00344 }
00345 
00346 char Setup::IF_is_switch_pressed()
00347 {
00348     //Read data
00349     char data[1];
00350     char command[1] = {0}; //Command to read from input port 0
00351     primary_i2c.write(GPIO_IC_ADDRESS,command,1,false);
00352     primary_i2c.read(GPIO_IC_ADDRESS,data,1,false);
00353     return (data[0] & 0x80);  //Returns a 1 if the center button is being pushed
00354 }
00355 
00356 
00357 char Setup::IF_get_switch_state()
00358 {
00359     //Read data
00360     char data[1];
00361     char command[1] = {0}; //Command to read from input port 0
00362     primary_i2c.write(GPIO_IC_ADDRESS,command,1,false);
00363     primary_i2c.read(GPIO_IC_ADDRESS,data,1,false);
00364     return (data[0] & 0xF8) >> 3;  //Returns the current switch state
00365 }
00366 
00367 void Setup::IF_update_gpio_inputs()
00368 {
00369     update_timeout.detach();
00370     //Read data
00371     char data[2];
00372     char command[1] = {0}; //Command to read from input port 0
00373     primary_i2c.write(GPIO_IC_ADDRESS,command,1,false);
00374     primary_i2c.read(GPIO_IC_ADDRESS,data,2,false);
00375     if(data[0]!=gpio_byte0) {
00376         IF_parse_gpio_byte0(data[0]);
00377     }
00378     if(data[1]!=gpio_byte1) {
00379         IF_parse_gpio_byte1(data[1]);
00380     }
00381     update_timeout.attach_us(this,&Setup::IF_update_gpio_inputs,50000);
00382 }
00383 
00384 
00385 void Setup::IF_write_to_led_ic(char byte_0, char byte_1)
00386 {
00387     //Set LEDs
00388     char data[3];
00389     data [0] = 0x02;    //Write to output port
00390     data [1] = byte_0;
00391     data [2] = byte_1;
00392     primary_i2c.write(LED_IC_ADDRESS,data,3,false);
00393 }
00394 
00395 
00396 void Setup::IF_setup_temperature_sensor()
00397 {
00398     char data[3];
00399     data[0] = 0x04; //Set critical temp limit
00400     data[1] = TEMPERATURE_CRITICAL_HI;
00401     data[2] = TEMPEARTURE_CRITICAL_LO;
00402     primary_i2c.write(TEMPERATURE_ADDRESS,data,3,false);
00403     data[0] = 0x02; //Set high temp limit
00404     data[1] = TEMPERATURE_HIGH_HI;
00405     data[2] = TEMPEARTURE_HIGH_LO;
00406     primary_i2c.write(TEMPERATURE_ADDRESS,data,3,false);
00407     data[0] = 0x03; //Set low temp limit
00408     data[1] = TEMPERATURE_LOW_HI;
00409     data[2] = TEMPEARTURE_LOW_LO;
00410     primary_i2c.write(TEMPERATURE_ADDRESS,data,3,false);
00411 }
00412 
00413 float Setup::IF_read_from_temperature_sensor()
00414 {
00415     char command[1] = {0x05};  //Write to Ta Register
00416     char data[3];
00417     signed int temp;
00418     float temperature;
00419     primary_i2c.write(TEMPERATURE_ADDRESS,command,1,false);
00420     primary_i2c.read(TEMPERATURE_ADDRESS,data,2,false);
00421 
00422     //Convert the temperature data
00423     //First Check flag bits
00424     char UpperByte = data[0];
00425     char LowerByte = data[1];
00426     if ((UpperByte & 0x80) == 0x80) {
00427         psi.debug("- WARNING: Temperature sensor reports critical temperature\n");
00428     }
00429     if ((UpperByte & 0x40) == 0x40) {
00430         psi.debug("- WARNING: Temperature sensor reports above upper limit\n");
00431     }
00432     if ((UpperByte & 0x20) == 0x20) {
00433         psi.debug("- WARNING: Temperature sensor reports below lower limit\n");
00434     }
00435     UpperByte = UpperByte & 0x1F;       //Clear flag bits
00436     if ((UpperByte & 0x10) == 0x10) {
00437         UpperByte = UpperByte & 0x0F;   //Clear SIGN
00438         temp = (UpperByte * 256) + LowerByte;
00439         temperature = - (temp / 16.0f);
00440     } else   {
00441         temp = (UpperByte * 256) + LowerByte;
00442         temperature = (temp / 16.0f);
00443     }
00444     return temperature;
00445 }