על ידי ארבל » ש' פברואר 17, 2007 5:08 pm
#define F_CPU 20000000UL
#include <avr/io.h>
#include <util/delay.h>
void motor_b_fwd(void)
{
PORTB |= (1 << PB1); // IN1=1
PORTD &= ~(1 << PD5); // IN2=0
}
void motor_b_rev(void)
{
PORTB &= ~(1 << PB1); // IN1=0
PORTD |= (1 << PD5); // IN2=1
}
void motor_b_brake(void)
{
PORTB |= (1 << PB1); // IN1=1
PORTD |= (1 << PD5); // IN2=1
}
void pwm0_init(void)
{
TCCR0A = (2 << COM0A0) | (2 << COM0B0) | (3 << WGM00);
TCCR0B = (4 << CS00);
TIMSK0 = 0;
// Enable output on PD5 and PD6 so we get our PWM outputs on the
// LMB1836M motor driver inputs:
DDRD |= (1 << PD6);
// Enable output on PB1 and PB2 so we can set those bits to get
// directional control on the other inputs on the LMB1836M:
DDRB |= (1 << PB2);
// Set our waveforms to zero for the time being: (Zero speed.)
OCR0A = 0;
OCR0B = 0;
}
void motor_init()
{
// Set up the data direction registers so our four control
// pins are outputs:
DDRB |= (1 << PB1) | (1 << PB2);
DDRD |= (1 << PD5) | (1 << PD6);
// Set both motor to coast
PORTB &= ~(1 << PB2);
PORTD &= ~(1 << PD6);
}
void pwm0_a(int speed)
{
if (speed > 0)
{
// Forward
// Turn off PB2
PORTB &= ~(1 << PB2);
// Clear compare mode for PWM0A and set to clear on compare
// match, set at bottom:
TCCR0A = (TCCR0A & ~(3 << COM0A0)) | (2 << COM0A0);
// Set our PWM duty cycle
OCR0A = speed;
}
if (speed == 0)
{
// Disconnect PWM from 0C0A:
TCCR0A &= ~(3 << COM0A0);
// Set outputs to zero to coast
PORTD &= ~(1 << PD6);
PORTB &= ~(1 << PB2);
// No PWM duty cycle to set
}
if (speed < 0)
{
// Reverse
// Turn on PB2
PORTB |= (1 << PB2);
// Clear compare mode for PWM0A and set to set on compare
// match, clear at bottom:
TCCR0A = (TCCR0A & ~(3 << COM0A0)) | (3 << COM0A0);
// Set our PWM duty cycle
OCR0A = -speed;
}
}
void pwm0_a_brake(void)
{
// Set motor speed to zero (turns off PWM):
pwm0_a(0);
// Enable both PD6 and PB2 to hit the brakes (POWER HUNGRY):
PORTD &= ~(1 << PD6);
PORTB &= ~(1 << PB2);
}
void delay_sec(unsigned char sec)
{
unsigned int cycles;
// Delay 25ms at a time (38.4ms is the most we can delay with a
// 20MHz processor, unfortunately. See the delay.h include file
// for more info.)
for(cycles = 0; cycles < (sec * 40); cycles ++)
{
_delay_ms(25);
}
}
//If the robot bumps into something
void bump(void)
{
motor_b_fwd();
pwm0_a(-100);
delay_sec(2);
motor_b_brake();
}
int main(void)
//create a variable to hold the sensor reading
unsigned int Bump;
{
// Make sure all our registers are clear
DDRB = 0;
DDRC = 0;
DDRD = 0;
// Make sure all our pull-up resistors are clear
PORTB = 0;
PORTC = 0;
PORTD = 0;
// Initialize the timer0 PWM system:
pwm0_init();
// Initialize motor B
motor_init();
// Motors are stopped when we initialize them, so there's
// no need to do that after the init routine.
Bump = 0;
//making port D3 as input
DDRD &= ~(1 << 3);
// The endless loop
for(;;)
{
//Making sure that Bump is 0
Bump = 0;
Bump = (PIND & (1 << 3)) >> 3;
if (Bump==0)
{
bump();
}
// Both motors half forward
pwm0_a(127);
pwm0_b(127);
delay_sec(1);
// Both motors full forward
pwm0_a(255);
pwm0_b(255);
delay_sec(1);
// Coast to a stop
pwm0_a(0);
pwm0_b(0);
delay_sec(2);
// Both motors two-thirds reverse
pwm0_a(-192);
pwm0_b(-192);
delay_sec(1);
// Both motors full reverse
pwm0_a(-255);
pwm0_b(-255);
delay_sec(1);
// Brake to a stop
pwm0_a_brake();
pwm0_b_brake();
delay_sec(2);
}
// We never get here, but return a zero if we ever do.
return(0);
}
[/code][code][/code]