Min kod
/*
* GccApplication1.c
*
* Created: 4/18/2018 12:21:35 AM
* Author : Gustav
*/
#define _BV(bit) (1<<(bit))
#define clear_bit(a,z) (a &= ~_BV(z))
#define set_bit(a,z) (a|=_BV(z))
#define ADCH0 ((1<<(REFS0)) | 0)
#define ADCH1 ((1<<(REFS0)) | 1)
#define ADCH2 ((1<<(REFS0)) | 2)
#define ADCH3 ((1<<(REFS0)) | 3)
#define F_CPU 8000000UL /* Define CPU Frequency e.g. here its 8MHz */
#include /* Include AVR std. library file */
#include /* Include std. library file */
#include /* Include Delay header file */
#include
typedef int bool;
#define true 1
#define false 0
uint8_t stopDist, dist1, dist2, leftDist, rightDist;
uint16_t adc_result, adc_sum,angle,angle2, adc_mic_sum;
uint8_t adc_count, adc_mic_count;
volatile int analogV1;
volatile int analogV2;
volatile int analogV3;
volatile int adcCh;
int dist;
bool nextDist, adc_mean_done, adc_done, clap_done, right_block, left_block, center_block, on, off;
//DC Motors
/*
Motor 1:
PD3 till BIN1 - > BOUT 1 till engine. (PB3 = OC0)
PB4 till BIN2 -> BOUT 2 till engine.
Motor 2:
PB 5 till AIN2 -> AOUT2
PD7 till AIN1 -> AOUT 1 (OBS pd7 = OC2)
*/
void initMotor1(){
DDRD |= _BV(DDD7); //set PD7 OC2 to output)
DDRB |= _BV(DDB5); //() (PB5) for motor ctrl set PB5 output
TCCR2 |= _BV(WGM20); //set Phase correct PWM mode.
TCCR2 |= _BV(COM21); //clear on compare match, non-inverted.
TCCR2 |= _BV(CS21); //prescale divider 1.
}
void forwardM1(){
set_bit(PORTB,PB5);
OCR2 = 165; //170
}
void reverseM1(){
clear_bit(PORTB,PB5);
OCR2 = 80; // 85
}
void breakM1(){
set_bit(PORTB,PB5);
OCR2 = 255;
}
//Motor two. (Hbridge BIN1)s
void initMotor2(){
DDRB |= _BV(DDB3); // set PB3 OC0 as output.
DDRB |= _BV(DDB4); // set PB4 as output. (PA1 to bin2) for motor ctrl
TCCR0 |= _BV(WGM00); //set Phase correct PWM Mode.
TCCR0 |= _BV(COM01); // clear on compare match. non-inverted.
TCCR0 |= _BV(CS01);// prescaler divider 1.
}
void forwardM2(){
set_bit(PORTB,PB4);
OCR0 = 170; //170
}
void reverseM2(){
clear_bit(PORTB,PB4);
OCR0 = 80; //85
}
void breakM2(){
set_bit(PORTB,PB4);
OCR0 = 255;
}
void initVars(){
on = false;
off = true;
}
//ServoController
void initPWMServo()
{
DDRD |= _BV(DDD4); // OC1B as output
TCCR1A |= _BV(COM1B1); // Clear OC1A/OC1B on compare match
TIMSK |= _BV(TOIE1);
TCNT1H = 0x00;
TCNT1L = 0x00;
ICR1 = 10000; //50Hz
TCCR1B |= _BV(WGM13) // mode 8, PWM, Phase and Frequency Correct (TOP value is ICR1)
| _BV(CS11); //prescaler 8
}
void SetServoPosition(int a)
{
// Parameters set #1, full turn, OCR=750 => 90 degrees = straight ahead.
const uint16_t MinOCR = 420;
const uint16_t MaxOCR = 750;
/* Servo shaft max rotation */
//const uint16_t Range = 180; //in deg
// Compute OCR not using floating point math
uint16_t ocr = MinOCR + ((MaxOCR-MinOCR) * ((a*100)/180))/100;
OCR1B = ocr;
}
int GetServoPosition(){
if(angle >=180) {
return angle2;
} else {
return angle;
}
}
void initADC(){
DDRA = 0x00;
ADMUX = ADCH0;
ADCSRA |= _BV(ADPS2)|_BV(ADPS1)|_BV(ADPS0)|_BV(ADIE)|_BV(ADEN);
sei();
analogV1=0;
analogV2=0;
ADCSRA |= (1< 450) {
if(OCR1B < 440) {
right_block = true;
}
if(OCR1B > 730) {
left_block = true;
}
if(OCR1B > 560 && OCR1B < 590) {
center_block = true;
}
}
else {
forwardM1();
forwardM2();
}
}
}
void drive() {
if(right_block && left_block && center_block) {
reverseM1();
breakM2();
_delay_ms(1000);
center_block = false;
right_block = false;
left_block = false;
}
if(right_block) {
reverseM2();
_delay_ms(500);
right_block = false;
}
if(left_block) {
reverseM1();
_delay_ms(500);
left_block = false;
}
}
// Motor 1 = höger motor i färdriktning, Motor2 i vänstra motorn.
// OCR1B = 420 => 0 grader (d.v.s. kollar åt höger), OCR1B = 750 => 90 grader (kollar åt vänster). 585 rakt fram.
void checkObstacle(){
if(clap_done){
clap_done = false;
if(analogV2 > 45) {
if(!on) {
off = false;
on = true;
DDRB |= _BV(DDB0); //Set PB0 pin as output
set_bit(PORTB,PB0);
return;
}
if(on) {
on = false;
off = true;
clear_bit(PORTB, PB0);
return;
}
}
}
}
ISR(TIMER1_OVF_vect) {
if(angle < 180) {
SetServoPosition(angle);
angle=angle + 10;
}
if(angle >= 180) {
SetServoPosition(angle2);
angle2=angle2-10;
}
if(angle2 <=0) {
angle = 0;
angle2 = 180;
}
}
ISR(ADC_vect)
{
adcCh = ADMUX & (_BV(MUX3) | _BV(MUX2) | _BV(MUX1) | _BV(MUX0));
uint16_t adc_result = ADC;
switch(adcCh) {
case 0:
analogV1 = adc_result;
adc_sum = adc_sum + adc_result;
adc_count++;
if(adc_count == 8){
adc_mean_done = true;
analogV1 = adc_sum >> 3;
adc_sum = 0;
adc_count = 0;
ADMUX = ADCH2;
}
break;
case 2:
analogV2 = adc_result;
adc_mic_sum = adc_mic_sum + adc_result;
adc_mic_count++;
if(adc_mic_count == 8) {
clap_done = true;
analogV2 = adc_mic_sum >>3;
adc_mic_sum = 0;
adc_mic_count = 0;
ADMUX = ADCH0;
}
break;
}
ADCSRA |= _BV(ADSC);
}
int main()
{
initVars();
initADC();
initPWMServo();
initMotor1();
initMotor2();
while(1){
checkObstacle();
if(on) {
Obstacle();
drive();
}
if(off) {
breakM1();
breakM2();
}
}
}
