/************************************************************************** 
rtos_ex3									
Implements Derbot Blind Navigation, with upward-looking US sensor to 
detect if AGV is going under an overhang. 
Tasks are: Ultrasound Sensor (higher priority), Motor_set (lower priority). 
Interrupts are: Microswitch (Low priority) and Timer 0 (High, for clock tick). 
One message and numerous delays are also used.
Applies Salvo LITE with Library sfc18sfa. Can run on Derbot, or be simulated.
TJW 3.1.06												Tested 7.1.06
***************************************************************************/
//Clock is 4MHz

#include <salvo.h>
#undef OSC	//necessary for this Salvo version, as it also defines this name
#include <p18f242.h>
#include <timers.h>		//header file for delays
#include <delays.h>		//header file for delays
#include <pwm.h>		//header file for PWM

#pragma config OSC = HS, OSCS = OFF  //HS oscillator, oscillator switch off 
#pragma config PWRT = ON, BOR = OFF  //pwr-up timer on, brown-out detect off
#pragma config WDT = OFF		     //watchdog timer off
#pragma config STVR = ON, LVP = OFF  //Stack overflow reset enable on, 
						//low voltage programming off

//User-defined function prototypes
	void Micro_Init(void);
	void leftmot_fwd (void);
	void rtmot_fwd (void);
	void leftmot_rev (void);
	void rtmot_rev  (void);

//These functions are tasks.
	void Motor_Task( void ); 	//Sets motor according to messages received
	void USnd_Task( void );		//Fires Ultrasound Sensor periodically

//Define labels for context switches 
_OSLabel(Motor_Task1)  
_OSLabel(Motor_Task2)  
_OSLabel(Motor_Task3)
_OSLabel(Motor_Task4)
_OSLabel(Motor_Task5)
_OSLabel(USnd_Task1)
_OSLabel(USnd_Task2)
_OSLabel(USnd_Task3)
_OSLabel(LED_Task1)
_OSLabel(LED_Task2)

//Carries messages from microswitch and ultrasound
#define Msg_to_Motor OSECBP(1) 

	char Hole = 0x18;	//This value used, but never tested

/***************************************************************************
User-defined Functions, including RTOS Tasks.
****************************************************************************/
//This task controls motor action, determined by messages received from elsewhere
void Motor_Task( void )
{	static char msge;    //hold message once recd
	OStypeMsgP msgP;	//Declare msgP as special Salvo pointer type
	for (;;) 			//set up the infinite Task loop
	{					 
  	rtmot_fwd ();			//set motors running forward. This is status quo 
	leftmot_fwd ();				//until message arrives
	//Wait for message
	OS_WaitMsg(Msg_to_Motor,&msgP,OSNO_TIMEOUT,Motor_Task1); 
//Proceed when message arrives
	msge = *(char*)msgP;
	PORTAbits.RA5 = 0;			//stop motors for 500ms
	PORTAbits.RA2 = 0;
	OS_Delay (50,Motor_Task1);
	if( (msge == 0x80)||(msge == 0x01) ) //was it a microswitch?
		{			
		rtmot_rev ();			//Yes, so both motors reverse
		leftmot_rev ();
		OS_Delay (100,Motor_Task2);
		if(msge == 0x80) 		//was left uswitch hit?
			{leftmot_fwd ();	//Yes, so turn right
			OS_Delay (80,Motor_Task3);
			}
			else				//right uswitch was hit
			{rtmot_fwd ();		//so turn left
			OS_Delay (80,Motor_Task4);
			}
		}
		else					//We're under an overhang, hence turn on spot
		{rtmot_rev ();	
		leftmot_fwd ();		 
		OS_Delay (200,Motor_Task5);
		}
	}			   				//end of "for" loop
}

/*This task periodically pulses the Ultrasound, and sends a message if an overhang
detected. In this case, it suspends pulsing, to allow Derbot to exit*/
void USnd_Task(void)
{	int echo_time = 0;
    for (;;) 			//set up the infinite Task loop
	{				
 	OS_Delay (20,USnd_Task1); //Task switch, and delay for 20x10ms, (200ms)
	OSDi();					//disable interrupts, this measurement is time sensitive
	echo_time = 0;
	PORTCbits.RC5 = 1;		//output us pulse. 
	Delay10TCYx(2);			//20us delay approx, gives pulse width
	PORTCbits.RC5 = 0;	
	Delay10TCYx(30);	//pause for op to be set high; ie blank for 5cm

//Values in this loop are adjusted experimentally to give detection threshold 
		//of 30cm approx
	while (echo_time < 50)		//limit the measurement to close objects
		{Delay10TCYx(1);		//10us delay 
		echo_time++;			//increment the counter
		if(PORTCbits.RC6 == 0)	//test, and send message if target detected
			{OSSignalMsg(Msg_to_Motor,(OStypeMsgP)&Hole);
			OSEi();				//enable interrupts before delay
			OS_Delay (250,USnd_Task2); //Suspend the USnd, to allow Derbot to exit "hole"			
			OS_Delay (250,USnd_Task3);
			break;
			}
	 	}
	OSEi();				//enable interrupts
   OS_Yield(USnd_Task2);
    }					//end of "for" loop
}

//This function initialises the Microcontroller peripherals
void Micro_Init(void)
{
//Initialise Ports
	TRISA = 0b00000000;	//All bits output, 2 & 5 used for motor enables. 
	TRISB = 0b00110000;	//Bits 5 and 4 (microswitches) only are input, 
	TRISC = 0b11000000;	//All bits output except 7 (mode switch) & 6 (USnd echo) 
							//1 & 2 used for PWM
	ADCON1 = 0b00000110; //Set Port A for digital i/o
//Switch all outputs off
	PORTA = PORTB = PORTC = 0;

/*Initialise Timer 0: interrupt enabled,16-bit operation, internal clock, 
prescaler divide by 16, hence (with 4MHz clock) input cycle period of 16us*/
    OpenTimer0 (TIMER_INT_ON & T0_SOURCE_INT & T0_16BIT & T0_PS_1_16);
	WriteTimer0 (64918);		//and initialise
//Initialise PWM
	OpenTimer2 (TIMER_INT_OFF & T2_PS_1_1 & T2_POST_1_1);
	OpenPWM1 (0xFF); //Enable PWM1 and set period
	OpenPWM2 (0xFF); //Enable PWM2 and set period
//Set Port B Interrupt on change to be low priority
	RCONbits.IPEN = 1;   //Enable low priority interrupts
    INTCON2bits.RBIP = 0; //Set port change bit to be low priority 
 	INTCONbits.RBIE = 1;//Enable Port B change interrupt
}

/***************************************************************************
Main
****************************************************************************/
void main( void )
{
//Initialise Microcontroller
	Micro_Init();
	Delay10KTCYx (250);		//pause 2.5secs with conventional delay
//Initialise RTOS
    OSInit();
//Create Tasks and Message
    OSCreateTask(Motor_Task, OSTCBP(1), 4);  
    OSCreateTask(USnd_Task, OSTCBP(2), 2);
    OSCreateMsg(Msg_to_Motor, (OStypeMsgP)0);
//Enable Global interrupts
   OSEi(); 		
//Scheduling Loop
    for (;;)
        OSSched();
}

/**************************************************************
Motor Drive Functions
***************************************************************/
void leftmot_fwd (void)	//sets left motor running forward
{	CCPR2L = 196;
	PORTAbits.RA5 = 1;	//enable motor
}

void rtmot_fwd (void)	//sets right motor running forward
{	CCPR1L = 196;
	PORTAbits.RA2 = 1;	//enable motor
}

void leftmot_rev (void)	//sets left motor running in reverse
{	CCPR2L = 60;
	PORTAbits.RA5 = 1;	//enable motor
}

void rtmot_rev (void)	//sets right motor running in reverse
{	CCPR1L = 60;
	PORTAbits.RA2 = 1;	//enable motor
}
