Code:
/*!
* \file asuro.c
* \brief Die ASURO Bibliothek
*
* $Revision: 2.60 $
* $Date: 27. September 2005 $
* $Author: Jan Grewe, Robotrixer, Waste, Stochri, Andun, m.a.r.v.i.n $
*
*/
/*******************************************************************************
*
* File Name: asuro.c
* Project : ASURO
*
* Description: This file contains ASURO main features
*
* Ver. Date Author Comments
* ------- ---------- -------------- ------------------------------
* 1.00 14.08.2003 Jan Grewe build
* 2.00 14.10.2003 Jan Grewe LEFT_VEL, RIGHT_VEL -> MotorSpeed(unsigned char left_speed, unsigned char right_speed);
* LeftRwd(),LeftFwd(),RightRwd(),
* RigthFwd() -> MotorDir(unsigned char left_dir, unsigned char right_dir);
* GREEN_ON,GREEN_OFF,RED_ON,RED_OFF -> StatusLED(unsigned char color);
* LED_RED_ON, LED_RED_OFF -> FrontLED(unsigned char color);
* Blink(unsigned char left, unsigned char right) -> BackLED(unsigned char left, unsigned char right);
* Alles in Funktionen gefasst => leichter verständlich ?!?!
* 2.10 17.10.2003 Jan Grewe new Timer funktion void Sleep(unsigned char time36kHz)
*
* Copyright (c) 2003 DLR Robotics & Mechatronics
*****************************************************************************/
/****************************************************************************
*
* File Name: asuro.c
* Project : asuro library "Robotrixer Buxtehude"
*
* Description: This file contains additional functions:
*
* signal (SIG_ADC) interrupt/signal routine for encoder-counter
* signal (SIG_INTERRUPT1) signal for switches
* Encoder_Init() initializing encoder-counter
* Encoder_Start() start autoencoding
* Encoder_Stop() stop autoencoding
* Encoder_Set(int,int) set encodervalue
* Msleep(int delay) wait for delay in milliseconds
* Gettime() get systemtime in milliseconds
* PrintInt(int)
*
* modifications in Sleep, SIG_OUTPUT_COMPARE2, PollSwitch, LineData
*
* Ver. Date Author Comments
* ------- ---------- -------------- ------------------------------
* beta1 31.03.2005 Robotrixer asuro library
* ------- ---------- -------------- ------------------------------
* the encoder source is based on RechteckDemo.c ver 2.0 by Jan Grewe 22.10.2003
* Copyright (c) 2003 DLR Robotics & Mechatronics
*****************************************************************************/
/****************************************************************************
*
* File Name: asuro.c
* Project : asuro library modified for IR collision detector
*
* Description: modifications made in following functions:
*
* SIGNAL (SIG_OUTPUT_COMPARE2) -> SIGNAL (SIG_OVERFLOW2)
* Gettime() counts now 36kHz
* Init() timer2 modified for adjustable duty cycle
* Batterie() bug fixed
* Sleep() counts now 36kHz
* Msleep() counts now 36kHz
*
* Ver. Date Author Comments
* ------- ---------- -------------- ------------------------------
* beta2 11.06.2005 Waste asuro library
* ------- ---------- -------------- ------------------------------
*****************************************************************************/
/****************************************************************************
*
* File Name: asuro.c
* Project : asuro library
*
* Description: This file contains additional functions:
*
* motor control functions 29.7.2005 stochri
* void Go(int distance)
* void Turn(int degree)
*
* unsigned char Wheelspeed[2] measured Wheelspeed by interupt
*
* Ver. Date Author Comments
* ------- ---------- -------------- ------------------------------------------
* sto1 29.07.2005 stochri asuro library with motor control functions
* ------- ---------- -------------- ------------------------------------------
*****************************************************************************/
/****************************************************************************
*
* File Name: asuro.c
* Project : asuro library
*
* Description: modifications made in following functions:
*
* void Go(int distance, int speed)
* void Turn(int degree, int speed)
*
*
* Ver. Date Author Comments
* ------- ---------- -------------- ------------------------------------------
* And1 31.07.2005 Andun Added Speed and Odometrie
* ------- ---------- -------------- ------------------------------------------
*****************************************************************************/
/****************************************************************************
*
* File Name: asuro.c
* Project : asuro library
*
* Description: modifications made in following functions:
*
* void PrintInt(int wert)
*
*
* Ver. Date Author Comments
* ------- ---------- -------------- ------------------------------------------
* 2.60 28.09.2005 m.a.r.v.i.n doxygen comments
* ------- ---------- -------------- ------------------------------------------
*****************************************************************************/
/****************************************************************************
*
* File Name: asuro.c
* Project : asuro library
*
* Description: modifications made in following functions:
*
* SIGNAL (SIG_ADC)
* void PrintInt(int wert)
*
*
* Ver. Date Author Comments
* ------- ---------- -------------- ------------------------------------------
* 2.61 20.11.2006 m.a.r.v.i.n SIGNAL (SIG_ADC): static Variable toggle initialisiert
* auf False (Bug report von Rolf_Ebert)
* PrintInt: Initialisierung text String kann zu Fehler
* beim Flashen mit RS232/IR Adapter fuehren
* (Bug report von francesco)
* ------- ---------- -------------- ------------------------------------------
*****************************************************************************/
/***************************************************************************
* *
* This program is free software; you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation; either version 2 of the License, or *
* any later version. *
***************************************************************************/
#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));
(gekürzt)
Zitieren
Lesezeichen