[AtMEGA8][C] CNY70 + LF - własny kod

[Keadwen]

Witam

Jestem aktualnie podczas ostatniego kroku do ukończenia pierwszego robota, Line Followera opartego na trzech czujnikach CNY70. Robot jest zmontowany i jeździ, lecz nie do końca tak jak bym chciał.

Schemat został zaczerpnięty z posta

Przepis na programowalnego robota

jednakże program chciałem napisać sam.

Przy okazji dodam, że widziałem już tego typu temat na tej stronie, ale nie chciałem go refreshować i stworzyłem nowy.

Mój program wygląda o tak:

#include <avr/io.h>
#include <util/delay.h>
#include <stdbool.h>

#define M1_A	PD0
#define M1_B	PD1
#define M2_A	PD2
#define M2_B	PD3

// define LED'a kontrolnego
#define LED_PIN	(1<<PD6)
#define LED_ON	PORTD |= LED_PIN
#define LED_OFF PORTD &= ~LED_PIN

// define czujników
#define CZUJNIK_LEFT	(1<<PC3)
#define CZUJNIK_MIDDLE	(1<<PC4)
#define CZUJNIK_RIGHT	(1<<PC5)
#define CZUJNIK_LEFT_STAN	!(PINC & CZUJNIK_LEFT)
#define CZUJNIK_MIDDLE_STAN	!(PINC & CZUJNIK_MIDDLE)
#define CZUJNIK_RIGHT_STAN	!(PINC & CZUJNIK_RIGHT)

// define silniczka M1
#define M1_JAZDA	PORTD |= (1<<M1_A);	PORTD &= ~(1<<M1_B)
#define M1_COFKA	PORTD &= ~(1<<M1_A);	PORTD |= (1<<M1_B)
#define M1_STOP		PORTD &= ~( (1<<M1_A) | (1<<M1_B) )

// define silniczka M2
#define M2_JAZDA	PORTD |= (1<<M2_A);	PORTD &= ~(1<<M2_B)
#define M2_COFKA	PORTD &= ~(1<<M2_A);	PORTD |= (1<<M2_B)
#define M2_STOP		PORTD &= ~( (1<<M2_A) | (1<<M2_B) )

int main(void)
{
DDRC &= ~(CZUJNIK_LEFT | CZUJNIK_MIDDLE | CZUJNIK_RIGHT);	// CZUJNIKI
// PORTC |= (CZUJNIK_LEFT | CZUJNIK_MIDDLE | CZUJNIK_RIGHT);    // BRAK WEW. REZYSTORA

DDRD |= ((1<<PD0) | (1<<PD1) | (1<<PD2) | (1<<PD3));	// SERVO

DDRD |= LED_PIN;	// LED
PORTD |= LED_PIN;

while(1)
{
	static bool cny_mid;
	static bool cny_left;
	static bool cny_right;
// *********************************
	if(CZUJNIK_MIDDLE_STAN)      // TEST WYKRYWANIA 0[V]
		cny_mid = false;             // 
	else
		cny_mid = true;
// *********************************
	if(CZUJNIK_LEFT_STAN)
		cny_left = false;
	else
		cny_left = true;
// *********************************
	if(CZUJNIK_RIGHT_STAN)
		cny_right = false;
	else
		cny_right = true;
// *********************************

	if(cny_mid == false)       // KILKA PETLI (ZALEZNOŚCI)
	{
		if(cny_left == false)
			if(cny_right == false)
			{
				M1_JAZDA;
				M2_STOP;
			}

		if(cny_left == true)
		{
			M1_JAZDA;
			M2_STOP;
		}

		if(cny_right == true)
		{
			M1_STOP;
			M2_JAZDA;
		}
	}

	if(cny_mid == true)
	{
		M1_JAZDA;
		M2_JAZDA;
		LED_ON;
	}
}
}

Kod ten pisałem z 10min i jestem pewny, że gdzieś są błędy, bo robo nie wykrywa nic a nic. Jednak zastanawiam się jeszcze nad tym czy czujniki dobrze są ułożone, gdyż mam je lekko nachylone względem podłoża.

Pozdrawiam i liczę na pomoc,
Keadwen