#include <p18f4550.h>
#include <timers.h>
#include <adc.h>
#include <delays.h>
#include "lcd.h"
#include <pwm.h>
 
// Fuses configuration
#pragma config PLLDIV = 5       // Divide by 5 (20 MHz oscillator input)
#pragma config FOSC = HSPLL_HS  // HS oscillator, PLL enabled, HS used by USB
#pragma config IESO = OFF       // Oscillator Switchover mode disabled
#pragma config PWRT = OFF       // PWRT disabled
#pragma config BOR = OFF        // Brown-out Reset enabled in hardware only (SBOREN is disabled)
#pragma config WDT = OFF        // HW Disabled - SW Controlled
#pragma config WDTPS = 32768    // 1:32768
#pragma config MCLRE = ON       // MCLR pin enabled; RE3 input pin disabled
#pragma config LVP = OFF        // Disable low-voltage programming
#pragma config CCP2MX = ON      // CCP2 multiplexed to RC1 and not to RB3
#pragma config PBADEN = OFF     // PORB digital IO on powerup
 
#define TRUE 1
//De 4 drukknoppen definen (wel maar 1tje nodig)
#define S_N             PORTBbits.RB4
#define S_E             PORTBbits.RB1
#define S_S             PORTBbits.RB5
#define S_W             PORTBbits.RB0
#define S_C             PORTBbits.RB2
 
void main(void) {
 unsigned char j;  //voor een for
 unsigned char i;  //voor een andere for
//Het parkoer dat mijn robot moet afleggen. De eerste 5 stappen zouden genoeg zijn
 int parkoer[10];	//1 rechtdoor, 2 links, 3 rechts, 4 achteruit
	 parkoer[0] = 1;
	 parkoer[1] = 1;
	 parkoer[2] = 2;
	 parkoer[3] = 1;
	 parkoer[4] = 2;
	 parkoer[5] = 1;
	 parkoer[6] = 1;
	 parkoer[7] = 2;
	 parkoer[8] = 1;
	 parkoer[9] = 2;

TRISB = 0xFF;  // port B as inputs (buttons)
INTCON2bits.NOT_RBPU = 0;  // enable port B pull-up (buttons)

  initLCD();
  clearLCD();
  LCDBacklightOn();

  // set motor pins as output
  TRISAbits.TRISA4 = 0;
  TRISBbits.TRISB3 = 0; 
  TRISCbits.TRISC1 = 0;
  TRISCbits.TRISC2 = 0;
 
  PORTAbits.RA4 = 0; //ground
  PORTBbits.RB3 = 0; //ground
 
  // PWM period = (0xFF + 1)*4*Tosc*T2Prescaler
  OpenPWM1(0xFF);  // configuring PWM module 1
  OpenPWM2(0xFF);  // configuring PWM module 2
 
  // Configuring timer 2 which provides timing for PWM
  // TIMER_INT_OFF: disable timer interrupt
  // T2_PS_1_4: Timer2 prescaling set to 4
  // T2_POST_1_1: Timer2 postscaling set to 1
  OpenTimer2(TIMER_INT_OFF & T2_PS_1_4 & T2_POST_1_1);
 
  SetDCPWM1(0);   // sets PWM duty cycle
  SetDCPWM2(0);   // sets PWM duty cycle

  appendStringToLCD("Druk C om te beginnen.");
while(TRUE) {
  if(S_C == 0){//als er op C is gedruk, het programma starten
	  clearLCD();
      appendStringToLCD("Made by Stienoo");
    while(TRUE){
	for(j=0; j<sizeof(parkoer)/sizeof(int); j++){
	  if(parkoer[j] == 1){ //vooruit
		  SetDCPWM1(1023);
		  SetDCPWM2(1023);
	      PORTAbits.RA4 = 0;
	      PORTBbits.RB3 = 0;
	     for(i=0; i<5; i++){ //even wachten
		     Delay10KTCYx(255);
		 }
	    }
	 if(parkoer[j] == 4){  //achteruit
		  SetDCPWM1(1023-1023);
		  SetDCPWM2(1023-1023);
	      PORTAbits.RA4 = 1;
	      PORTBbits.RB3 = 1;
	     for(i=0; i<5; i++){
		     Delay10KTCYx(255);
		 }
	    }
	 if(parkoer[j] == 2){  //links
		  SetDCPWM1(1023);
		  SetDCPWM2(1023-1023);
	      PORTAbits.RA4 = 0;
	      PORTBbits.RB3 = 1;
	     for(i=0; i<5; i++){
		     Delay10KTCYx(255);
		 }
	    }
	 if(parkoer[j] == 0){ //rechts
		  SetDCPWM1(1023-900);
		  SetDCPWM2(900);
	      PORTAbits.RA4 = 1;
	      PORTBbits.RB3 = 0;
	     for(i=0; i<5; i++){
		     Delay10KTCYx(255);
		 }
	    }
	}
	   SetDCPWM2(0);
	   SetDCPWM1(0);
	   PORTAbits.RA4 = 0;
	   PORTBbits.RB3 = 0;
	  }	
	Delay10TCYx(150);     // wait a bit
  }
 }
}