Micro and ProgLog CW1: Line-follower

#include <p18F252.h>

int antiTwist = 0;	// Flag for anti-twist code.
int i = 0;			// Counting var.
int turnedAround = 0;

void allStop(void);
void turnRight(void);
void turnLeft(void);
void goStraight(void);
void adcOff(void);
void triggerPulse(void);
void catchInterrupt(void);
void setupInterrupts(void);
void turnAround(void);

#pragma code int_vector=0x08
void ISR (void) {
	_asm
	goto catchInterrupt
	_endasm
}
#pragma code

#pragma interrupt catchInterrupt
void catchInterrupt(void) {
	int p;
	int turningFlag;

	if(PORTBbits.RB0 == 1){		// If it's rising.
		IPR1bits.TMR2IP = 0;	// Tmr 2, low priority.
		PIR1bits.TMR2IF = 0;	// Tmr 2 flag cleared.
		PIE1bits.TMR2IE = 1;	// Enable overflow interrupt.

		//Timer2 Registers Prescaler= 16 - TMR2 PostScaler = 16 - PR2 = 255 - Freq = 91.91 Hz - Period = 0.010880 seconds
		T2CON |= 120;        // bits 6-3 Post scaler 1:1 thru 1:16
		T2CONbits.TMR2ON = 1;  	 // bit 2 turn timer2 on;
		T2CONbits.T2CKPS1 = 1;	 // bits 1-0  Prescaler Rate Select bits
		T2CONbits.T2CKPS0 = 0;
		PR2 = 15;        	 // PR2 (Timer2 Match value)

		INTCONbits.INT0IF = 0; // Clear interrupt flag.
	}else{
		if(PIR1bits.TMR2IF == 0){
			PORTAbits.RA4 = 0; // LED ON.
			T2CONbits.TMR2ON = 0;	// Timer off.

			allStop();
			INTCONbits.GIEH = 0; // Temporarily disable int.

			while(PORTCbits.RC0 > 0 || PORTCbits.RC1 > 0 || PORTCbits.RC2 > 0){
				if(antiTwist == 0){
					turnRight();
				}else{
					turnLeft();
				}
			}
		}else{
			PORTAbits.RA4 = 1;		// LED OFF.
		}

		PIR1bits.TMR2IF = 0;	// Tmr 2 flag cleared.
		INTCON3bits.INT1IF = 0; // Clear interrupt flag.

		for(p = 0; p <= 5000; p++){
			// Kill 12-ish mS.
		}

		triggerPulse();
	}

}

void main (void) {
	adcOff();

	PORTAbits.RA4 = 1;		// LED OFF.

	setupInterrupts();

	TRISC = 0xff;       // Port C = input
	TRISB = 0xff;		// Port B = input
	TRISA = 0x00;       // Port A = output

	allStop();			// Start with motors off.
	triggerPulse();		// Start the ultrasound.

	while(1){
			// Sensors:     RIGHT           	 MIDDLE     	     	 LEFT	

		while(PORTCbits.RC0 == 0 && PORTCbits.RC1 == 0 && PORTCbits.RC2 == 0){
			turnAround();
		}

		if(PORTCbits.RC0 == 1 && PORTCbits.RC1 == 1 && PORTCbits.RC2 == 1){
			allStop();
		}else if(PORTCbits.RC0 == 1 && PORTCbits.RC1 == 0 && PORTCbits.RC2 == 0){
			turnRight();
		}else if(PORTCbits.RC0 == 0 && PORTCbits.RC1 == 1 && PORTCbits.RC2 == 0){
			goStraight();
		}else if(PORTCbits.RC0 == 0 && PORTCbits.RC1 == 0 && PORTCbits.RC2 == 1){
			turnLeft();
		}else if(PORTCbits.RC0 == 1 && PORTCbits.RC1 == 1 && PORTCbits.RC2 == 0){
			turnRight();
		}else if(PORTCbits.RC0 == 0 && PORTCbits.RC1 == 1 && PORTCbits.RC2 == 1){
		   	turnLeft();
	   	}

		for(i = 0; i <= 2500; i++){
			// Kill some time.
		}

		// Reset the motors.
		allStop();
	}
}

void turnRight(void) {
	PORTAbits.RA0 = 0;  // Right reverse.
	PORTAbits.RA2 = 0;  // Enable right.

	PORTAbits.RA1 = 1;  // Left forward.
	PORTAbits.RA3 = 0;  // Enable left.
}

void turnLeft(void) {
	PORTAbits.RA0 = 1;  // Right forward.
	PORTAbits.RA2 = 0;  // Enable right.

	PORTAbits.RA1 = 0;  // Left reverse.
	PORTAbits.RA3 = 0;  // Enable left.
}

void goStraight(void) {
	PORTAbits.RA0 = 1;  // Right forward.
	PORTAbits.RA2 = 0;  // Enable right.

	PORTAbits.RA1 = 1;  // Left forward.
	PORTAbits.RA3 = 0;  // Enable left.
}

void allStop(void) {
	PORTAbits.RA2 = 1; 	// Disable right.
	PORTAbits.RA3 = 1; 	// Disable left.
}

void adcOff(void) {
	// The next three lines set all of PORTA to be digital,
	// disabling the ADC. According to datasheet, 110 is
	// all pins digital.
	ADCON1bits.PCFG1 = 1;
	ADCON1bits.PCFG2 = 1;
	ADCON1bits.PCFG3 = 0;
}

void triggerPulse(void) {
	int q;

	PORTAbits.RA5 = 1;   // Start pulse.
	for(q = 0; q <= 20; q++){
		// Kill 54.2-ish uS.
	}
	PORTAbits.RA5 = 0;   // Stop pulse.
}

void turnAround(void) {
			if(PORTCbits.RC0 == 0 && PORTCbits.RC1 == 0 && PORTCbits.RC2 == 0){
				while(PORTCbits.RC0 == 0 && PORTCbits.RC1 == 0 && PORTCbits.RC2 == 0){
					turnedAround = 1;
					if(antiTwist == 0){
						turnRight();
					}else{
						turnLeft();
					}
				}
			}

			if(turnedAround == 1){
				antiTwist =~ antiTwist;			//Toggle Twist
				turnedAround = 0;
			}

}

void setupInterrupts() {
	RCONbits.IPEN = 1;			// Priorities on.

	INTCONbits.INT0IF = 0;		// Clear IF.
	INTCONbits.INT0IE = 1;		// INT0 on - it's always high priority.
	INTCON2bits.INTEDG0 = 1;	// Catch INT0 on rising edge.

	INTCON3bits.INT1IF = 0;		// Clear IF.
	INTCON3bits.INT1IE = 1;		// INT1 on.
	INTCON3bits.INT1IP = 1;		// High priority.
	INTCON2bits.INTEDG1 = 0;	// Catch INT1 on falling edge.

	INTCONbits.GIEH = 1;		// Enable all high priority interrupts.
}