robotiklubi kood

#include <avr/io.h>
#include "drivers/board.h"
#include "drivers/adc.h"
#include "drivers/motor.h"
#include "drivers/com.h"
#include "drivers/gyro.h"

#define base_speed 500
#define P_gain 45
#define D_gain 20

int main(void)
{
    int i, check, Perror, kaalud[] = {3, 2, 1, -1, -2, -3}, speedL, speedR;
    int square_flag, Derror, last_error = 0;
    clock_init();       // Seadista süsteemi kell 32MHz peale
    board_init();       // Seadista LED ja nupud
    adc_init();         // Seadista ADC kanal 0
    radio_init(57600);  // Seadista raadiomooduli UART
    motor_init();       // Seadista mootorikontroller
    while(!sw1_read());
    _delay_ms(1000);

    while(1)
    {
        // andurid 0 kuni 5 = 6 tk

        for(i = 0, Perror = 0, check = 0; i < 6; i++)
        {
            if (adc_read(i) > 1700) // näeb musta(joont)
            {
                Perror += kaalud[i];
                check = 1;
                if (i == 0) {
                    square_flag = 0;
                }
                if (i == 5) {
                    square_flag = 1;
                }
            }

        }
        if (check) // kui näeb joont vähemalt ühe anduriga
        {
            Derror = Perror - last_error;
            last_error = Perror;
            Perror *= P_gain;
            Perror += Derror * D_gain;
            if (Perror < 0)
            {
                if ((base_speed - Perror) > 1000)
                {
                    speedR = 1000;
                }
                else
                    speedR = (base_speed - Perror);

                if(base_speed + Perror < 0)
                {
                    speedL = 0;
                }
                else
                    speedL = base_speed + Perror;
            }

            else
            {
                if ((base_speed + Perror) > 1000)
                {
                    speedL = 1000;
                }
                else
                speedL = (base_speed + Perror);

                if(base_speed - Perror < 0)
                {
                    speedR = 0;
                }
                else
                speedR = base_speed - Perror;
            }


            motor_set(speedL, speedR);
        }

        else
        {
            if (square_flag)
            {
                motor_set(0, 1000);
            }

            else
            {
                motor_set(1000, 0);
            }
        }


        /*
        if(sw1_read())
        {
            rgb_set(RED);
        }
        else if(sw2_read())
        {
            rgb_set(BLUE);
        }
        else
        {
            rgb_set(GREEN);
            sprintf(buff,"%4d,%4d,%5d \n\r",adc_read(0),adc_read(1),systick);
            radio_puts(buff);
            _delay_ms(100);
        }
        */

    }
}