Code, robots and stuff

My minisumo for Robot SM is getting close to done. Here's a nice little GIF-image showing the assembly process of Perfected Titan.

. All the code can be found at my Gitweb.
I've also filmed a short video of Perfected Titan which you can see below. I'll probably add another video when I've "optimized" the target finding code a bit. It's a finite state machine which reads the front sensors and decides on state from that.
It has got seven states and seven "behaviours".
One thing I did differently with the code on this robot is that I did a function for setting the "heading" of the robot as opposed to setting individual motorspeeds. This gives you some interesting "effects" for free. For example; the "search"-behaviour is this;
set_heading(FULL_SPEED,(ad_value[0] - ad_value[1])*2);
Set_heading works by taking speed and then using the second argument, heading, to calculate individual motorspeeds. In this particular case it works great because if ad_value[1] is larger than ad_value[0] heading will be negative, and the robot will weer to the left, if it's positive it'll weer to the right.

void set_heading(int16_t speed, uint16_t difference) {

        // Set heading, left = speed + diff, right = speed - diff

        int left = speed + difference;

        left = constrain(255,-255,left);

        int right = speed - difference;

        right = constrain(255,-255,right);



The constrain() is a function that keeps a value between x, y. Nothing fancy. Set_motors() just sets motorspeed on a -255 to 255 scale, where 255 is full speed forward and -255 is full reverse.
Another interesting part of the code is the ADC-reading code. It doesn't do any Kalman filtering or anything, but it constantly reads the ADC-values in an interrupt, roughly 125kHz. It works like this

// ad_value is an array that contains all the ad values

volatile uint16_t ad_value[NR_AD_CHANNEL];

// Local values

volatile unsigned char ad_count=0;

void init_adc(void) {

        //5V AREF

        ADMUX |= (1<<REFS0);

        //AD enable, AD interrupt enable and clk/128 prescaling

        ADCSRA |= (<<ADEN) | (1<<ADIE) | (1<<ADPS2) | (1<<ADPS1) | (1<<ADPS0);


        // Prepares first ad channel

        ADMUX &= 0b11110000;

        // Disable digital on the ADC-pins

        DIDR0 = 0x01;

        //Starts the first conversion

        ADCSRA |= (1<<ADSC);








        // Select ADC channel with safety mask

        ADMUX = (ADMUX & 0b11110000) | ad_count;


        ADCSRA |= (1<<ADSC);


Neat huh?