KÄLLKOD

#define F_CPU 16000000UL
#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>
#include <stdlib.h>
#include <stdio.h>

#define LEDRED 0
#define REVERSE_R 0 //INPUT1M
#define FORWARD_R 1 //INPUT2M
#define FORWARD_L 2 //INPUT3M
#define REVERSE_L 4 //INPUT4M
#define MOTOR_L 4
#define MOTOR_R 5
#define SERVO1 3
#define SERVO2 7
#define TRIGPORT 1 //port 39
#define ECHOPIN 2  //pin 38
#define BTNPIN 3   //pin 37
#define LEDWHITE 4 //pin 36
#define BTN2PIN 5  //pin 35
#define false 0
#define true 1

static volatile int pulse = 0; 
static volatile int i = 0; 
volatile uint8_t servo_min = 15;
volatile uint8_t servo_max = 30; //actual max 33
volatile uint8_t t3o_cnt;
double distance = 0;
uint8_t turnDirection = -1;
uint8_t turnPulse1 = 15;
uint8_t turnPulse2 = 15;
uint8_t noBrush = 1;
uint8_t pressed = 0;
uint8_t trapCounter = 0;
uint16_t forward = 0;
uint16_t count = 0;

int main(void){
	initialize();
	setup_motor_PWM();
	setup_servo1_PWM();
	setup_servo2_PWM();
	timer1_PWM_output();
	timer0_PWM_output();
	timer2_PWM_output();
	set_PWM_period(512);
	set_PWM_motor_pulse(300, 300); //speed left, speed right
	
	sei();
	
	if(!noBrush){
		PORTA |= (1<<LEDWHITE);
	} else {
		PORTA &= ~(1<<LEDWHITE);
	}
	
    	while (1){
		turnHeadServo();
		measureDistance();
		distance = count*0.272; 
		checkButton();
		checkObstacle();

		if(noBrush){
			PORTA &= ~(1<<LEDWHITE);
		} else if(!noBrush) {
			waveServo();
			PORTA |= (1<<LEDWHITE);
		}	
	}
}

void initialize(){
	//Initiate red LED
	DDRA |= (1<<LEDRED);
	
	//Initiate white LED
	DDRA |= (1<<LEDWHITE);

	//Initiate ultrasonic sensor
	DDRA |= (1<<TRIGPORT); //start trig-port
	DDRA &= ~(1<<ECHOPIN); //start echo-pin
	TIMSK3 |= (1 << TOIE3);
	
	//Initiate motors
	DDRB |= (1<<REVERSE_R)|(1<<FORWARD_R)|(1<<FORWARD_L)|(1<<REVERSE_L);
	
	//Initiate servos
	DDRB |= (1<<SERVO1);
	
	//Initiate buttons
	DDRA &= ~(1<<BTNPIN);
	DDRA &= ~(1<<BTN2PIN);
}


void checkButton(){
	while(!(PINA & (1<<BTN2PIN))){ //wait when button is pressed
		_delay_ms(100);
		if(!(PINA & (1<<BTN2PIN))){
			pressed += 1;
		}
	}
	
	if(pressed > 5){
		if(noBrush == 1){
			noBrush = 0;
			PORTA &= ~(1<<LEDWHITE);		
		} else if(noBrush == 0) {
			noBrush = 1;
			PORTA |= (1<<LEDWHITE);
		}
	}
	pressed = 0;
}

void turnHeadServo(){
	if(turnPulse2 == servo_max || turnPulse2 == servo_min){
		turnDirection *= -1;
	}
	turnPulse2 += turnDirection;
	set_PWM_servo2_pulse(turnPulse2);
}

void waveServo(){
	if(turnPulse1 == 30){
		turnPulse1 = 17;
		set_PWM_servo1_pulse(17);
	} else {
		turnPulse1 = 30;
		set_PWM_servo1_pulse(30);
	}
}

void checkObstacle(){
	if(distance < 15){
		trapCounter++;
		_delay_ms(20);
	}
	
	if(distance < 50){
		set_PWM_motor_pulse(300, 300);
		
		PORTA |= (1<<LEDRED);
 
		if(turnPulse2 > servo_max - 4 && distance > 30){
			turnRight(turnPulse2);
			turnPulse2 = servo_max-3;
		} else if(turnPulse2 > servo_max - 4) {
			rotateRight(100*(1+servo_max-turnPulse2));
			turnPulse2 = servo_max-3;
		} else if(turnPulse2 < servo_min + 4 && distance > 30) {
			turnLeft(turnPulse2);
			turnPulse2 = servo_min+3;
		} else if(turnPulse2 < servo_min + 4) {
			rotateLeft(100*(1+turnPulse2-servo_min));
			turnPulse2 = servo_min+3;
		}
	} else {
		PORTA &= ~(1<<LEDRED);
		set_PWM_motor_pulse(300, 300);
		moveForward();
		trapCounter = 0;
	}
	
	if(trapCounter > 20){
		moveStop();
		_delay_ms(200);
		moveReverse();
		_delay_ms(1000);
		rotateRight(400);
		trapCounter = 0;
		set_PWM_motor_pulse(300, 300);
		distance = 0;
	}	
}

void rotateRight(uint16_t delay){
	PORTB &= ~(1<<REVERSE_R);
	PORTB &= ~(1<<FORWARD_L);
	PORTB |= (1<<FORWARD_R);
	PORTB |= (1<<REVERSE_L);
	_delay_ms(delay);
	moveStop();
}

void rotateLeft(uint16_t delay){
	PORTB &= ~(1<<FORWARD_R);
	PORTB &= ~(1<<REVERSE_L);
	PORTB |= (1<<FORWARD_L);
	PORTB |= (1<<REVERSE_R);
	_delay_ms(delay);
	moveStop();
}

void turnRight(uint8_t turnPulse){
	set_PWM_motor_pulse(300-6*(servo_max-turnPulse)*(servo_max-turnPulse), 500);
	PORTB &= ~(1<<REVERSE_R);
	PORTB &= ~(1<<REVERSE_L);
	PORTB |= (1<<FORWARD_L);
	PORTB |= (1<<FORWARD_R);
	_delay_ms(100);
}

void turnLeft(uint8_t turnPulse){
	set_PWM_motor_pulse(500, 300-6*(turnPulse-servo_min)*(turnPulse-servo_min));
	PORTB &= ~(1<<REVERSE_R);
	PORTB &= ~(1<<REVERSE_L);
	PORTB |= (1<<FORWARD_L);
	PORTB |= (1<<FORWARD_R);
	_delay_ms(100);
}

void measureDistance(){
	t3o_cnt = 0;
	PORTA |= (1<<TRIGPORT); //on
	_delay_us(10);
	PORTA &= ~(1<<TRIGPORT);//off
	
	while(!(PINA & (1<<ECHOPIN))); //while ECHOPIN is low
	TCCR3B |= (1<<CS32);  	       //start timer
	while(PINA & (1<<ECHOPIN));    //while ECHOPIN is high
	TCCR3B &= ~(1<<CS32);          //stop timer
	
	count = TCNT3;
	_delay_ms(60);//specified according to data sheet
	TCNT3 = 0;
}

void setup_motor_PWM(){
	//Setting timer properties for timer 1, for the motors. 16-bit timer.
	TCCR1A |= (1<<COM1A1)|(1<<COM1B1)|(1<<WGM11); //COM1n1: Clear Compare Match (shall trigger pulse at certain clock time), WGMn1: Fast PWM (mode)
	TCCR1B |= (1<<WGM13)|(1<<WGM12)|(1<<CS12); //WGMn2: Fast PWM, CSn2: Pre-scaler 256 (choosen according to requirements for pulse speed)
	
}

void setup_servo1_PWM(){
	//Setting timer properties for timer 0, for the servo. 8-bits timer (no ICR to be set, always 255).
	TCCR0A |= (1<<COM0A1)|(1<<WGM01)|(1<<WGM00); //COM1n1: Clear Compare Match (shall trigger pulse at certain clock time), WGMn1: Fast PWM (mode)
	TCCR0B |= (1<<CS02)|(1<<CS00); //Apparently no WGMn2: CSn2: pre-scaler 1024 (choosen according to requirements for pulse speed)
}

void setup_servo2_PWM(){
	//Setting timer properties for timer 2, for the servo. 8-bits timer (no ICR to be set, always 255).
	TCCR2A |= (1<<COM2A1)|(1<<WGM21)|(1<<WGM20); //COM1n1: Clear Compare Match (shall trigger pulse at certain clock time), WGMn1: Fast PWM (mode)
	TCCR2B |= (1<<CS22)|(1<<CS21)|(1<<CS20); //CSn2: pre-scaler 1024 (choosen according to requirements for pulse speed)
}

void timer2_PWM_output(){
	//Start servo 2 port
	DDRD |= (1<<SERVO2);
	
}

void timer1_PWM_output(){
	//Start enableA and enableB
	DDRD |= (1<<MOTOR_L)|(1<<MOTOR_R); 
	
}

void timer0_PWM_output(){
	//Start servo 1 port
	DDRB |= (1<<SERVO1);
	
}

void set_PWM_period(uint16_t period){
	//Set period i.e. on-time, set to 512
	ICR1 = period;
}


void set_PWM_motor_pulse(uint16_t pulseLEFT, uint16_t pulseRIGHT){
	//Set speed of motor between ~100-500
	OCR1A = pulseRIGHT;
	OCR1B = pulseLEFT;
}

void set_PWM_servo1_pulse(uint16_t pulse){
	//Set angle of servo
	OCR0A = pulse;
}

void set_PWM_servo2_pulse(uint16_t pulse){
	//Set angle of servo
	OCR2A = pulse;
}

void moveForward(){
	PORTB |= (1<<FORWARD_R);
	PORTB |= (1<<FORWARD_L); 
	PORTB &= ~(1<<REVERSE_R);
	PORTB &= ~(1<<REVERSE_L);
}

void moveReverse(){
	PORTB &= ~(1<<FORWARD_R);
	PORTB &= ~(1<<FORWARD_L);
	PORTB |= (1<<REVERSE_R);
	PORTB |= (1<<REVERSE_L);
}

void moveStop(){
	PORTB &= ~(1<<FORWARD_R);
	PORTB &= ~(1<<FORWARD_L);
	PORTB &= ~(1<<REVERSE_R);
	PORTB &= ~(1<<REVERSE_L);
}

ISR(TIMER3_OVF_vect) {
	t3o_cnt++;
}