Difference between revisions of "Kilobot Firmware"

From Kilobots
Jump to: navigation, search
(→‎Upgrade to Kilobot Firmware 2.0)
 
(5 intermediate revisions by the same user not shown)
Line 48: Line 48:
 
:     '''Solution 3:''' Low battery. Place the kilobot on a charging dock for a few minutes and retry
 
:     '''Solution 3:''' Low battery. Place the kilobot on a charging dock for a few minutes and retry
 
:     '''Solution 4:''' Damaged battery. Replace the kilobot battery
 
:     '''Solution 4:''' Damaged battery. Replace the kilobot battery
−
 
  
 
== Firmware 1.0 ==
 
== Firmware 1.0 ==
  
 
In case needed, here you can get the firmware 1.0 (from K-team): [[Media:KilobotFirstFirmware.hex|KilobotFirstFirmware.hex]]
 
In case needed, here you can get the firmware 1.0 (from K-team): [[Media:KilobotFirstFirmware.hex|KilobotFirstFirmware.hex]]
 +
 +
== Modified Kilobot Firmware 2.0 to filter out noise ==
 +
 +
At the Bristol Robotics Laboratory we have realised that some Kilobots go to SLEEPING or IDLE mode when running. We believe this happens due to 1) background IR noise that sometimes interferes with the Kilobots, and/or 2) if the message from a Kilobot gets corrupted. When this happens, the robots might interpret the signal/message as one coming from the overhead controller commanding them to go to SLEEPING or IDLE state.
 +
 +
We have modified Firmware 2.0 to filter out this noise by adding a counter for the number of consecutive times that the robot receives a message with type &ge; 128 (user messages must have a type < 128) in the RUNNING state. When it reaches a certain threshold (10 consecutive messages), it then processes the command. After this modification to the firmware, the Kilobots take a bit more time to process the signal from the controller when running, but none of them go the SLEEPING or IDLE while running.
 +
 +
The modified library can be found [https://github.com/Danixk/kilolib here]. Concretely, a new constant ''THRESHOLD_N_COMMANDS'' and variable ''n_commands_received'' have been added to ''kilolib.c'', and the ''process_message'' function has been properly adapted. The modified code is the following:
 +
 +
<nowiki>// Parameter for IR filter
 +
#define THRESHOLD_N_COMMANDS 10
 +
 +
// Number of consecutive commands received from the programmer (or noise)
 +
uint8_t n_commands_received;
 +
 +
static inline void process_message() {
 +
    AddressPointer_t reset = (AddressPointer_t)0x0000, bootload = (AddressPointer_t)0x7000;
 +
    calibmsg_t *calibmsg = (calibmsg_t*)&rx_msg.data;
 +
    if (rx_msg.type < BOOT) {
 +
        kilo_message_rx(&rx_msg, &rx_dist);
 +
       
 +
        n_commands_received = 0;
 +
        return;
 +
    }
 +
   
 +
    // Doesn't respond to other signals apart from neighbors while running if a few received
 +
    if(kilo_state == RUNNING && n_commands_received < THRESHOLD_N_COMMANDS){
 +
   
 +
        n_commands_received++;
 +
       
 +
        return;
 +
    }
 +
   
 +
    // In RUNNING state, it has to receive at least THREHOLD_N_COMMANDS continuous commands to react
 +
    else if( (kilo_state == RUNNING && n_commands_received == THRESHOLD_N_COMMANDS) || (kilo_state != RUNNING)){
 +
   
 +
        n_commands_received = 0;
 +
 +
        if (rx_msg.type != READUID && rx_msg.type != RUN && rx_msg.type != CALIB)
 +
            motors_off();
 +
        switch (rx_msg.type) {
 +
            case BOOT:
 +
                tx_timer_off();
 +
                bootload();
 +
                break;
 +
            case RESET:
 +
                reset();
 +
                break;
 +
            case SLEEP:
 +
                kilo_state = SLEEPING;
 +
                break;
 +
            case WAKEUP:
 +
                kilo_state = IDLE;
 +
                break;
 +
            case CHARGE:
 +
                kilo_state = CHARGING;
 +
                break;
 +
            case VOLTAGE:
 +
                kilo_state = BATTERY;
 +
                break;
 +
            case RUN:
 +
                if (kilo_state != SETUP && kilo_state != RUNNING) {
 +
                    motors_on();
 +
                    kilo_state = SETUP;
 +
                }
 +
                break;
 +
            case CALIB:
 +
                switch(calibmsg->mode) {
 +
                    case CALIB_SAVE:
 +
                        if (kilo_state == MOVING) {
 +
                            eeprom_write_byte(EEPROM_UID, kilo_uid&0xFF);
 +
                            eeprom_write_byte(EEPROM_UID+1, (kilo_uid>>8)&0xFF);
 +
                            eeprom_write_byte(EEPROM_LEFT_ROTATE, kilo_turn_left);
 +
                            eeprom_write_byte(EEPROM_RIGHT_ROTATE, kilo_turn_right);
 +
                            eeprom_write_byte(EEPROM_LEFT_STRAIGHT, kilo_straight_left);
 +
                            eeprom_write_byte(EEPROM_RIGHT_STRAIGHT, kilo_straight_right);
 +
                            motors_off();
 +
                            kilo_state = IDLE;
 +
                        }
 +
                        break;
 +
                    case CALIB_UID:
 +
                        kilo_uid = calibmsg->uid;
 +
                        cur_motion = MOVE_STOP;
 +
                        break;
 +
                    case CALIB_TURN_LEFT:
 +
                        if (cur_motion != MOVE_LEFT || kilo_turn_left != calibmsg->turn_left) {
 +
                            prev_motion = MOVE_STOP;
 +
                            cur_motion = MOVE_LEFT;
 +
                            kilo_turn_left = calibmsg->turn_left;
 +
                        }
 +
                        break;
 +
                    case CALIB_TURN_RIGHT:
 +
                        if (cur_motion != MOVE_RIGHT || kilo_turn_right != calibmsg->turn_right) {
 +
                            prev_motion = MOVE_STOP;
 +
                            cur_motion = MOVE_RIGHT;
 +
                            kilo_turn_right = calibmsg->turn_right;
 +
                        }
 +
                        break;
 +
                    case CALIB_STRAIGHT:
 +
                        if (cur_motion != MOVE_STRAIGHT || kilo_straight_right != calibmsg->straight_right || kilo_straight_left != calibmsg->straight_left) {
 +
                            prev_motion = MOVE_STOP;
 +
                            cur_motion = MOVE_STRAIGHT;
 +
                            kilo_straight_left = calibmsg->straight_left;
 +
                            kilo_straight_right = calibmsg->straight_right;
 +
                        }
 +
                        break;
 +
                }
 +
                if (calibmsg->mode != CALIB_SAVE && kilo_state != MOVING) {
 +
                    motors_on();
 +
                    kilo_state = MOVING;
 +
                }
 +
                break;
 +
            case READUID:
 +
                if (kilo_state != MOVING) {
 +
                    motors_on();
 +
                    set_color(RGB(0,0,0));
 +
                    prev_motion = cur_motion = MOVE_STOP;
 +
                    kilo_state = MOVING;
 +
                }
 +
 +
                if (kilo_uid&(1<<rx_msg.data[0]))
 +
                    cur_motion = MOVE_LEFT;
 +
                else
 +
                    cur_motion = MOVE_STOP;
 +
                break;
 +
            default:
 +
                break;
 +
        }
 +
    }
 +
}</nowiki>
 +
 +
== Modified Kilobot Firmware 2.0 to protect the battery ==
 +
 +
At the Bristol Robotics Laboratory we have realised that Firmware 2.0 doesn't have any battery protection. This means that if Kilobots are left switched on indefinitely, their batteries will drain until the point when they can't be recharged anymore and a new battery is needed. To solve this issue, a hardware solution would be ideal. However, this would require remanufacturing the robots, which might be time consuming. Therefore, we have modified Firmware 2.0 to add a software solution to this. The modified library can be found [https://github.com/Danixk/kilolib here].
 +
 +
The solution we have implemented consists of the robots estimating the voltage frequently during the RUNNING, IDLE and SLEEPING state. If they measure a low voltage value (3.2V) for a certain number of consecutive times, they will switch to the SLEEPING state, i.e., to low-power mode to stop running and save battery.  Below is a detailed explanation of all the changes.
 +
 +
The following constants and variables have been added to ''kilolib.c'':
 +
 +
<nowiki>// Parameters for battery protection
 +
#define N_READINGS_VOLTAGE 10
 +
#define N_REPETITIONS_LOW_VOLTAGE 5
 +
#define LOW_VOLTAGE_THRESHOLD 547      // Equivalent to 3.2V
 +
#define HIGH_VOLTAGE_THRESHOLD 716      // Equivalent to 4.2V
 +
 +
int16_t voltage = -1;                  // Voltage estimation
 +
uint8_t is_in_low_voltage = 0;          // Number of consecutive times it reads a voltage below the low-voltage threshold
 +
uint8_t counter_ticks_for_voltage = 0;  // To estimate voltage every 255 ticks (~8 seconds)</nowiki>
 +
 +
The Kilobots estimate the voltage approximately every 8 seconds in the RUNINNG state (less time might interfere with distance estimation) using a 1-byte counter that is increased every kilo_tick (255 kilo_ticks =~ 8 seconds), constantly in the IDLE state, and every time they wake up in the SLEEPING state. The voltage isn't estimated in the BATTERY state or the rest (SETUP, CHARGING, MOVING) because the user is supposed to be supervising or interacting with the robots. To increase ''counter_ticks_for_voltage'' every kilo_ticks, the Timer0 interrupt function has been modified as follows:
 +
 +
<nowiki>/**
 +
* Timer0 interrupt.
 +
* Used to send messages every kilo_tx_period ticks.
 +
*/
 +
ISR(TIMER0_COMPA_vect) {
 +
    tx_clock += tx_increment;
 +
    tx_increment = 0xFF;
 +
    OCR0A = tx_increment;
 +
    kilo_ticks++;
 +
 +
    // Increments counter_ticks_for_voltage by 1 tick until 255
 +
    if(kilo_state == RUNNING && counter_ticks_for_voltage < 255)
 +
        counter_ticks_for_voltage++;
 +
 +
    if(!rx_busy && tx_clock>kilo_tx_period && kilo_state == RUNNING) {
 +
        message_t *msg = kilo_message_tx();
 +
        if (msg) {
 +
            if (message_send(msg)) {
 +
                kilo_message_tx_success();
 +
                tx_clock = 0;
 +
            } else {
 +
                tx_increment = rand()&0xFF;
 +
                OCR0A = tx_increment;
 +
            }
 +
        }
 +
    }
 +
}</nowiki>
 +
 +
To estimate the voltage, a new function ''estimate_voltage'' has been added, accepting the binary parameter ''trigger_high_gain'' as input. This function takes ''N_READINGS_VOLTAGE'' consecutive measurements of the voltage, and returns the maximum of them. This acts as a filter for measurements of -1 from ''get_voltage()'' but also as a filter for the first voltage reading in case the last time this function was executed with ''trigger_high_gain'' == 1. The reason is that most of the functions using the analog-to-digital converter (''get_ambientlight'', ''get_temperature'', ''rand_hard'' and ''estimate_distance'') need to measure high gain to give a correct value (that is why they execute ''adc_trigger_high_gain()'' at the end). However, ''get_voltage()'' gives a wrong value of the voltage if measuring high gain (that is why ''adc_trigger_high_gain()'' is commented out at the end). In the case of the RUNNING state, the user might call any of the functions using the analog-to-digital converter that need to measure high gain, so those functions will leave high gain triggered. If voltage is then read, the value will be wrong. However, next time the voltage is read, the value will be correct as long as none of the other functions is executed in between. To then allow the other functions to give correct values, high gain must be triggered after the last reading of the voltage in the new ''estimate_voltage'' function. However, we are only interested in this in the RUNNING state, because in the other states where the voltage is estimated (SLEEPING and IDLE), the functions using the analog-to-digital converter aren't used. Therefore, ''estimate_voltage(1)'' is executed when estimating the voltage during the RUNNING state, and ''estimate_voltage(0)'' in SLEEPING and IDLE. The code for ''estimate_voltage'' function is the following:
 +
 +
<nowiki>int16_t estimate_voltage(uint8_t trigger_high_gain){
 +
 +
    uint8_t i;
 +
    int16_t voltage;
 +
    int16_t estimated_voltage = -1;
 +
 +
    for(i = 0; i < N_READINGS_VOLTAGE; i++){
 +
 +
        voltage = get_voltage();
 +
   
 +
        if(voltage <= HIGH_VOLTAGE_THRESHOLD && voltage > estimated_voltage){
 +
         
 +
            estimated_voltage = voltage;
 +
         
 +
        }
 +
    }
 +
 +
    if(trigger_high_gain){
 +
        cli();
 +
        adc_trigger_high_gain();
 +
        sei();
 +
    }
 +
 +
    return estimated_voltage;
 +
}</nowiki>
 +
 +
Finally, the ''kilo_start'' function had to be modified to include voltage estimation. Furthermore, the LED colour flashed in the SLEEPING and IDLE states has been slightly modified to show the corresponding colour of the battery level (same scale as in the BATTERY state). This allows easy identification of the level of charge of the battery without sending them to the BATTERY state (particularly useful when sleeping, because they don't have to be waken up to know how much battery they have got left). In case they switch to SLEEPING state due to low battery (or because they detect low battery while sleeping), they will flash red for 1 second to signal their battery is in danger. The modified ''kilo_start'' function is the following:
 +
 +
<nowiki>void kilo_start(void (*setup)(void), void (*loop)(void)) {
 +
 +
    uint8_t has_setup = 0;
 +
 +
    while (1) {
 +
 +
        switch(kilo_state) {
 +
            case SLEEPING:
 +
 +
                cli();
 +
                acomp_off();
 +
                adc_off();
 +
                ports_off();
 +
                wdt_enable(WDTO_8S);
 +
                WDTCSR |= (1<<WDIE);
 +
                set_sleep_mode(SLEEP_MODE_PWR_DOWN);
 +
                cli();
 +
                sleep_enable();
 +
                sei();
 +
                sleep_cpu();
 +
                sleep_disable();
 +
                sei();
 +
                rx_busy = 0;
 +
                ports_on();
 +
                adc_on();
 +
                _delay_us(300);
 +
                acomp_on();
 +
 +
                if(is_in_low_voltage == N_REPETITIONS_LOW_VOLTAGE){ // Low battery
 +
 +
                    set_color(RGB(3,0,0));
 +
 +
                    _delay_ms(1000);
 +
                    set_color(RGB(0,0,0));
 +
   
 +
                }
 +
                else{
 +
 +
                    voltage = estimate_voltage(0);
 +
 +
                    if(voltage != -1 && voltage < LOW_VOLTAGE_THRESHOLD){
 +
 +
                        is_in_low_voltage++;
 +
 +
                    }
 +
                    else if(voltage != -1){
 +
 +
                        is_in_low_voltage = 0;
 +
                       
 +
                    }
 +
 +
                    if(voltage > 682)
 +
                        set_color(RGB(0,3,0));
 +
                    else if(voltage > 648)
 +
                        set_color(RGB(0,0,3));
 +
                    else if(voltage > 614)
 +
                        set_color(RGB(3,3,0));
 +
                    else if(voltage != -1)
 +
                        set_color(RGB(3,0,0));
 +
                    else
 +
                        set_color(RGB(3,3,3));
 +
 +
                    _delay_ms(10);
 +
                    if (rx_busy) {
 +
                        set_color(RGB(0,3,0));
 +
                        _delay_ms(100);
 +
                    }
 +
                    set_color(RGB(0,0,0));
 +
                }
 +
 +
                break;
 +
            case IDLE:
 +
 +
                if(rx_busy){
 +
 +
                    set_color(RGB(0,3,0));
 +
                    _delay_ms(1);
 +
                    set_color(RGB(0,0,0));
 +
                    _delay_ms(200);
 +
 +
                }
 +
 +
                else if(is_in_low_voltage < N_REPETITIONS_LOW_VOLTAGE){
 +
 +
                    voltage = estimate_voltage(0);
 +
 +
                    if(voltage != -1 && voltage < LOW_VOLTAGE_THRESHOLD){
 +
 +
                        is_in_low_voltage++;
 +
 +
                    }
 +
                    else if(voltage != -1){
 +
 +
                        is_in_low_voltage = 0;
 +
                       
 +
                    }
 +
 +
                    if(voltage > 682)
 +
                        set_color(RGB(0,3,0));
 +
                    else if(voltage > 648)
 +
                        set_color(RGB(0,0,3));
 +
                    else if(voltage > 614)
 +
                        set_color(RGB(3,3,0));
 +
                    else if(voltage != -1)
 +
                        set_color(RGB(3,0,0));
 +
                    else
 +
                        set_color(RGB(3,3,3));
 +
 +
 +
                    _delay_ms(1);
 +
                    set_color(RGB(0,0,0));
 +
                    _delay_ms(200);
 +
 +
                }
 +
                else{
 +
                    kilo_state = SLEEPING;
 +
                }
 +
 +
                break;
 +
            case BATTERY:
 +
                voltage = get_voltage();
 +
                is_in_low_voltage = 0;
 +
 +
                if(voltage > 682)
 +
                    set_color(RGB(0,3,0));
 +
                else if(voltage > 648)
 +
                    set_color(RGB(0,0,3));
 +
                else if(voltage > 614)
 +
                    set_color(RGB(3,3,0));
 +
                else
 +
                    set_color(RGB(3,0,0));
 +
 +
                break;
 +
            case CHARGING:
 +
                is_in_low_voltage = 0;
 +
                if (is_charging()) {
 +
                    set_color(RGB(1,0,0));
 +
                    _delay_ms(1);
 +
                    set_color(RGB(0,0,0));
 +
                    _delay_ms(200);
 +
                } else
 +
                    set_color(RGB(0,0,0));
 +
 +
                break;
 +
            case SETUP:
 +
                if (!has_setup) {
 +
                    setup();
 +
                    has_setup = 1;
 +
                }
 +
                is_in_low_voltage = 0;
 +
                kilo_state = RUNNING;
 +
            case RUNNING:
 +
 +
                if(rx_busy){
 +
 +
                    loop();
 +
 +
                }
 +
 +
                else if(is_in_low_voltage < N_REPETITIONS_LOW_VOLTAGE){
 +
 +
                    // Estimate voltage every 8 seconds, approximately
 +
                    cli(); // Disable interrupts to read or write variable counter_ticks_for_voltage
 +
                    if(counter_ticks_for_voltage == 255){
 +
 +
                        counter_ticks_for_voltage = 0;
 +
                        sei(); // Enable interrupts
 +
 +
                        voltage = estimate_voltage(1);
 +
 +
                        if(voltage != -1 && voltage < LOW_VOLTAGE_THRESHOLD){
 +
 +
                            is_in_low_voltage++;
 +
 +
                        }
 +
                        else if(voltage != -1){
 +
 +
                            is_in_low_voltage = 0;
 +
                           
 +
                        }
 +
                    }
 +
                    else
 +
                        sei(); // Enable interrupts
 +
 +
                    loop();
 +
 +
                }
 +
                else{
 +
                    kilo_state = SLEEPING;
 +
                }
 +
 +
 +
                break;
 +
            case MOVING:
 +
                is_in_low_voltage = 0;
 +
                if (cur_motion == MOVE_STOP) {
 +
                    set_motors(0,0);
 +
                    prev_motion = MOVE_STOP;
 +
                } else {
 +
                    if (cur_motion != prev_motion) {
 +
                        prev_motion = cur_motion;
 +
                        if (cur_motion == MOVE_LEFT) {
 +
                            set_motors(0xFF, 0);
 +
                            _delay_ms(15);
 +
                            set_motors(kilo_turn_left, 0);
 +
                        } else if (cur_motion == MOVE_RIGHT) {
 +
                            set_motors(0, 0xFF);
 +
                            _delay_ms(15);
 +
                            set_motors(0, kilo_turn_right);
 +
                        } else {
 +
                            set_motors(0, 0xFF);
 +
                            set_motors(0xFF, 0xFF);
 +
                            _delay_ms(15);
 +
                            set_motors(kilo_straight_left, kilo_straight_right);
 +
                        }
 +
                    }
 +
                }
 +
                break;
 +
        }
 +
    }
 +
}</nowiki>
 +
 +
So far, all tests at the Bristol Robotics Laboratory have been successful in terms of protecting the Kilobots by sending them to sleep when the battery is low. In case of faulty/bad batteries, robots might go to sleep well before the voltage has dropped. However, this tells the battery is indeed faulty. We believe this protection mechanism is worth having even if some robots are sent to sleep prematurely, as it avoids the loss of batteries. More tests from other teams would be very valuable.
  
 
== Links ==  
 
== Links ==  

Latest revision as of 09:29, 20 June 2019

We recommend to use the firmware 2.0, which has a more stable code, more supported communication interface and allows working with Linux and MacOS. Additionally, most of the tools provided in this website require the firmware 2.0 to work properly.

Upgrade to Kilobot Firmware 2.0

Most of the information reported here below come from (and can be found also on) the kilobotics website (https://www.kilobotics.com/documentation).


NOTE:
You will only need to do this once. If you already have the latest controller firmware and bootlader installed, then you can skip the next steps.


We recommend doing the firmware upgrade from a Linux machine.

Here below, the steps to upgrade the kilobot firmware:

1. Setup of your Linux machine installing avrdude. For Ubuntu, use the following command:
sudo apt-get install avrdude
2. If it's not done yet, update the firmware of the Overhead Controller (OHC) (see OHC Firmware for details)
3. Download the file bootloader.hex
4. Set the OHC jumper to "external" mode as in this figure:
OHC jumper in external mode. Image source: http://ftp.k-team.com/kilobot/user_manual/Kilobot_UserManual.pdf
5. Turn on the kilobot by adding the power jumper:
Turn on the kilobot by adding the power jumper. Image source: https://www.kilobotics.com/documentation
6. Connect the OHC to the PC through Usb, and plug the cable connecting the OHC to the kilobot as in this figure:
Connect the ICSP programming cable to the robot. Image source: https://www.kilobotics.com/documentation
Gently press and tilt the programming cable to ensure a good connection.
7. Run (from the directory containing the bootloader file) the update command:
sudo avrdude -p m328p -P usb -c avrispmkII -U "flash:w:bootloader.hex:i"
8. If you succeed, the robot should be happy and start vibrating in your hands the terminal message should say: Fuses OK. avrdude done. Thank you. Similarly to this screenshot.

Troubleshooting

1. Problem: The kilobot (that you're trying to upgrade) is not detected by the avrdude as connected through USB.
    Solution 1: Bad connection. Check that the OHC jumper is in the correct position
    Solution 2: Software failure. Try to reset the kilobot by removing and placing back its jumper
    Solution 3: Low battery. Place the kilobot on a charging dock for a few minutes and retry
    Solution 4: Damaged battery. Replace the kilobot battery

Firmware 1.0

In case needed, here you can get the firmware 1.0 (from K-team): KilobotFirstFirmware.hex

Modified Kilobot Firmware 2.0 to filter out noise

At the Bristol Robotics Laboratory we have realised that some Kilobots go to SLEEPING or IDLE mode when running. We believe this happens due to 1) background IR noise that sometimes interferes with the Kilobots, and/or 2) if the message from a Kilobot gets corrupted. When this happens, the robots might interpret the signal/message as one coming from the overhead controller commanding them to go to SLEEPING or IDLE state.

We have modified Firmware 2.0 to filter out this noise by adding a counter for the number of consecutive times that the robot receives a message with type ≥ 128 (user messages must have a type < 128) in the RUNNING state. When it reaches a certain threshold (10 consecutive messages), it then processes the command. After this modification to the firmware, the Kilobots take a bit more time to process the signal from the controller when running, but none of them go the SLEEPING or IDLE while running.

The modified library can be found here. Concretely, a new constant THRESHOLD_N_COMMANDS and variable n_commands_received have been added to kilolib.c, and the process_message function has been properly adapted. The modified code is the following: 

// Parameter for IR filter
#define THRESHOLD_N_COMMANDS 10

// Number of consecutive commands received from the programmer (or noise)
uint8_t n_commands_received;

static inline void process_message() {
    AddressPointer_t reset = (AddressPointer_t)0x0000, bootload = (AddressPointer_t)0x7000;
    calibmsg_t *calibmsg = (calibmsg_t*)&rx_msg.data;
    if (rx_msg.type < BOOT) {
        kilo_message_rx(&rx_msg, &rx_dist);
        
        n_commands_received = 0;
        return;
    }
    
    // Doesn't respond to other signals apart from neighbors while running if a few received
    if(kilo_state == RUNNING && n_commands_received < THRESHOLD_N_COMMANDS){
    
        n_commands_received++;
        
        return;
    }
    
    // In RUNNING state, it has to receive at least THREHOLD_N_COMMANDS continuous commands to react
    else if( (kilo_state == RUNNING && n_commands_received == THRESHOLD_N_COMMANDS) || (kilo_state != RUNNING)){
    
        n_commands_received = 0;

        if (rx_msg.type != READUID && rx_msg.type != RUN && rx_msg.type != CALIB)
            motors_off();
        switch (rx_msg.type) {
            case BOOT:
                tx_timer_off();
                bootload();
                break;
            case RESET:
                reset();
                break;
            case SLEEP:
                kilo_state = SLEEPING;
                break;
            case WAKEUP:
                kilo_state = IDLE;
                break;
            case CHARGE:
                kilo_state = CHARGING;
                break;
            case VOLTAGE:
                kilo_state = BATTERY;
                break;
            case RUN:
                if (kilo_state != SETUP && kilo_state != RUNNING) {
                    motors_on();
                    kilo_state = SETUP;
                }
                break;
            case CALIB:
                switch(calibmsg->mode) {
                    case CALIB_SAVE:
                        if (kilo_state == MOVING) {
                            eeprom_write_byte(EEPROM_UID, kilo_uid&0xFF);
                            eeprom_write_byte(EEPROM_UID+1, (kilo_uid>>8)&0xFF);
                            eeprom_write_byte(EEPROM_LEFT_ROTATE, kilo_turn_left);
                            eeprom_write_byte(EEPROM_RIGHT_ROTATE, kilo_turn_right);
                            eeprom_write_byte(EEPROM_LEFT_STRAIGHT, kilo_straight_left);
                            eeprom_write_byte(EEPROM_RIGHT_STRAIGHT, kilo_straight_right);
                            motors_off();
                            kilo_state = IDLE;
                        }
                        break;
                    case CALIB_UID:
                        kilo_uid = calibmsg->uid;
                        cur_motion = MOVE_STOP;
                        break;
                    case CALIB_TURN_LEFT:
                        if (cur_motion != MOVE_LEFT || kilo_turn_left != calibmsg->turn_left) {
                            prev_motion = MOVE_STOP;
                            cur_motion = MOVE_LEFT;
                            kilo_turn_left = calibmsg->turn_left;
                        }
                        break;
                    case CALIB_TURN_RIGHT:
                        if (cur_motion != MOVE_RIGHT || kilo_turn_right != calibmsg->turn_right) {
                            prev_motion = MOVE_STOP;
                            cur_motion = MOVE_RIGHT;
                            kilo_turn_right = calibmsg->turn_right;
                        }
                        break;
                    case CALIB_STRAIGHT:
                        if (cur_motion != MOVE_STRAIGHT || kilo_straight_right != calibmsg->straight_right || kilo_straight_left != calibmsg->straight_left) {
                            prev_motion = MOVE_STOP;
                            cur_motion = MOVE_STRAIGHT;
                            kilo_straight_left = calibmsg->straight_left;
                            kilo_straight_right = calibmsg->straight_right;
                        }
                        break;
                }
                if (calibmsg->mode != CALIB_SAVE && kilo_state != MOVING) {
                    motors_on();
                    kilo_state = MOVING;
                }
                break;
            case READUID:
                if (kilo_state != MOVING) {
                    motors_on();
                    set_color(RGB(0,0,0));
                    prev_motion = cur_motion = MOVE_STOP;
                    kilo_state = MOVING;
                }

                if (kilo_uid&(1<<rx_msg.data[0]))
                    cur_motion = MOVE_LEFT;
                else
                    cur_motion = MOVE_STOP;
                break;
            default:
                break;
        }
    }
}

Modified Kilobot Firmware 2.0 to protect the battery

At the Bristol Robotics Laboratory we have realised that Firmware 2.0 doesn't have any battery protection. This means that if Kilobots are left switched on indefinitely, their batteries will drain until the point when they can't be recharged anymore and a new battery is needed. To solve this issue, a hardware solution would be ideal. However, this would require remanufacturing the robots, which might be time consuming. Therefore, we have modified Firmware 2.0 to add a software solution to this. The modified library can be found here.

The solution we have implemented consists of the robots estimating the voltage frequently during the RUNNING, IDLE and SLEEPING state. If they measure a low voltage value (3.2V) for a certain number of consecutive times, they will switch to the SLEEPING state, i.e., to low-power mode to stop running and save battery. Below is a detailed explanation of all the changes.

The following constants and variables have been added to kilolib.c: 

// Parameters for battery protection
#define N_READINGS_VOLTAGE 10
#define N_REPETITIONS_LOW_VOLTAGE 5
#define LOW_VOLTAGE_THRESHOLD 547       // Equivalent to 3.2V
#define HIGH_VOLTAGE_THRESHOLD 716      // Equivalent to 4.2V

int16_t voltage = -1;                   // Voltage estimation
uint8_t is_in_low_voltage = 0;          // Number of consecutive times it reads a voltage below the low-voltage threshold
uint8_t counter_ticks_for_voltage = 0;  // To estimate voltage every 255 ticks (~8 seconds)

The Kilobots estimate the voltage approximately every 8 seconds in the RUNINNG state (less time might interfere with distance estimation) using a 1-byte counter that is increased every kilo_tick (255 kilo_ticks =~ 8 seconds), constantly in the IDLE state, and every time they wake up in the SLEEPING state. The voltage isn't estimated in the BATTERY state or the rest (SETUP, CHARGING, MOVING) because the user is supposed to be supervising or interacting with the robots. To increase counter_ticks_for_voltage every kilo_ticks, the Timer0 interrupt function has been modified as follows: 

/**
 * Timer0 interrupt.
 * Used to send messages every kilo_tx_period ticks.
 */
ISR(TIMER0_COMPA_vect) {
    tx_clock += tx_increment;
    tx_increment = 0xFF;
    OCR0A = tx_increment;
    kilo_ticks++;

    // Increments counter_ticks_for_voltage by 1 tick until 255
    if(kilo_state == RUNNING && counter_ticks_for_voltage < 255)
        counter_ticks_for_voltage++;

    if(!rx_busy && tx_clock>kilo_tx_period && kilo_state == RUNNING) {
        message_t *msg = kilo_message_tx();
        if (msg) {
            if (message_send(msg)) {
                kilo_message_tx_success();
                tx_clock = 0;
            } else {
                tx_increment = rand()&0xFF;
                OCR0A = tx_increment;
            }
        }
    }
}

To estimate the voltage, a new function estimate_voltage has been added, accepting the binary parameter trigger_high_gain as input. This function takes N_READINGS_VOLTAGE consecutive measurements of the voltage, and returns the maximum of them. This acts as a filter for measurements of -1 from get_voltage() but also as a filter for the first voltage reading in case the last time this function was executed with trigger_high_gain == 1. The reason is that most of the functions using the analog-to-digital converter (get_ambientlight, get_temperature, rand_hard and estimate_distance) need to measure high gain to give a correct value (that is why they execute adc_trigger_high_gain() at the end). However, get_voltage() gives a wrong value of the voltage if measuring high gain (that is why adc_trigger_high_gain() is commented out at the end). In the case of the RUNNING state, the user might call any of the functions using the analog-to-digital converter that need to measure high gain, so those functions will leave high gain triggered. If voltage is then read, the value will be wrong. However, next time the voltage is read, the value will be correct as long as none of the other functions is executed in between. To then allow the other functions to give correct values, high gain must be triggered after the last reading of the voltage in the new estimate_voltage function. However, we are only interested in this in the RUNNING state, because in the other states where the voltage is estimated (SLEEPING and IDLE), the functions using the analog-to-digital converter aren't used. Therefore, estimate_voltage(1) is executed when estimating the voltage during the RUNNING state, and estimate_voltage(0) in SLEEPING and IDLE. The code for estimate_voltage function is the following: 

int16_t estimate_voltage(uint8_t trigger_high_gain){

    uint8_t i;
    int16_t voltage;
    int16_t estimated_voltage = -1;

    for(i = 0; i < N_READINGS_VOLTAGE; i++){

        voltage = get_voltage();
    
        if(voltage <= HIGH_VOLTAGE_THRESHOLD && voltage > estimated_voltage){
          
            estimated_voltage = voltage;
          
        }
    }

    if(trigger_high_gain){
        cli();
        adc_trigger_high_gain();
        sei();
    }

    return estimated_voltage;
}

Finally, the kilo_start function had to be modified to include voltage estimation. Furthermore, the LED colour flashed in the SLEEPING and IDLE states has been slightly modified to show the corresponding colour of the battery level (same scale as in the BATTERY state). This allows easy identification of the level of charge of the battery without sending them to the BATTERY state (particularly useful when sleeping, because they don't have to be waken up to know how much battery they have got left). In case they switch to SLEEPING state due to low battery (or because they detect low battery while sleeping), they will flash red for 1 second to signal their battery is in danger. The modified kilo_start function is the following: 

void kilo_start(void (*setup)(void), void (*loop)(void)) {

    uint8_t has_setup = 0;

    while (1) {

        switch(kilo_state) {
            case SLEEPING:

                cli();
                acomp_off();
                adc_off();
                ports_off();
                wdt_enable(WDTO_8S);
                WDTCSR |= (1<<WDIE);
                set_sleep_mode(SLEEP_MODE_PWR_DOWN);
                cli();
                sleep_enable();
                sei();
                sleep_cpu();
                sleep_disable();
                sei();
                rx_busy = 0;
                ports_on();
                adc_on();
                _delay_us(300);
                acomp_on();

                if(is_in_low_voltage == N_REPETITIONS_LOW_VOLTAGE){ // Low battery

                    set_color(RGB(3,0,0));

                    _delay_ms(1000);
                    set_color(RGB(0,0,0));
    
                }
                else{

                    voltage = estimate_voltage(0);

                    if(voltage != -1 && voltage < LOW_VOLTAGE_THRESHOLD){

                        is_in_low_voltage++;

                    }
                    else if(voltage != -1){

                        is_in_low_voltage = 0;
                        
                    }

                    if(voltage > 682)
                        set_color(RGB(0,3,0));
                    else if(voltage > 648)
                        set_color(RGB(0,0,3));
                    else if(voltage > 614)
                        set_color(RGB(3,3,0));
                    else if(voltage != -1)
                        set_color(RGB(3,0,0));
                    else
                        set_color(RGB(3,3,3));

                    _delay_ms(10);
                    if (rx_busy) {
                        set_color(RGB(0,3,0));
                        _delay_ms(100);
                    }
                    set_color(RGB(0,0,0));
                }

                break;
            case IDLE:

                if(rx_busy){

                    set_color(RGB(0,3,0));
                    _delay_ms(1);
                    set_color(RGB(0,0,0));
                    _delay_ms(200);

                }

                else if(is_in_low_voltage < N_REPETITIONS_LOW_VOLTAGE){

                    voltage = estimate_voltage(0);

                    if(voltage != -1 && voltage < LOW_VOLTAGE_THRESHOLD){

                        is_in_low_voltage++;

                    }
                    else if(voltage != -1){

                        is_in_low_voltage = 0;
                        
                    }

                    if(voltage > 682)
                        set_color(RGB(0,3,0));
                    else if(voltage > 648)
                        set_color(RGB(0,0,3));
                    else if(voltage > 614)
                        set_color(RGB(3,3,0));
                    else if(voltage != -1)
                        set_color(RGB(3,0,0));
                    else
                        set_color(RGB(3,3,3));


                    _delay_ms(1);
                    set_color(RGB(0,0,0));
                    _delay_ms(200);

                }
                else{
                    kilo_state = SLEEPING;
                }

                break;
            case BATTERY:
                voltage = get_voltage();
                is_in_low_voltage = 0;

                if(voltage > 682)
                    set_color(RGB(0,3,0));
                else if(voltage > 648)
                    set_color(RGB(0,0,3));
                else if(voltage > 614)
                    set_color(RGB(3,3,0));
                else
                    set_color(RGB(3,0,0));

                break;
            case CHARGING:
                is_in_low_voltage = 0;
                if (is_charging()) {
                    set_color(RGB(1,0,0));
                    _delay_ms(1);
                    set_color(RGB(0,0,0));
                    _delay_ms(200);
                } else
                    set_color(RGB(0,0,0));

                break;
            case SETUP:
                if (!has_setup) {
                    setup();
                    has_setup = 1;
                }
                is_in_low_voltage = 0;
                kilo_state = RUNNING;
            case RUNNING:

                if(rx_busy){

                    loop();

                }

                else if(is_in_low_voltage < N_REPETITIONS_LOW_VOLTAGE){

                    // Estimate voltage every 8 seconds, approximately
                    cli(); // Disable interrupts to read or write variable counter_ticks_for_voltage
                    if(counter_ticks_for_voltage == 255){

                        counter_ticks_for_voltage = 0;
                        sei(); // Enable interrupts

                        voltage = estimate_voltage(1);

                        if(voltage != -1 && voltage < LOW_VOLTAGE_THRESHOLD){

                            is_in_low_voltage++;

                        }
                        else if(voltage != -1){

                            is_in_low_voltage = 0;
                            
                        }
                    }
                    else
                        sei(); // Enable interrupts

                    loop();

                }
                else{
                    kilo_state = SLEEPING;
                }


                break;
            case MOVING:
                is_in_low_voltage = 0;
                if (cur_motion == MOVE_STOP) {
                    set_motors(0,0);
                    prev_motion = MOVE_STOP;
                } else {
                    if (cur_motion != prev_motion) {
                        prev_motion = cur_motion;
                        if (cur_motion == MOVE_LEFT) {
                            set_motors(0xFF, 0);
                            _delay_ms(15);
                            set_motors(kilo_turn_left, 0);
                        } else if (cur_motion == MOVE_RIGHT) {
                            set_motors(0, 0xFF);
                            _delay_ms(15);
                            set_motors(0, kilo_turn_right);
                        } else {
                            set_motors(0, 0xFF);
                            set_motors(0xFF, 0xFF);
                            _delay_ms(15);
                            set_motors(kilo_straight_left, kilo_straight_right);
                        }
                    }
                }
                break;
        }
    }
}

So far, all tests at the Bristol Robotics Laboratory have been successful in terms of protecting the Kilobots by sending them to sleep when the battery is low. In case of faulty/bad batteries, robots might go to sleep well before the voltage has dropped. However, this tells the battery is indeed faulty. We believe this protection mechanism is worth having even if some robots are sent to sleep prematurely, as it avoids the loss of batteries. More tests from other teams would be very valuable.

Links

For further information, check the kilobotics website: https://www.kilobotics.com/documentation

The source code of the firmware (both of OHC and kilobots) can be found here: https://github.com/acornejo/kilolib

Retrieved from "http://giovannireina.com/kilowiki/index.php?title=Kilobot_Firmware&oldid=188"