// ----------------------------------------------------
// Cruise Control application for Panther Electro-Steer
// Copyright 2005, Mike Noel, All Rights Reserved
// ----------------------------------------------------

#include <aCore.tea>
#include <aCompass.tea>
#include <aDig.tea>
#include "aMath.tea"

// ********************* Definitions *********************

// define which DIO ports to use to go Starboard and Port

#define STARDIO 1
#define PORTDIO 2

// define which DIO ports to read Up or Down Buttons

#define UpButton 3
#define DnButton 4

// define PID coefficients

#define PFACT	30
#define IFACT	100
#define DFACT	20

// define max Rudder offset (in 1/10 secs)

#define RudderMax 40

// define rudder average time constant (secs to halve)

#define RudderSecsToHalve 300

// ************** Routine to rattle the relays **************
void rattle() {
	aDig_Write(STARDIO, 1); aCore_Sleep(1000); aDig_Write(STARDIO, 0);
	aDig_Write(PORTDIO, 1); aCore_Sleep(1000); aDig_Write(PORTDIO, 0);
	aDig_Write(STARDIO, 1); aCore_Sleep(1000); aDig_Write(STARDIO, 0);
	aDig_Write(PORTDIO, 1); aCore_Sleep(1000); aDig_Write(PORTDIO, 0);
}

// ******************** Routine to Steer ********************
void main () {
//
int current_direction;		// current heading
int desired_direction;		// desired heading
int rudder;			// new rudder position | 0-40, 1/10 sec of   |
int oldrud=20;			// old rudder position | rudder displacement |
int rudchange;			// change in rudder position
int AvgRud=20;			// average rudder position 
int cumrud=6000;		// sum of rudder values
int cumrudnum=300;		// count of rudder values 
int delta;			// how far off desired heading?
int olddelta=0;			// delta in last call to steer
int oldswitch=11;		// hand switch pressed

int s;				// work variable...
#define swing s

// initialize the digital ports
aDig_Config(UpButton, ADIG_INPUT);
aDig_Config(DnButton, ADIG_INPUT);
aDig_Config(PORTDIO, ADIG_OUTPUT);
aDig_Config(STARDIO, ADIG_OUTPUT);

// initialize the current heading
current_direction = aCompass_ReadInt() / 10;

// make current heading the desired heading
desired_direction = current_direction;

while (1) {

	// update the desired heading
	s = 10 * aDig_ReadInt(UpButton) + aDig_ReadInt(DnButton);
	if (s == 11) {
		if (oldswitch == 1) {
			desired_direction = desired_direction + 365;
			rattle();
			}
		if (oldswitch == 10) {
			desired_direction = desired_direction + 355;
			rattle(); aCore_Sleep(3000); rattle();
			}
		if (desired_direction > 359)
			desired_direction = desired_direction - 360;
		}
	oldswitch = s;

	// update the current heading
	current_direction = aCompass_ReadInt() / 10;

	// delta = amount off heading, (the P in PID)
	// it's + if we need to go to starboard, - for port
	
	delta = desired_direction - current_direction;
	if (delta > 180) delta = delta - 360;
	if (delta < -180) delta = delta + 360;
	
	// swing = difference between how far off heading
	//	we are this time compared to last time
	//	(the D in PID)
	// this can be + or - but the absolute value
	// should be as small as we can make it.
	
	swing = delta - olddelta;
	
	// the new rudder position should 
	// (1) reduce swing (D)
	//	controlled by DFACT. If DFACT = 100
	//	then 1 degrees of swing will generate 1 degree
	//	of rudder offset from center, or 20 degrees would be
	//	enough to go hard port/starboard (assuming no offset
	//	and Average Rudder about dead center).
	//	absval DFACT should not exceed 500
	// (2) get us back on course (P)
	//	controlled by PFACT. If PFACT = 100
	//	then 1 degrees off heading will generate 1 degree
	//	of rudder offset from center, or 20 degrees would be
	//	enough to go hard port/starboard (assuming no swing
	//	and Average Rudder about dead center).
	//	absval PFACT should not exceed 100
	// (3) get rudder to historical midpoint (I) 
	//	if no heading offset or swing we should be at the 
	//	average rudder position.
	//	IFACT should probably remain fixed at 100
	
	// start with rudder at mid point, add in delta and swing
	rudder = (IFACT * AvgRud) + (PFACT * delta) - (DFACT * swing);
	rudder = rudder / IFACT;
	
   	// save delta for next time
	olddelta = delta;
	
	// don't oversteer (stop at each extreme)
	if (rudder<0) rudder = 0;
	if (rudder>RudderMax) rudder = RudderMax;

	// compute amount of change (from old rudder position)
	rudchange = rudder - oldrud;	// from -ruddermax to +ruddermax
	
	// apply steering
	delta = aMath_Absval(rudchange); 	
	if (rudchange > 0) aDig_Write(STARDIO, 1);
	if (rudchange < 0) aDig_Write(PORTDIO, 1);
	for (s=0; s<=delta; s++) aCore_Sleep(1000);
	if (s>0) {
		aDig_Write(STARDIO, 0);
		aDig_Write(PORTDIO, 0);
		}
	
	// update old rudder and average rudder (the I in PID)
	oldrud = rudder;
	cumrud = cumrud + rudder;
	cumrudnum = cumrudnum + 1;
	if (cumrudnum > RudderSecsToHalve) {
		cumrud = cumrud / 2;
		cumrudnum = cumrudnum / 2;
		}
	AvgRud = cumrud / cumrudnum;

	// pause a second then do it all again!
	aCore_Sleep(10000);
	}
}