Code:
/************************************************************************
Test Programm fuer den ASURO IR-Adapter mit 2 Sende-LEDs
Der ASURO ermittelt ueber den nach vorne gerichteten
Infrarot Empfänger und Sender über die Reflektion des
Lichtes an einem Objekt ungefaehr dessen Abstand.
Es wird je nach Abstand der Rechten SendeLED die Status-LED in
verschiedenen Farben eingeschaltet. Wir der Abstand kleiner als 17 cm
werden die Motoren mit verschiedenen Geschwindigkeiten eingeschaltet.
Zur Compilierung dieses Programmes wird asuro.c und asuro.h aus
der ASURO Lib 2.6.1 benötigt. Die ASURO Lib ist auf sourceforge
zu finden.
************************************************************************
Hardware: ASURO Roboter
prozessor: ATMEGA32
clock: 16MHz Crystal
****************************************************************************
date authors version comment
====== ====================== ======= ==============================
Jan.08 (ch) robo.fr V1.0 First implemetation
Apr.08 (kk) pinsel V1.1 Erweiterung auf 2 SendeLEDs
Versions:
V1.0
- first version
V2.0
- Auf 2 SendeLEDs erweitert
***************************************************************************/
/***************************************************************************
*
* (c) 2007 robo.fr , christoph(at)roboterclub-freiburg.de
*
***************************************************************************
* 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 version 2 of the License, *
* If you extend the program please maintain the list of authors. *
* If you want to use this software for commercial purposes and you *
* don't want to make it open source, please contact the authors for *
* licensing. *
***************************************************************************/
#include "asuro.h"
#include <stdlib.h>
unsigned char i, servo_stellzeit;
/*************************************************************************
uint8_t objekt_sichtbar(uint8_t abstand)
Ist ein Objekt in der Entfernung kleiner oder gleich
dem Eingangsparameter "abstand" erkennbar?
objekt_sichtbar(7) liefert TRUE zurück, falls überhaupt
ein Object detektierbar.
abstand:
0: 5cm
1: 7cm
2: 13cm
3: 15cm
4: 16cm
5: 17cm
6: 18cm
7: 22cm
( Testobjekt: Joghurtecher, Umgebungsbeleuchtung: Zimmer )
return: TRUE falls Objekt gefunden
FALSE wenn nicht
Zeitbedarf: 5ms
author: robo.fr, christoph ( ät ) roboterclub-freiburg.de
date: 2008
*************************************************************************/
uint8_t objekt_sichtbar_rechts(uint8_t distance_r)
{
uint16_t j,z;
PORTB |= (1 << PB6); // PB6 auf HIGH (LED ausschalten)
DDRD |= (1 << DDD1); // Port D1 als Ausgang
PORTD &= ~(1 << PD1); // PD1 auf LOW
OCR2 = 254-distance_r; // wenn OCR2=0xFE dann Objekt sehr nahe
z=0;
for(j=0;j<30;j++) // loop time: 5ms
{
if (PIND & (1 << PD0))z++;
Sleep(6); // 6*Sleep(6)=1ms
}
if (z>=29) return FALSE; // Objekt nicht gefunden
else return TRUE;
}
uint8_t objekt_sichtbar_links(uint8_t distance_l)
{
uint16_t i,y;
PORTD |= (1 << PD1); // PD1 auf HIGH (LED ausschalten)
DDRB |= (1 << DDB6); // Port B6 als Ausgang
PORTB &= ~(1 << PB6); // PB6 auf LOW
OCR2 = 254-distance_l; // wenn OCR2=0xFE dann Objekt sehr nahe
y=0;
for(i=0;i<30;i++) // loop time: 5ms
{
if (PIND & (1 << PD0))y++;
Sleep(6); // 6*Sleep(6)=1ms
}
if (y>=29) return FALSE; // Objekt nicht gefunden
else return TRUE;
}
/*************************************************************************
uint8_t abstand()
Abstandsermittelung über IR-Sendiode / IR-Empfänger.
0:kleinster Abstand
7:größter Abstand
255: kein Objekt
Zeitbedarf: 5ms*9=45ms
author: robo.fr, christoph ( ät ) roboterclub-freiburg.de
date: 2008
*************************************************************************/
uint8_t abstand_rechts()
{
uint8_t k,n;
k=255;
for(n=0;n<4;n++)
{
if (!objekt_sichtbar_rechts(n)) k=n; // solange kein Objekt, Distanz erhoehen
}
return k;
}
uint8_t abstand_links()
{
uint8_t l,m;
l=255;
for(m=0;m<4;m++)
{
if (!objekt_sichtbar_links(m)) l=m; // solange kein Objekt, Distanz erhoehen
}
return l;
}
//************************************************************************
void RaceStart(void) // blink until any switch is pressed,
{ // then wait until switch is released
uint8_t t1, t2;
unsigned int col=OFF;
while (1) // blinking StatusLED until any switch is pressed
{
t1 = PollSwitch();
t2 = PollSwitch();
if (t1==t2)
{
if (t1)
{
break;
}
else
{
col ^= GREEN;
StatusLED(col);
}
}
Msleep(50);
}
StatusLED(OFF); // turn off StatusLED and ...
BackLED(ON,ON); // ... turn on both BackLED's
while (1) // wait until switch is released
{
t1 = PollSwitch();
t2 = PollSwitch();
if (t1==t2)
{
if (!t1)
{
break;
}
}
Msleep(50);
}
BackLED(OFF,OFF); // turn off BackLED's indication start of race
}
//************************************************************************
void servo(unsigned char winkel)
{
unsigned int count=0;
do
{
count++;
if(winkel)
{
PORTB |= (1 << PB0);
Sleep(winkel);
}
PORTB &= ~(1 << PB0);
Sleep(255-winkel);
}
while (count<servo_stellzeit);
}
/*************************************************************************
Hauptprogramm
*************************************************************************/
int main(void)
{
uint8_t n,m;
float volt;
int proz;
unsigned int sensors[2];
DDRB |= (1 << PB0);
Init();
initLcd();
volt=2.56/1024*Batterie()*22/10;
proz=(int)(((volt)/5)*100); //so ungefähr
// Output some strings to the LCD
writeStringToLCD("*ASURO bereit*");
writeStringToLCD("Batterie: ");
writeIntegerToLCD(proz);
writeStringToLCD("%");
RaceStart();
FrontLED(OFF);
while(1)
{
n=abstand_rechts();
m=abstand_links();
if ((n!=255) || (m!=255))
{
if ((n<0) && (m<0))
{
MotorDir(RWD,RWD);
MotorSpeed(155,165);
}
else if (m>n)
{
StatusLED(RED);
BackLED(ON,ON);
servo_stellzeit=12; //Servogeschw. einstellen
MotorSpeed(10,10); //kurze Pause
Msleep(1);
MotorSpeed(BREAK,BREAK);
Msleep(500);
MotorSpeed(10,10); //kurze Pause
Msleep(1);
MotorDir(FWD,FWD); //Kleines Stück vor
MotorSpeed(110,100);
Msleep(1100);
MotorSpeed(BREAK,BREAK);
clearLCD();
writeStringToLCD("Close Gripper");
for (i=15; i<85; i+=2) servo(i); //Greifer zumachen
MotorSpeed(10,10); //Kurze Pause
Msleep(1);
MotorDir(RWD,RWD); //Kleines Stück zurück
MotorSpeed(120,120);
Msleep(500);
MotorSpeed(BREAK,BREAK);
clearLCD();
writeStringToLCD("Turn left");
MotorSpeed(135,0); //Vierteldrehung nach links
Msleep(1000);
MotorSpeed(BREAK,BREAK);
clearLCD();
writeStringToLCD("Open Gripper");
for (i=75; i>17; i-=2) servo(i); //Greifer aufmachen
MotorSpeed(10,10); //Kurze Pause
Msleep(1);
MotorDir(RWD,RWD); //Kleines Stück zurück
MotorSpeed(120,120);
Msleep(500);
MotorSpeed(BREAK,BREAK);
clearLCD();
writeStringToLCD("Turn right");
MotorSpeed(0,135); //Vierteldrehung nach rechts
Msleep(1000);
MotorSpeed(BREAK,BREAK);
MotorSpeed(10,10); //Kurze Pause
Msleep(1);
clearLCD();
writeStringToLCD("That's it!");
MotorSpeed(0,155);
BackLED(ON,OFF);
clearLCD();
IRData(sensors);
writeStringToLCD("**NACH LINKS**");
writeIntegerToLCD(sensors[0]);
writeStringToLCD("*Wert*");
writeIntegerToLCD(sensors[1]);
}
else if (n>m)
{
StatusLED(RED);
BackLED(ON,ON);
servo_stellzeit=12; //Servogeschw. einstellen
MotorSpeed(10,10); //kurze Pause
Msleep(1);
MotorSpeed(BREAK,BREAK);
Msleep(500);
MotorSpeed(10,10); //kurze Pause
Msleep(1);
MotorDir(FWD,FWD); //Kleines Stück vor
MotorSpeed(95,120);
Msleep(1100);
MotorSpeed(BREAK,BREAK);
clearLCD();
writeStringToLCD("Close Gripper");
for (i=15; i<85; i+=2) servo(i); //Greifer zumachen
MotorSpeed(10,10); //Kurze Pause
Msleep(1);
MotorDir(RWD,RWD); //Kleines Stück zurück
MotorSpeed(120,120);
Msleep(500);
MotorSpeed(BREAK,BREAK);
clearLCD();
writeStringToLCD("Turn right");
MotorSpeed(0,135); //Vierteldrehung nach links
Msleep(1000);
MotorSpeed(BREAK,BREAK);
clearLCD();
writeStringToLCD("Open Gripper");
for (i=75; i>17; i-=2) servo(i); //Greifer aufmachen
MotorSpeed(10,10); //Kurze Pause
Msleep(1);
MotorDir(RWD,RWD); //Kleines Stück zurück
MotorSpeed(120,120);
Msleep(500);
MotorSpeed(BREAK,BREAK);
clearLCD();
writeStringToLCD("Turn left");
MotorSpeed(135,0); //Vierteldrehung nach rechts
Msleep(1000);
MotorSpeed(BREAK,BREAK);
MotorSpeed(10,10); //Kurze Pause
Msleep(1);
clearLCD();
writeStringToLCD("That's it!");
MotorSpeed(155,0);
BackLED(OFF,ON);
clearLCD();
IRData(sensors);
writeStringToLCD("**NACH RECHTS**");
writeIntegerToLCD(sensors[0]);
writeStringToLCD("*Wert*");
writeIntegerToLCD(sensors[1]);
}
else
{
StatusLED(GREEN);
BackLED(ON,ON);
MotorDir(FWD,FWD);
MotorSpeed(155,165);
clearLCD();
IRData(sensors);
writeStringToLCD("***GERADEAUS***");
writeIntegerToLCD(sensors[0]);
writeStringToLCD("*Wert*");
writeIntegerToLCD(sensors[1]);
}
}
}
}
Du kannst dir sicher die Servoschnipsel rauspflücken.
Zitieren
Lesezeichen