Habe es endlich geschafft. Jeder Sensor (IR-Phototransistor) wird jetzt angesteuert. Habe es mit meinem Wissen unter Hilfe der hier im Beitrag genannten Quellen programmiert. Das Problem war hier noch eine „kalte“ Lötstelle .. Aber auch mein Programm.
Habe hier das Beispiel von Roboter.cc erweitert. Hier habe ich den Fall für Transistor T3 & T4 vertauscht, da hier bei…
T4 -> D9, D10, D11, D12 ..
&
T3 -> D13, D14, D15, D16…
…geschaltet sind, und somit eine Messung im Uhrzeigersinn in richtiger Reihenfolge stattfindet.
Code:
#include <nibobee/iodefs.h>
#include <nibobee/led.h>
#include <nibobee/delay.h>
#include <avr/io.h>
#define T1 (1 << PC0)
#define T2 (1 << PC2)
#define T3 (1 << PC3)
#define T4 (1 << PC1)
#define IR_LED_A (1 << PA3)
#define IR_LED_B (1 << PA2)
#define IR_LED_AB ((1 << PA2) | (1 << PA3))
#define IR_LED_OFF 0x00
void adc_init(void);
uint16_t ADC_Read( uint8_t channel );
uint16_t ADC_Read_Avg( uint8_t channel, uint8_t nsamples );
void setupTransistor(uint8_t val);
void setupIR(uint8_t val);
uint16_t messen(uint8_t i);
void led(uint8_t i_led);
int main ()
{
adc_init();//AD-Wandler initialisieren
led_init();//Standard LEDs initialisieren
//Ausgänge
DDRA |= (IR_LED_A | IR_LED_B);//IR-LED PINs als Ausgang
DDRC |= (T1|T2|T3|T4); // Transistor-PINs als Ausgang
DDRD |= ((1 << DDD0) | (1 << DDD1)); //Obere LEDs als Ausgang
while(1==1)
{
enable_interrupts();
uint8_t i;
uint16_t adcval;
for (i = 1; i<9; ++i)
{
adcval = messen(i);
if (adcval > 8)
{
led(i);
}
else
{
led(0);
}
delay(30);
}
}
return 0;
}
/* ADC initialisieren */
void adc_init(void)
{
uint16_t result;
/* REFS1...REFS0 (ReferenceSelection Bits) Mit diesen Bits kann die Referenzspannung eingestellt werden
- interne Referenzspannung als Refernz */
ADMUX = (1 << REFS1) | (1 << REFS0 );
ADCSRA = (1 << ADEN) /*ADEN (ADC Enable)*/
| (1 << ADPS2 ) | (1 << ADPS1 ) | (1 << ADPS0 ) ;
/*ADPS2...ADPS0 (ADC Prescaler Select Bits) - Diese Bits bestimmen den Teilungsfaktor
zwischen der Taktfrequenz und dem Eingangstakt des ADC - mit 15 MHz / 128 ~ 120 kHz*/
/* nach Aktivieren des ADC wird ein "Dummy-Readout" empfohlen, man liest
also einen Wert und verwirft diesen, um den ADC "warmlaufen zu lassen" */
ADCSRA |= (1 << ADSC); /*ADC Start Conversion - eine ADC-Wandlung */
while (ADCSRA & (1 << ADSC))
{
// auf Abschluss der Konvertierung warten
}
/* ADC muss einmal gelesen werden, sonst wird Ergebnis der nächsten
Wandlung nicht übernommen. */
result = ADC ;
}
/* ADC Einzelmessung */
uint16_t ADC_Read( uint8_t channel )
{
// Kanal waehlen, ohne andere Bits zu beeinflußen
ADMUX = (ADMUX & ~(0x1F)) | (channel & 0x1F);
ADCSRA |= (1<<ADSC); // eine Wandlung "single conversion"
while (ADCSRA & (1<<ADSC) )
{ // auf Abschluss der Konvertierung warten
}
return ADC; // ADC auslesen und zurückgeben
}
/* ADC Mehrfachmessung mit Mittelwertbbildung */
/* beachte: Wertebereich der Summenvariablen */
uint16_t ADC_Read_Avg( uint8_t channel, uint8_t nsamples )
{
uint32_t sum = 0;
for (uint8_t i = 0; i < nsamples; ++i )
{
sum += ADC_Read( channel );
}
return (uint16_t)( sum / nsamples );
}
// Transistor auswählen / Spalte nach Schaltplan selektieren (T1-T4)
void setupTransistor(uint8_t val) {
PORTC |= (T1|T2|T3|T4); // alle aus, da Basis der PNP-Transistoren auf high (VCC)
PORTC &= ~val;
}
// IR-LED auswählen / Zeile nach Schaltplan selektieren (unten: IR_LED_B, oben: IR_LED_A)
void setupIR(uint8_t val) {
PORTA |= (IR_LED_A | IR_LED_B);//IR-LEDs aus
PORTA &= ~val;
}
//LED, je nach IR-Reflexion
void led(uint8_t i_led)
{
uint8_t fun_led_Nr1 = 0;
uint8_t fun_led_Nr2 = 0;
uint8_t fun_led_Nr3 = 0;
uint8_t fun_led_Nr4 = 0;
uint8_t fun_led_Nr5 = 0;
uint8_t fun_led_Nr6 = 0;
switch (i_led)
{
case 0: fun_led_Nr1 = 0; fun_led_Nr2 = 0; fun_led_Nr3 = 0; fun_led_Nr4 = 0; fun_led_Nr5 = 0; fun_led_Nr6 = 0; break;
case 1: fun_led_Nr1 = 1; fun_led_Nr2 = 0; fun_led_Nr3 = 0; fun_led_Nr4 = 0; fun_led_Nr5 = 1; fun_led_Nr6 = 0; break;
case 2: fun_led_Nr1 = 0; fun_led_Nr2 = 1; fun_led_Nr3 = 0; fun_led_Nr4 = 0; fun_led_Nr5 = 1; fun_led_Nr6 = 0; break;
case 3: fun_led_Nr1 = 1; fun_led_Nr2 = 0; fun_led_Nr3 = 0; fun_led_Nr4 = 0; fun_led_Nr5 = 1; fun_led_Nr6 = 0; break;
case 4: fun_led_Nr1 = 0; fun_led_Nr2 = 1; fun_led_Nr3 = 0; fun_led_Nr4 = 0; fun_led_Nr5 = 1; fun_led_Nr6 = 0; break;
case 5: fun_led_Nr1 = 0; fun_led_Nr2 = 0; fun_led_Nr3 = 1; fun_led_Nr4 = 0; fun_led_Nr5 = 0; fun_led_Nr6 = 1; break;
case 6: fun_led_Nr1 = 0; fun_led_Nr2 = 0; fun_led_Nr3 = 0; fun_led_Nr4 = 1; fun_led_Nr5 = 0; fun_led_Nr6 = 1; break;
case 7: fun_led_Nr1 = 0; fun_led_Nr2 = 0; fun_led_Nr3 = 1; fun_led_Nr4 = 0; fun_led_Nr5 = 0; fun_led_Nr6 = 1; break;
case 8: fun_led_Nr1 = 0; fun_led_Nr2 = 0; fun_led_Nr3 = 0; fun_led_Nr4 = 1; fun_led_Nr5 = 0; fun_led_Nr6 = 1; break;
}
led_set(LED_L_YE, fun_led_Nr1);
led_set(LED_L_RD, fun_led_Nr2);
led_set(LED_R_RD, fun_led_Nr3);
led_set(LED_R_YE, fun_led_Nr4);
if (fun_led_Nr5)
{
PORTD |= (1 << PD0);
}
else
{
PORTD &= ~(1 << PD0);
}
if (fun_led_Nr6)
{
PORTD |= (1 << PD1);
}
else
{
PORTD &= ~(1 << PD1);
}
}
// IR-Reflexion messen
// im Uhrzeigersinn: 1 (links hinten) bis 8 (rechts hinten)
uint16_t messen(uint8_t i)
{
setupIR(IR_LED_OFF); // IR-LEDs aus
switch (i)
{
case 1: setupTransistor(T1); break; //D1, D2, D3, D4
case 2: setupTransistor(T1); break;
case 3: setupTransistor(T2); break; //D5, D6, D7, D8
case 4: setupTransistor(T2); break;
case 5: setupTransistor(T4); break; //D9, D10, D11, D12
case 6: setupTransistor(T4); break;
case 7: setupTransistor(T3); break; //D13, D14, D15, D16
case 8: setupTransistor(T3); break;
}
int16_t val = 0;
switch (i)
{
case 1:
setupIR(IR_LED_B);
delay(1);
val = ADC_Read(1);
setupIR(IR_LED_OFF);
delay(1);
val -= ADC_Read(1);
break;
case 2:
setupIR(IR_LED_A);
delay(1);
val = ADC_Read(0);
setupIR(IR_LED_OFF);
delay(1);
val -= ADC_Read(0);
break;
case 3:
setupIR(IR_LED_B);
delay(1);
val = ADC_Read(1);
setupIR(IR_LED_OFF);
delay(1);
val -= ADC_Read(1);
break;
case 4:
setupIR(IR_LED_A);
delay(1);
val = ADC_Read(0);
setupIR(IR_LED_OFF);
delay(1);
val -= ADC_Read(0);
break;
case 5:
setupIR(IR_LED_A);
delay(1);
val = ADC_Read(0);
setupIR(IR_LED_OFF);
delay(1);
val -= ADC_Read(0);
break;
case 6:
setupIR(IR_LED_B);
delay(1);
val = ADC_Read(1);
setupIR(IR_LED_OFF);
delay(1);
val -= ADC_Read(1);
break;
case 7:
setupIR(IR_LED_A);
delay(1);
val = ADC_Read(0);
setupIR(IR_LED_OFF);
delay(1);
val -= ADC_Read(0);
break;
case 8:
setupIR(IR_LED_B);
delay(1);
val = ADC_Read(1);
setupIR(IR_LED_OFF);
delay(1);
val -= ADC_Read(1);
break;
}
if (val<0)
{
val=0;
}
return val;
}
Zitat von
elektrolutz
Hallo Christian Strasser,
Welver - Werl, das sieht ja nach Nachbarschaft aus.
..ja ist nicht sooooo weit weg
Lesezeichen