Code:
#include "asuro.h"
#include "string.h"
/*! \brief Counter fuer 36kHz.\n
* Wird in derInterrupt Funktion SIG_OVERFLOW2 hochgezaehlt\n
* und in der Sleep() Funktion abgefragt.
* \see Sleep
*/
volatile unsigned char count36kHz;
/*! \brief Sytemzeit.\n
* Wird in der Interrupt Funktion SIG_OVERFLOW2 hochgezaehlt\n
* und in der Gettime() Funktion verwendet.
* \see Gettime
*/
volatile unsigned long timebase;
/*! \brief Odometrie Sensor Abfrage im Interrupt Betrieb.\n
* Wird in der Interrupt Funktion SIG_ADC abgefragt,\n
* in der Encoder_Init() und Encoder_Start() Funktion gesetzt\n
* und in der Encoder_Stop() Funktion geloescht .
* \see Encoder_Init, Encoder_Start, Encoder_Stop
*/
volatile int autoencode=FALSE;
/*!
* \func SIG_OVERFLOW2
* \brief Interrupt Funktion: Timer2 Overflow
* uses timer2 (36kHz for IR communication)
*/
SIGNAL (SIG_OVERFLOW2)
{
TCNT2 += 0x25;
count36kHz ++;
if (!count36kHz) timebase ++;
}
/*!
* \func SIG_INTERRUPT1
* \brief Interrupt Funktion: INT1
*/
SIGNAL (SIG_INTERRUPT1)
{
switched=1;
StopSwitch();
}
/*!
* \func SIG_ADC
* \brief Interrupt Funktion: A/D Wandler
* last modification:
* Ver. Date Author Comments
* ------- ---------- -------------- ---------------------------------
* 2.61 20.11.2006 m.a.r.v.i.n static Variable toggle initialisiert
* auf False (Bug report von Rolf_Ebert)
*
*
*/
SIGNAL (SIG_ADC)
{
static unsigned char tmp[2],flag[2],toggle=FALSE;
if (autoencode){
tmp[toggle]= ADCH;
if (toggle) ADMUX = (1 <<ADLAR) | (1 <<REFS0) | WHEEL_RIGHT;
else ADMUX = (1 <<ADLAR) | (1 <<REFS0) | WHEEL_LEFT;
if ( (tmp[toggle] < 140) && (flag[toggle] == TRUE)) {
encoder[toggle] ++;
flag[toggle] = FALSE;
}
if ( (tmp[toggle] > 160) && (flag[toggle] == FALSE)) {
encoder[toggle] ++;
flag[toggle] = TRUE;
}
toggle ^= 1;
}}
// neue Zeitfunktion
unsigned long Gettime(void)
{
return ((timebase*256)+count36kHz)/36;
}
/* Init function Processor will be initalized to work correctly */
void Init (void)
{
//-------- seriell interface programmed in boot routine and already running -------
// prepare 36kHz for IR - Communication
TCCR2 = (1 << WGM20) | (1 << WGM21) | (1 << COM20) | (1 << COM21) | (1 << CS20);
OCR2 = 0x91; // duty cycle for 36kHz
TIMSK |= (1 << TOIE2); // 36kHz counter for sleep
// prepare RS232
UCSRA = 0x00;
UCSRB = 0x00;
UCSRC = 0x86; // No Parity | 1 Stop Bit | 8 Data Bit
UBRRL = 0xCF; // 2400bps @ 8.00MHz
// I/O Ports
DDRB = IRTX | LEFT_DIR | PWM | GREEN_LED;
DDRD = RIGHT_DIR | FRONT_LED | ODOMETRIE_LED | RED_LED;
// for PWM (8-Bit PWM) on OC1A & OC1B
TCCR1A = (1 << WGM10) | (1 << COM1A1) | (1 << COM1B1);
// tmr1 running on MCU clock/8
TCCR1B = (1 << CS11);
// A/D Conversion
ADCSRA = (1 << ADEN) | (1 << ADPS2) | (1 << ADPS1); // clk/64
ODOMETRIE_LED_OFF;
FrontLED(OFF);
BackLED(ON,ON);
BackLED(OFF,OFF);
StatusLED(GREEN);
MotorDir(FWD,FWD);
MotorSpeed(0,0);
sei();
}
/* Set motor speed */
inline void MotorSpeed(unsigned char left_speed, unsigned char right_speed)
{
OCR1A = left_speed;
OCR1B = right_speed;
}
/* Set motor direction */
inline void MotorDir(unsigned char left_dir, unsigned char right_dir)
{
PORTD = (PORTD &~ ((1 << PD4) | (1 << PD5))) | left_dir;
PORTB = (PORTB &~ ((1 << PB4) | (1 << PB5))) | right_dir;
}
/* Status LED (OFF,GREEN,YELLOW,RED)*/
/* example code set StatusLED GREEN */
/* StatusLED(GREEN); */
inline void StatusLED(unsigned char color)
{
if (color == OFF) {GREEN_LED_OFF; RED_LED_OFF;}
if (color == GREEN) {GREEN_LED_ON; RED_LED_OFF;}
if (color == YELLOW) {GREEN_LED_ON; RED_LED_ON;}
if (color == RED) {GREEN_LED_OFF; RED_LED_ON;}
}
/* Front LED */
/* example code FrontLED ON */
/* FrontLED(ON); */
inline void FrontLED(unsigned char status)
{
PORTD = (PORTD &~(1 << PD6)) | (status << PD6);
}
/* function for Break LEDs */
/* example code right LED On left LED Off */
/* BackLED(OFF,ON); */
void BackLED(unsigned char left, unsigned char right)
{
if (left || right) {
PORTD &= ~(1 << PD7); // Wheel LED OFF
DDRC |= (1 << PC0) | (1 << PC1); // Output => no odometrie
PORTC |= (1 << PC0) | (1 << PC1);
}
if (!left) PORTC &= ~(1 << PC1);
if (!right) PORTC &= ~(1 << PC0);
}
int Batterie(void)
{
ADMUX = (1 << REFS0) | (1 << REFS1) | BATTERIE; // internal 2.56V reference with external capacitor
ADCSRA |= (1 << ADSC); // Start conversion
while (!(ADCSRA & (1 << ADIF))); // wait for conversion complete
ADCSRA |= (1 << ADIF); // clear ADCIF
return ADCL + (ADCH << 8);
}
/* function to read out line follow phototransistors (left,rigth) */
void LineData(unsigned int *data)
{
int ec_bak=autoencode;
autoencode=FALSE;
ADMUX = (1 << REFS0) | IR_LEFT; // AVCC reference with external capacitor
Sleep(10);
ADCSRA |= (1 << ADSC); // Start conversion
while (!(ADCSRA & (1 << ADIF))); // wait for conversion complete
ADCSRA |= (1 << ADIF); // clear ADCIF
data[0] = ADCL + (ADCH << 8);
ADMUX = (1 << REFS0) | IR_RIGHT; // AVCC reference with external capacitor
Sleep(10);
ADCSRA |= (1 << ADSC); // Start conversion
while (!(ADCSRA & (1 << ADIF))); // wait for conversion complete
ADCSRA |= (1 << ADIF); // clear ADCIF
data[1] = ADCL + (ADCH << 8);
autoencode=ec_bak;
}
/* function to read out odometrie phototransistors (left,rigth) */
void OdometrieData(unsigned int *data)
{
DDRC &= ~((1 << PC0) | (1 << PC1)); // Input => no break LED
ODOMETRIE_LED_ON;
ADMUX = (1 << REFS0) | WHEEL_LEFT; // AVCC reference with external capacitor
ADCSRA |= (1 << ADSC); // Start conversion
while (!(ADCSRA & (1 << ADIF))); // wait for conversion complete
ADCSRA |= (1 << ADIF); // clear ADCIF
data[0] = ADCL + (ADCH << 8);
ADMUX = (1 << REFS0) | WHEEL_RIGHT; // AVCC reference with external capacitor
ADCSRA |= (1 << ADSC); // Start conversion
while (!(ADCSRA & (1 << ADIF))); // wait for conversion complete
ADCSRA |= (1 << ADIF); // clear ADCIF
data[1] = ADCL + (ADCH << 8);
}
/* function for serial communication */
void SerWrite(unsigned char *data,unsigned char length)
{
unsigned char i = 0;
UCSRB = 0x08; // enable transmitter
while (length > 0) {
if (UCSRA & 0x20) { // wait for empty transmit buffer
UDR = data[i++];
length --;
}
}
while (!(UCSRA & 0x40));
for (i = 0; i < 0xFE; i++)
for(length = 0; length < 0xFE; length++);
}
void SerRead(unsigned char *data, unsigned char length,unsigned int timeout)
{
unsigned char i = 0;
unsigned int time = 0;
UCSRB = 0x10; // enable receiver
/* non blocking */
if (timeout != 0) {
while (i < length && time++ < timeout) {
if (UCSRA & 0x80) {
data[i++] = UDR;
time = 0;
}
}
if (time > timeout) data[0] = 'T';
}
/* blocking */
else {
while (i < length) {
if (UCSRA & 0x80)
data[i++] = UDR;
}
}
}
/* function to read out switches */
unsigned char PollSwitch (void)
{
unsigned int i;
int ec_bak=autoencode;
autoencode=FALSE;
DDRD |= SWITCHES; // Switches as Output
SWITCH_ON; // Output HIGH for measurement
ADMUX = (1 << REFS0) | SWITCH; // AVCC reference with external capacitor
Sleep(10);
ADCSRA |= (1 << ADSC); // Start conversion
while (!(ADCSRA & (1 << ADIF)));// wait for conversion complete
ADCSRA |= (1 << ADIF); // clear ADCIF
i = ADCL + (ADCH << 8);
SWITCH_OFF;
Sleep(5);
autoencode=ec_bak;
//return ((unsigned char) ((( 1024.0/(float)i - 1.0)) * 61.0 + 0.5));
return ((10240000L/(long)i-10000L)*61L+5000L)/10000;
}
/* for working with Interrupt */
void StartSwitch(void)
{
SWITCH_OFF;
DDRD &= ~SWITCHES; // Switches as Input => ext. Int 1
MCUCR &= ~((1 << ISC11) | (1 << ISC10));// Low level generates interrupt
GICR |= (1 << INT1); // Enable external Interrupt 1
}
void StopSwitch(void)
{
GICR &= ~(1 << INT1);
}
/* uses 36kHz timer => Sleep(x) = x/36kHz [sec] */
void Sleep(unsigned char time36kHz)
{
unsigned char ziel=(time36kHz+count36kHz) & 0x00FF;
while (count36kHz != ziel);
}
void Encoder_Init(void)
{
cli();
DDRC &= ~ ((1<<PC0) | (1<<PC1)); // Input => no break LED
ODOMETRIE_LED_ON;
ADCSRA = (1<<ADEN) | (1<<ADFR) | (1<<ADIE) | (1<<ADSC) | (1<<ADPS0) | (1<<ADPS1) | (1<<ADPS2); // clk/128
ADMUX = (1<<ADLAR) | (1<<REFS0) | WHEEL_LEFT; // AVCC reference with external capacitor
autoencode=TRUE;
sei();
Encoder_Set(0,0);
}
void Encoder_Stop(void){autoencode=FALSE;}
void Encoder_Start(void){autoencode=TRUE;}
void Encoder_Set(int setl,int setr)
{
encoder[LEFT]=setl;
encoder[RIGHT]=setr;
}
/***************************************************************************
* void PrintInt(int wert)
*
* last modification:
* Ver. Date Author Comments
* ------- ---------- -------------- ---------------------------------
* 2.60 28.09.2005 m.a.r.v.i.n strlen instead fixed length
* 2.61 20.11.2006 m.a.r.v.i.n Initialisierung text String kann zu Fehler
* beim Flashen mit RS232/IR Adapter fuehren
* (Bug report von francesco)
***************************************************************************/
void PrintInt(int wert)
{
char text[6];
itoa(wert,text,10);
SerWrite(text,strlen(text));
}
void Msleep(int dauer)
{
int z;
for(z=0;z<dauer;z++) Sleep(36);
}
/***************************************************************************
* void Go(int distance, int speed = 150)
*
* input
* distance: postiv->go forward ; negativ-> go backward
* speed: sets motorspeed
*
* last modification:
* Ver. Date Author Comments
* ------- ---------- -------------- ---------------------------------
* sto1 29.07.2005 stochri motorfunction
* And1 31.07.2005 Andun added speed and Odometrie
* ------- ---------- -------------- ---------------------------------
*
***************************************************************************/
void Go(int distance, int speed)
{
int enc_count = 0;
int tot_count = 0;
int diff = 0;
int l_speed = speed, r_speed = speed;
enc_count=abs(distance);
// enc_count=distance*10000;
// enc_count/=12823;
Encoder_Set(0,0); // reset encoder
MotorSpeed(l_speed,r_speed);
if(distance<0) MotorDir(RWD,RWD);
else MotorDir(FWD,FWD);
while(tot_count<enc_count) {
tot_count += encoder[LEFT];
diff = encoder[LEFT] - encoder[RIGHT];
if (diff > 0) { //Left faster than right
if ((l_speed > speed) || (r_speed > 244)) l_speed -= 10;
else r_speed += 10;
}
if (diff < 0) { //Right faster than left
if ((r_speed > speed) || (l_speed > 244)) r_speed -= 10;
else l_speed += 10;
}
Encoder_Set(0,0); // reset encoder
MotorSpeed(l_speed,r_speed);
Msleep(1);
}
MotorDir(BREAK,BREAK);
Msleep(200);
}
/***************************************************************************
* void Turn(int degree, int speed)
*
* input
* degree: postiv->turn right ; negativ-> turn left
*
* last modification:
* Ver. Date Author Comments
* ------- ---------- -------------- ---------------------------------
* sto1 29.07.2005 stochri motorfunction
* And1 07.08.2005 Andun Added Odometrie function
* ------- ---------- -------------- ---------------------------------
*
***************************************************************************/
void Turn(int degree, int speed)
{
long enc_count;
enc_count=abs(degree)*0166L;
enc_count /= 0360L;
int tot_count = 0;
int diff = 0;
int l_speed = speed, r_speed = speed;
Encoder_Set(0,0); // reset encoder
MotorSpeed(l_speed,r_speed);
if(degree<0) MotorDir(RWD,FWD);
else MotorDir(FWD,RWD);
while(tot_count<enc_count) {
tot_count += encoder[LEFT];
diff = encoder[LEFT] - encoder[RIGHT];
if (diff > 0) { //Left faster than right
if ((l_speed > speed) || (r_speed > 244)) l_speed -= 10;
else r_speed += 10;
}
if (diff < 0) { //Right faster than left
if ((r_speed > speed) || (l_speed > 244)) r_speed -= 10;
else l_speed += 10;
}
Encoder_Set(0,0); // reset encoder
MotorSpeed(l_speed,r_speed);
Msleep(1);
}
MotorDir(BREAK,BREAK);
Msleep(200);
}
Hab nur asuro.h und asuro.c esetzt muss ich noch was anderes ersetzten oder hinzufügen vielleicht
Zitieren
DANKE! Hatte die neue Version schon runtergeladen. Nur auf die idee sie auch zu installieren bin ich irgendwie net gekommen ^^
Lesezeichen