// Inglnook.C	30-Dec-2008		Loren Blaney	loren.blaney@idcomm.com
// Inglenook Sidings Controller
//
// Allow for a maximum speed of 10 inches per second, and a minimum of 1"/sec.
//
// WARNING: Do not attempt to run with ICD2 plugged in.
// Remove jumper across diode when B15 is changed to an analog input.

#include <p24fj64ga002.h>

_CONFIG1(JTAGEN_OFF & GCP_OFF & GWRP_OFF & BKBUG_OFF & COE_OFF & FWDTEN_OFF & WINDIS_OFF)
_CONFIG2(IESO_OFF & FNOSC_FRC & FCKSM_CSDCMD & OSCIOFNC_ON & IOL1WAY_OFF & I2C1SEL_PRI & POSCMOD_NONE)

#define LEFT 0
#define RIGHT 1
#define OFF 0
#define ON 1

#define FwdSw	PORTBbits.RB0
#define RevSw	PORTBbits.RB1
#define TurnSw1	PORTBbits.RB2
#define TurnSw2	PORTBbits.RB3
#define ReedSw1	PORTAbits.RA0
#define ReedSw2	PORTAbits.RA1
#define ReedSw3	PORTAbits.RA2
#define ReedSw4	PORTAbits.RA3
#define ReedSw5	PORTAbits.RA4
#define ModeSw	PORTBbits.RB15

#define TrackPWM LATBbits.LATB5
#define TrackA	 LATBbits.LATB6
#define TrackB	 LATBbits.LATB7

#define Turnout1A LATBbits.LATB8
#define Turnout1B LATBbits.LATB9
#define Turnout2A LATBbits.LATB10
#define Turnout2B LATBbits.LATB11

#define Uncoupler1 LATBbits.LATB12
#define Uncoupler2 LATBbits.LATB13
#define Uncoupler3 LATBbits.LATB14



void DelayMS(int t)		// Delay specified number of milliseconds
{int i, j;
for (i=0; i<t; i++)
	for (j=0; j<367*2; j++)		// simple-minded delay loop
		asm volatile ("nop");
} // DelayMS



void MoveFwd()			// Move train forward
{TrackPWM = 1;
asm volatile ("nop");
TrackB = 0;
asm volatile ("nop");
TrackA = 1;
} // MoveFwd



void MoveRev()			// Move train backward (reverse)
{TrackPWM = 1;
asm volatile ("nop");
TrackB = 1;
asm volatile ("nop");
TrackA = 0;
} // MoveRev



void Stop()			// Stop train movement
{TrackPWM = 0;
} // Stop



int Reed(int n)			// Returns state of specified reed switch (on = true)
//	1
//	2   4
//	3       5
{int t;
switch (n)
	{
	case 1:	t = ReedSw1;
		break;
	case 2:	t = ReedSw2;
		break;
	case 3:	t = ReedSw3;
		break;
	case 4:	t = ReedSw4;
		break;
	case 5:	t = ReedSw5;
	}
if (t) t=0; else t=1;		// grounded switch is 'true'
DelayMS(1);			// delay past any bouncing
return t;
} // Reed



void SetTurnout(int n, int rt)	// Set specified turnout to specified direction
{				//  (Does not wait for movement to complete)
switch (n)
	{
	case 1:	if (rt) {Turnout1A = 0;  asm volatile ("nop");  Turnout1B = 1;}
		else	{Turnout1A = 1;  asm volatile ("nop");  Turnout1B = 0;}
		break;
	case 2:	if (rt) {Turnout2A = 0;  asm volatile ("nop");  Turnout2B = 1;}
		else	{Turnout2A = 1;  asm volatile ("nop");  Turnout2B = 0;}
	}
} // SetTurnout



void SelectSiding(int s)	// Set turnouts to specified siding
{switch (s)
	{
	case 1:	{SetTurnout(2, RIGHT);  SetTurnout(1, RIGHT);  break;}	// top siding
	case 2: {SetTurnout(2, RIGHT);  SetTurnout(1, LEFT);  break;}	// middle siding
	case 3: SetTurnout(2, LEFT);					// bottom siding
	}
int i;
for (i=0; i<22; i++)		// wait for turnouts to finish moving
	{DelayMS(100);
	if (ModeSw == 1) return;
	}
} // SetTurnout



void SetUncoupler(int n, int on)	// Turn specified uncoupler on or off
{switch (n)
	{
	case 1:	Uncoupler1 = on;
		break;
	case 2:	Uncoupler2 = on;
		break;
	case 3:	Uncoupler3 = on;
	}
} // SetUncoupler



void Fatal()			// Flash all LEDs to indicate error condition
{Stop();			// put layout in safe mode
SetUncoupler(1, OFF);
SetUncoupler(2, OFF);
SetUncoupler(3, OFF);

// set LED inputs as outputs

while (1) {			// continuously flash LEDs until reset occurs
	// turn on all LEDs
	DelayMS(250);
	// turn off all LEDs
	DelayMS(250);
	if (ModeSw == 1) return;
	}
} // Fatal



void Skip(int n)		// Move one car past the specified reed switch
{
do	{if (ModeSw == 1) return;}	// wait until on reed switch
while (! Reed(n));

while (Reed(n))				// wait until past reed switch
	{if (ModeSw == 1) return;}
} // Skip



void Uncouple(int n)		// Uncouple next car on siding n
{L100:
MoveRev();			// back train
while (Reed(n))			// if car is on reed then back until it's off
	{if (ModeSw == 1) return;}
do	{if (ModeSw == 1) return;} //keep backing until reed goes on
while (! Reed(n));
Stop();				// wait for train to coast to a halt
DelayMS(500);			// (so it doesn't throw cars backward)

MoveRev();			// back train to release tension in couplers
DelayMS(50);

SetUncoupler(n, ON);
while (Reed(n))			// keep backing if still on reed
	{if (ModeSw == 1) goto L200;}
DelayMS(100);			// back a maximum of 1"

MoveFwd();			// move forward
int i;
for (i=0; i<400; i++)		// check to see that car uncoupled
	{DelayMS(1);
	if (Reed(n))		// if reed comes on then car didn't uncouple
		{SetUncoupler(n, OFF);
		while (Reed(n))	// wait for car to pass reed
			{if (ModeSw == 1) return;}
		DelayMS(200);	// move forward (less than a car length)
		goto L100;	// go try to uncouple again
		}
	if (i == 100) SetUncoupler(n, OFF); // don't leave magnet on too long
	if (Reed(4) && i>100) break; // don't keep checking if a clearance reed
	if (Reed(5) && i>100) break; //  gets activated (but check a little)
	if (ModeSw == 1) break;
	}
L200:
SetUncoupler(n, OFF);		// make sure uncoupler is off
Stop();
} // Uncouple



void Couple(int n)		// Couple a car on siding n
{while (1)
	{
	MoveRev();		// back train
	while (Reed(n))		// if car is on reed then back until it's off
		{if (ModeSw == 1) return;}
	Skip(n);		// wait for car ahead to pass reed
				// car should now be coupled
	MoveFwd();
	Skip(n);		// wait for car ahead to pass reed
	int i;
	for (i=0; i<400; i++)	// check to make sure that car did couple
		{DelayMS(1);
		if (Reed(n))	// if reed comes on then car coupled
			{Skip(n); goto L200;}
		if (Reed(4)) goto L200;	// don't keep checking if a clearance
		if (Reed(5)) goto L200;	//   reed gets activated
		if (ModeSw == 1) return;
		}
	}
L200:
Stop();
} // Couple



void Test()			// Uncoupler tester (for debug)
{
SelectSiding(3);
Couple(3);			// car
MoveFwd();
Skip(5);			// coal car
Skip(5);			// car

Uncouple(3);			// car
MoveFwd();
Skip(5);			// coal car
Stop();
} // Test



void Reverse()			// Reverse sequence of cars
{
// Cars are initially arranged as follows:
//	siding	  cars
//	  1	   - -
//	  2	   - -
//	  3	 1 2 3 4 5 Eng
//
// uncouple cars 1, 2 & 3
SelectSiding(3);
Uncouple(3);			// cars 1, 2 & 3
MoveFwd();
Skip(5);			// car 1
Skip(5);			// car 2
Stop();
if (ModeSw == 1) return;

// move cars 4 & 5 to siding 2
SelectSiding(2);
MoveRev();
Skip(2);			// car 4
Uncouple(2);			// cars 4 & 5
MoveFwd();
Skip(5);			// coal car
Stop();
if (ModeSw == 1) return;

// move car 3 from siding 3 to siding 1
SelectSiding(3);
Couple(3);			// car 3
MoveFwd();
Skip(3);			// car 2
Uncouple(3);			// cars 1 & 2
MoveFwd();
Skip(5);			// coal car
Skip(5);			// car 3
Stop();
if (ModeSw == 1) return;

SelectSiding(1);
Uncouple(1);			// car 3
MoveFwd();
Skip(5);			// coal car
Stop();
if (ModeSw == 1) return;

// move car 2 from siding 3 to siding 1
SelectSiding(3);
Couple(3);			// car 2
MoveFwd();
Skip(3);			// car 1
Uncouple(3);			// car 1
MoveFwd();
Skip(5);			// coal car
Skip(5);			// car 2
Stop();
if (ModeSw == 1) return;

SelectSiding(1);
Uncouple(1);			// car 2
MoveFwd();
Skip(5);			// coal car
Stop();
if (ModeSw == 1) return;

// pick up car 1 on siding 3
SelectSiding(3);
Couple(3);			// car 1
MoveFwd();
Skip(5);			// coal car
Skip(5);			// car 1
Stop();
if (ModeSw == 1) return;

// pick up car 5 on siding 2
SelectSiding(2);
Couple(2);			// car 5
MoveFwd();
Skip(2);			// car 4
Uncouple(2);			// car 4
MoveFwd();
Skip(5);			// coal car
Skip(5);			// car 1
Skip(5);			// car 5
Stop();
if (ModeSw == 1) return;

// move car 5 to siding 3
SelectSiding(3);
Uncouple(3);			// 5
MoveFwd();
Skip(5);			// coal car
Skip(5);			// car 1
Stop();
if (ModeSw == 1) return;

// pick up car 4 on siding 2
SelectSiding(2);
Couple(2);			// car 4
MoveFwd();
Skip(5);			// coal car
Skip(5);			// car 1
Skip(5);			// car 4
Stop();
if (ModeSw == 1) return;

// move car 4 to siding 3
SelectSiding(3);
Uncouple(3);			// car 4
MoveFwd();
Skip(5);			// coal car
Skip(5);			// car 1
Stop();
if (ModeSw == 1) return;

// pick up car 2 on siding 1
SelectSiding(1);
Couple(1);			// car 2
MoveFwd();
Skip(1);			// car 3
Uncouple(1);			// car 3
MoveFwd();
Skip(5);			// coal car might already be over reed 4
Stop();
if (ModeSw == 1) return;

// move cars 2 and 1 to siding 2
SelectSiding(2);
MoveRev();
Skip(2);			// car 2
Uncouple(2);			// cars 2 & 1
MoveFwd();
Skip(4);			// coal car
Stop();
if (ModeSw == 1) return;

// pick up car 3 on siding 1
SelectSiding(1);
Couple(1);			// car 4
MoveFwd();
Skip(5);			// coal car
Skip(5);			// car 3
Stop();
if (ModeSw == 1) return;

// move car 3 to siding 3
SelectSiding(3);
Uncouple(3);			// car 3
MoveFwd();
Skip(5);			// coal car
Stop();
if (ModeSw == 1) return;

// pick up cars 2 & 1 on siding 2
SelectSiding(2);
Couple(2);			// car 1
MoveFwd();
Skip(5);			// coal car
Skip(5);			// car 1
Skip(5);			// car 2
Stop();
if (ModeSw == 1) return;

// couple cars 5, 4 & 3 on siding 3
SelectSiding(3);
Couple(3);			// car 3
MoveFwd();
Skip(5);			// coal car
Skip(5);			// car 1
Stop();
if (ModeSw == 1) return;

int i;
for (i=0; i<100; i++)		// pause at end of cycle
	{DelayMS(100);
	if (ModeSw == 1) return;
	}
} // Reverse



void Manual()			// Manual operation mode
{
SetTurnout(1, TurnSw1);		// set turnouts according to control panel switches
SetTurnout(2, TurnSw2);

if (FwdSw == 0) MoveFwd();	// move train according to Fwd/Rev switch
else if (RevSw == 0) MoveRev();
else Stop();
} // Manual



void Init()			// Initialize PIC I/O ports
{
//CNPU1 = 0x00FF;		// enable pull-ups for all digital inputs (B15=analog)
CNPU1 = 0x08FF;			// enable pull-ups for all digital inputs
CNPU2 = 0x6000;
//AD1PCFG = 0x1DFF;		// convert analog inputs to digital (except B15)
AD1PCFG = 0x1FFF;		// convert analog inputs to digital (including B15)
TRISB = 0x801F;			// enable outputs

Stop();				// make sure everything is turned off
SetUncoupler(1, OFF);
SetUncoupler(2, OFF);
SetUncoupler(3, OFF);
} // Init

// =============================================================================

void main(void)
{Init();

while (1)
	{if (ModeSw == 1) Manual();
	else Reverse();
	}
} // main