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
Lesezeichen