roboforum.ruТехнический форум по робототехнике. |
|
|
Duhas писал(а):осциллограф шумов на первый взгляд вообще не увидел...
#include <Mega32.h>
#include <stdio.h>
#include <math.h>
#include <delay.h>
#define begin {
#define end }
#define dTime 1000
#define srTime 300
#define slTime 200
#define rTime 100
#define left 0
#define right 1
#define forward 2
#define stop 3
char mic1, roundFinished, firstFinished, secondFinished; //mic ADC
char m1value, m2value, m3value; //mic signals
int driveTime, steerTime, resetTime; //timers for driving
char drive, steer, enableResetTime, direction, turnedLeft, turnedRight; //driving var
void initialize(void);
void resetWheel(void);
//==================================
//Timer0 Interrupt
interrupt [TIM0_COMP] void tim0isr(void)
begin
//rear motor timer
if (drive==1 && driveTime>0) driveTime--;
//steering motor timer
if (steer==1 && steerTime>0) steerTime--;
//reseting steering wheel timer
if (enableResetTime==1) resetTime--;
end //timer0 ISR
//==================================
//ADC Interrupt
interrupt [ADC_INT] void adcComp(void)
begin
if (mic1==1) //first mic ADC
begin
m1value = ADCH; //retrieve ADC value
ADMUX = 0b01100010; //switch pin
mic1 = 2;
firstFinished = 1;
end
else
begin
if (mic1==2) //second mic ADC
begin
m2value = ADCH; //retrieve ADC value
ADMUX = 0b01100011; //switch pin
mic1 = 3;
secondFinished = 1;
end
else //third mic ADC
begin
m3value = ADCH; //retrieve ADC value
ADMUX = 0b01100001; //switch pin
mic1 = 1;
roundFinished = 1;
end
end
end //ADC ISR
//==================================
//initialize
void initialize(void)
begin
DDRA = 0x00; //ADC
DDRB = 0xff; //motor B7=f/r B6=l/r B5=pwm
DDRC = 0xff; //LED
DDRD = 0xff; //LED
//TIM0 SETUP Interrupt every 1ms
OCR0 = 249;
TIMSK = 0b00000010;
TCCR0 = 0b00001011;
//ADC SETUP
//init the A to D converter
//channel zero/ left adj /EXTERNAL Aref
ADMUX = 0b01100001;
//enable ADC and set prescaler to 1/128*16MHz=125,000
ADCSR = 0b10001110;
//init variables
mic1 = 1;
firstFinished = 0;
secondFinished = 0;
roundFinished = 0;
drive = 0;
steer = 0;
driveTime = dTime;
steerTime = srTime;
resetTime = rTime;
turnedRight = 0;
turnedLeft = 0;
#asm
sei
#endasm
end //initialize
//==================================
//resetWheel
void resetWheel(void)
begin
if (turnedLeft==1) //previous turned left, turn right
begin
PORTB.6 = 1;
PORTB.5 = 1;
end
if (turnedRight==1) //previous turned right, turn left
begin
PORTB.6 = 0;
PORTB.5 = 1;
end
turnedLeft = 0;
turnedRight = 0;
enableResetTime = 1;
resetTime = rTime;
end
//==================================
//main
void main(void)
begin
initialize();
PORTD=0xff; //turn LED off
delay_ms(2000);
ADCSR.6 = 1; //start ADC
while(1)
begin
if (resetTime==0 && enableResetTime==1) //reseting wheels done
begin
PORTB.5 = 0; //stop PWM
enableResetTime = 0;
delay_ms(2000); //delay for noise from the motor
ADCSR.6 = 1; // anoter ADC
end
if (driveTime==0 && drive==1) //forward drive done
begin
drive = 0;
direction = stop;
PORTB.7 = 0;
if (turnedLeft==1 || turnedRight==1)
begin
resetWheel();
PORTC=0b00110011;
end
else
begin
delay_ms(2000);
ADCSR.6 = 1;
end
end
if (steerTime==0 && steer==1) //steering done
begin
PORTC = 0xaa;
steer = 0;
direction = stop;
PORTB.5 = 0;
end
if (firstFinished==1) //first mic ADC done
begin
firstFinished = 0;
ADCSR.6 = 1;
end
if (secondFinished==1) //second mic ADC done
begin
secondFinished = 0;
ADCSR.6 = 1;
end
if (roundFinished==1) //third mic ADC done
begin
roundFinished = 0;
//right turn
if (m1value>=180 && m2value<180 &&m3value<180) //first mic closest to the source
begin
roundFinished = 0;
direction = right;
end
//left turn
if (m1value<180 && m2value>=180 && m3value<180) //second mic closest to the source
begin
roundFinished = 0;
direction = left;
end
//forward
if (m1value<180 && m2value<180 && m3value>=180) //third mic closest to the source
begin
direction = forward;
roundFinished = 0;
end
if (m1value<180 && m2value<180 && m3value<180) //no signals received
begin
roundFinished = 0;
ADCSR.6 = 1;
end
if (m1value>=180 && m2value>=180 && m3value>=180) //reset
begin
PORTD=0x00;
initialize();
roundFinished = 0;
end
//driving right b6=0 left b6=1
if (direction==forward) //drive forward
begin
PORTC = 0b01111111;
drive = 1;
steer = 0;
direction = stop;
driveTime = dTime;
PORTB.7 = 1;
end
if (direction==left) //drive left
begin
PORTC = 0b10111111;
drive = 1;
steer = 1;
direction = stop;
driveTime = dTime;
steerTime = slTime;
PORTB.7 = 1;
PORTB.6 = 0;
PORTB.5 = 1;
turnedLeft = 1;
end
if (direction==right) //drive right
begin
PORTC = 0b11011111;
drive = 1;
steer = 1;
direction = stop;
driveTime = dTime;
steerTime = srTime;
PORTB.7 = 1;
PORTB.6 = 1;
PORTB.5 = 1;
turnedRight = 1;
end
end
end //while
end //main
Сейчас этот форум просматривают: нет зарегистрированных пользователей и гости: 4