/************************************************************************** 
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. 18F2420 mod. applied (App.3). 
Applies Salvo LITE with Library sfc18sfa. Can run on Derbot, or be simulated.
TJW 3.1.06. Rev. 10.6.09				Retested 13.6.09
***************************************************************************/
//Clock is 4MHz

#include <salvo.h>
#undef OSC	//necessary for this Salvo version, as it also defines this name
#include <p18f2420.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  	//HS oscillator 
#pragma config PWRT = ON, BOREN = OFF  //power-up timer on, brown-out detect off
#pragma config WDT = OFF	//watchdog timer off
#pragma config LVP = OFF  	//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
	PORTBbits.RB2 = 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(90);	//pause 900 us for op to be set high; ie blank for 15cm

//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, 5 is L motor enable. 
	TRISB = 0b00110000;	//bits 4 & 5 are uswitch inputs, bit 2 rt motor enable 
	TRISC = 0b11000000;	//All bits output except 7 (mode switch) & 6 (USnd echo) 
							//1 & 2 used for PWM
	ADCON1 = 0b00001111; //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;
	PORTBbits.RB2 = 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;
	PORTBbits.RB2 = 1;	//enable motor
}