//KY1H Rotor Control Program
//Copyright David Robbins KY1H 1996
//Rev-1  7/12/96
//Rev-2  8/18/96  fix preset buttons to use overlap region
//
//This code may be used by any amateur radio operator for their own
//use provided the notice above is kept.  No claims are made that this
//code does anything at all that it is supposed to.
//
//This is the first release of my program for controlling rotors using
//the Z-World Micro-2g controller board.  I have tested it on a G-2700SDX
//and a G-2800SDX, it should also work on a G-1000SDX I just haven't 
//plugged it in to be sure.  With the addition of relays it should also
//be able to control other rotors that have analog position circuits.
//
//The positioning algorithm does make use of the Yaesu 90 degree overlap
//that gives 450 degrees of rotation.  It will pick the shortest distance
//to rotate for bearings in the overlap region.
//
//The default rotor setup is for a G-1000/2700/2800sdx with the left stop 
//at north.  A dumb terminal program can be used to set other parameters
//but they will not be saved yet if power is removed.
//
// the com port setting is 9600 baud, 8 bit, no parity, one stop bit
//
//the commands available from the com port are:
//
//	n         == select rotor number n as default rotor to control
//			valid values are 1,2,3,4 (only '1' has been tested)
//
//	C         == print heading for default rotor
//	Cn        == print heading for rotor n
//
//	Pm        == turn default rotor to preset m (m=1,2,3)
//                 default presets are 1=45deg, 2=90deg, 3=180deg
//	Pnm       == turn rotor n to preset m
//
//	Lm        == load default rotor preset m with current heading
//	Lnm       == load rotor n preset m with current heading
//
//	S         == stop the default rotor
//	Sn        == stop rotor n
//
//	T         == print tolerance values
//	Tn x      == set tolerance for rotor n to x (default x=20)
//
//	Mqqq      == turn default rotor to heading qqq
//	X4rMqqq   == same as Mqqq.  CT puts 'X4r' in front of the M
//                 command, don't know why, but this works with it anyway.
//
//	Z         == print calibration values
//	ZnL mmm   == read a/d value for rotor n Left stop which is at
//                 a bearing of mmm, save as left end calibration value
//	ZnR mmm   == same as above for right stop
//	ZnD mmm   == assign degree range mmm to rotor n
//	ZnL xxxx=yyy   == manually assign rotor n Left calibration voltage
//                       xxxx to bearing yyy
//	ZnR xxxx=yyy   == same as above for Right stop.
//
//	?         == print some interesting info
//    V         == print a/d values
//    R         == print rotor control bits (L/R for all 4 rotors)
//
//examples:
//setup g-1000/2700/2800 for north stop (these are defaults)
//	Z1L 3990=000
//	Z1R 2440=090
//	Z1D 450 
//setup g-1000/2700/2800 for south stop
//	Z1L 3990=180
//	Z1R 2440=270
//	Z1D 450
//
//turn rotor to 35 degrees
//	M035
//

#define DEBUG

//prototype for calibration function
void calibrate(int rotornumber);
//calibration values for up to 4 rotors
int ADLeft[4], ADRight[4], DegLeft[4], DegRight[4];
int DegRange[4];
int def;		//default rotor number
//a to d input values
int ad[4];

////////////////////////////////////////////////////////////////////
//PIO Pin usage mapping
////////////////////////////////////////////////////////////////////

//msb=pin7, lsb=pin0 on pio's
//on both pio's port b can only use p4-p7, the lower 4 are preassigned
//just stick to port a for outputs, use port b's for inputs

//general port b input mask
#define PORTB_MASK 0xf0

////////////////////////////////////////////////////////////////////
//NOTE: the relay-6 board uses pio-1 port a for control,
//should probably move these to pio-2 for compatibility
////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////
//PIO-1 Port A = rotor control outputs
////////////////////////////////////////////////////////////////////
//define output bit mask
#define PIO1A_OUTPUT						0xf0
//define each bit used

//mask to OR in to set unused bits of outputs
#define UNUSED_1A							0x90

////////////////////////////////////////////////////////////////////
//PIO-1 Port B = rotor control inputs
////////////////////////////////////////////////////////////////////
//define output bit mask
#define PIO1B_OUTPUT						0x00
//define each bit used

//mask to OR in to set unused bits of outputs
#define UNUSED_1B							0x00

////////////////////////////////////////////////////////////////////
//PIO-2 Port A
////////////////////////////////////////////////////////////////////
//define output bit mask
#define PIO2A_OUTPUT						0xff
//define each bit used
#define ROTORCONTROL_1 					0x01
#define ROTORCONTROL_2 					0x02
//mask to OR in to set unused bits of outputs
#define UNUSED_2A							0x00

////////////////////////////////////////////////////////////////////
//PIO-2 Port B
////////////////////////////////////////////////////////////////////
//define output bit mask
#define PIO2B_OUTPUT						0x00
//define each bit used
#define ROTOR0_PRESET_1					0x80
#define ROTOR0_PRESET_2					0x40
#define ROTOR0_PRESET_3					0x20
#define ROTOR0_SAVE_PRESET				0x10
//mask to OR in to set unused bits of outputs
#define UNUSED_2B							0x00

////////////////////////////////////////////////////////////////////


////////////////////////////////////////////////////////////////////
//main program
////////////////////////////////////////////////////////////////////

#define RS232_RXBUFFER_SIZE 16
#define RS232_TXBUFFER_SIZE 256
char rbuf[RS232_RXBUFFER_SIZE];
char tbuf[RS232_TXBUFFER_SIZE];
char writebuf[RS232_TXBUFFER_SIZE];
char readbuf[RS232_RXBUFFER_SIZE];

int DegAbs2AD(int x, int n)
{
	//convert input absolute bearing to a/d value
	int y;

	x=x-DegLeft[n];
	if(x<0) x+=360;
	y= (int)(
		(float)ADLeft[n]
		+ (float)(ADRight[n]-ADLeft[n])
			*(
				(float)(x)
				/(float)(DegRange[n])
			 )
		);
	return y;
};
int AD2DegAbs(int n)
{
	//convert input s/d value to rotor absolute bearing
	//zero== left stop, DegRange=right stop
	int x;

	x = (int)(
					((float)(ad[n]-ADLeft[n])/(float)(ADRight[n]-ADLeft[n]))
					*(float)DegRange[n]
				);
	return x;
};
int AD2Deg(int n)
{
	//convert input a/d value to rotor display bearing
	int x;

	x = AD2DegAbs(n);
	x=x+DegLeft[n];
	if(x>360) x-=360;
	return x;
};

int Deg2AD(int x, int n)
{
	int y,z;
	
	//check for use of overlap region on rotor with
	//more than 360 degree rotation
	if(DegRange[n]<=360) return x;
				  		
	//get the absolute angle to go to
	z=x-DegLeft[n];
	if(z<0) z+=360;
	sprintf(writebuf,"checking overlap.  commanded absolute angle=%d\r",z);
  		Dwrite_z0(writebuf, strlen(writebuf));
					
	//is the absolute angle in the overlapable area?
	if(z<=(DegRange[n]-360))
	{
		//get current location
		y=AD2DegAbs(n);

		sprintf(writebuf,"can be overlap.  at %d(abs) now.\r",y);
	  		Dwrite_z0(writebuf, strlen(writebuf));
							
		if(y>=360)
		{
			//already in overlap region, stay there
			x=x+360;
			sprintf(writebuf,"in overlap at %d(abs), stay there at %d\r",y,x);
		  		Dwrite_z0(writebuf, strlen(writebuf));
		}
		else
		{
			if(y>z+180)
			{
				//use the overlap region

				x=x+360;
				sprintf(writebuf,"not in overlap, go to it at %d\r",x);
			  		Dwrite_z0(writebuf, strlen(writebuf));
			}
			else
			{
				sprintf(writebuf,"forget overlap\r");
			  		Dwrite_z0(writebuf, strlen(writebuf));
			}
		}
	}
	return DegAbs2AD(x,n);
}

main()
{


	int Preset[4][3];
	int SavePreset[4];
	
	int RotorControl;

	int tolerance[4];
	int setpoint[4];
	int setpointlist[4][3];	
	int moving[4];

	int InPIO1B,InPIO2B; 
	int OutPIO1A,OutPIO2A;

	int which;
	int pre;
	
	int x,y,z;
	////////////////////////////////////////////////////////////////////
	//set up buffer for rs-232 work and reset interrupt vector
	////////////////////////////////////////////////////////////////////

//#if ROM==0
	reload_vec(14, Dz0_circ_int);		// transfer the vector
//#endif

   Dinit_z0(rbuf, tbuf, RS232_RXBUFFER_SIZE, RS232_TXBUFFER_SIZE, 0x04, 8, 0, 1);
   // rbuf is circular buffer for receiver
   // tbuf is circular buffer for transmitter
   // RS232_BUFFER_SIZE is the size of the buffers
   // 0x04 for 8 bit, no parity, 1 stop bit
   // 0 for no modem
   // 1 for echo

	Dz0send_prompt();

	////////////////////////////////////////////////////////////////////
	//reset pio's and assign output bits
	////////////////////////////////////////////////////////////////////

	//setup PIO-1
	resPIOCA(PIO1A_OUTPUT);    //port a declare output bits
	resPIOCB(PIO1B_OUTPUT);		//port b declare output bits
	setPIODA(UNUSED_1A);	      //port a preset outputs
	setPIODB(0x00);		//port b preset outputs

	//setup PIO-2
	resPIOCA2(PIO2A_OUTPUT);		//port a declare output bits
	resPIOCB2(PIO2B_OUTPUT);		//port b declare output bits
	setPIODA2(0x00);		//port a preset outputs
	setPIODB2(0x00);		//port b preset outputs

	////////////////////////////////////////////////////////////////////
	//setup initial control parameters
	//if calibration data is available in eeprom that should be read out here
	////////////////////////////////////////////////////////////////////
	for(which=0;which<4;which++)
	{
		tolerance[which]=20;
		for(pre=0;pre<3;pre++)
		{
			setpointlist[which][pre]=0;
			Preset[which][pre]=0;
		}
		setpoint[which]=0;
		SavePreset[which]=0;
		moving[which]=0;
		ADLeft[which]=0;
		ADRight[which]=4096;
		DegLeft[which]=0;
		DegRight[which]=360;
		DegRange[which]=360;
	}
	def=0;

	//set up for g2700sdx with north stop on rotor 0
ADLeft[0]=3990;
ADRight[0]=2440;
DegLeft[0]=0;
DegRight[0]=90;
DegRange[0]=450;
setpointlist[0][0]=45;
setpointlist[0][1]=90;
setpointlist[0][2]=180;
	
			
	while(1)
	{
		////////////////////////////////////////////////////////////////////
		//hit watchdog and run debug watch control
		////////////////////////////////////////////////////////////////////
		hitwd();

#ifdef DEBUG
		runwatch();
#endif

		////////////////////////////////////////////////////////////////////
		//read and process pio inputs
		////////////////////////////////////////////////////////////////////
		
		//read PIO-2 port B and assign bits to variables for use
		InPIO2B = inport(PIODB2) & PORTB_MASK;

		Preset[0][0]	= !(InPIO2B & ROTOR0_PRESET_1);
		Preset[0][1]	= !(InPIO2B & ROTOR0_PRESET_2);
		Preset[0][2]	= !(InPIO2B & ROTOR0_PRESET_3);
		SavePreset[0] = !(InPIO2B & ROTOR0_SAVE_PRESET);

		//do rest of inputs		

		////////////////////////////////////////////////////////////////////
		//read and process a/d inputs
		////////////////////////////////////////////////////////////////////
		
		//read the a/d inputs and save to variables
		for(which=0;which<4;which++)
		{
			ad[which] = mg2adc_read(which);   // read A/D
		}

		////////////////////////////////////////////////////////////////////
		//read and process rs-232 inputs
		////////////////////////////////////////////////////////////////////

   	if(Dread_z0(readbuf,ENTER))	// wait for stream terminated with
   											// carriage return
   	{
			switch(toupper(readbuf[0]))
			{
				case '1':
				case '2':
				case '3':
				case '4':
					//set default rotor selection
					def=readbuf[0]-'1';
					sprintf(writebuf,"default rotor=%d\r",def+1);
 			  		Dwrite_z0(writebuf, strlen(writebuf));
				break;

				case 'C':
					//Cn print current heading for rotor n
					//C print current heading for default rotor
					if(readbuf[1]=='\0')
					{
						which=def;
					}
					else
					{
						which=readbuf[1]-'1';
					}
					if(which>3 || which<0) break;
					x=AD2Deg(which);
					sprintf(writebuf,"heading for rotor %d = %d\r",which+1,x);
			  		Dwrite_z0(writebuf, strlen(writebuf));
				break;
				
				case 'P':
					//Pnm go to preset m for rotor n
					//Pm go to preset m for default rotor
					if(readbuf[2]=='\0')
					{
						which=def;
						pre=readbuf[1]-'1';
					}
					else
					{
						which=readbuf[1]-'1';
						pre=readbuf[2]-'1';
					}
					if(which>3 || which<0 || pre>2 || pre<0) break;
					Preset[which][pre]=1;
				break;

				case 'L':
					//Lnm load preset m for rotor n
					//Lm  load preset m for default rotor
					if(readbuf[2]=='\0')
					{
						which=def;
						pre=readbuf[1]-'1';
					}
					else
					{
						which=readbuf[1]-'1';
						pre=readbuf[2]-'1';
					}
					if(which>3 || which<0 || pre>2 || pre<0) break;
					Preset[which][pre]=1;
					SavePreset[which]=1;
				break;

				case 'S':
					//Sn  stop rotor n
					//S stop default rotor
					if(readbuf[1]=='\0')
						which=def;
					else
						which=readbuf[1]-'1';
					if(which>3 || which<0) break;
					moving[which]=0;
				break;

				case 'T':
					//Tn x   set tolerance for rotor n to x
					//if n missing print tolerance values
					if(readbuf[1]=='\0')
					{
						sprintf(writebuf,"tol[1]=%d  tol[2]=%d  tol[3]=%d  tol[4]=%d\r",
							tolerance[0],tolerance[1],tolerance[2],tolerance[3]);
 				  		Dwrite_z0(writebuf, strlen(writebuf));
					}
					else
					{
						which=readbuf[1]-'1';
						x=atoi(&readbuf[3]);
						tolerance[which]=x;
					}
				break;

		   	case 'X':
					//ct/yaesu rotor control starts with  'X4r' then Mxxx
		   		//slide command over by 3
					for(x=0;x<6;x++) readbuf[x]=readbuf[x+3];
					if(readbuf[0]!='M') break;
				case 'M':
					x=atoi(&readbuf[1]);
					setpoint[def]=Deg2AD(x,def);
					moving[def]=1;	
					sprintf(writebuf,"deg=%d  setpoint=%d\r",x,setpoint[def]);
 			  		Dwrite_z0(writebuf, strlen(writebuf));
					sprintf(writebuf,"ADLeft=%d DegLeft=%d ADRight=%d DegRight=%d\r",ADLeft[def],DegLeft[def],ADRight[def],DegRight[def]);
 			  		Dwrite_z0(writebuf, strlen(writebuf));
				break;

		   	case 'Z':
					//Zna xxxx=yyyy
					//first case, just 'Z' == print calibration values
					if(readbuf[1]=='\0')
					{
						for(x=0;x<4;x++)
						{
							sprintf(writebuf,"%d = Left AD=%d Deg=%d  ",x+1,ADLeft[x],DegLeft[x]);
							sprintf(&writebuf[strlen(writebuf)-1],"Right AD=%d Deg=%d  ",ADRight[x],DegRight[x]);
							sprintf(&writebuf[strlen(writebuf)-1],"Range=%d\r",DegRange[x]);
	 			  			Dwrite_z0(writebuf, strlen(writebuf));
						}
						break;
					}

					//second case, ZxL/ZxR mmm   read calibration from rotor for
					//left or right stop and assign degree value of mmm
					//ZxD mmm  =  assign degree range mmm to rotor x
					if(strlen(readbuf)<9)
					{		   	
						x=atoi(&readbuf[4]);
						which=readbuf[1]-'1';
						if(which>3 || which<0) break;

						switch (toupper(readbuf[2]))
						{
							case 'L':
								ADLeft[which]=ad[which];
								DegLeft[which]=x;
							break;

							case 'R':
								ADRight[which]=ad[which];
								DegRight[which]=x;
							break;					

							case 'D':
								DegRange[which]=x;
							break;
							
							default:
								sprintf(writebuf,"ERROR: %s\r",readbuf);
 					  			Dwrite_z0(writebuf, strlen(writebuf));
					  		break;				
						}
						break;
					}

					//must be ZxL nnnn=mmm or ZxR nnnn=mmm
					//assign Left or Right calibration a/d=nnnn degrees=mmm to
					//rotor x
					which=readbuf[1]-'1';
					if(which>3 || which<0) break;
					x=atoi(&readbuf[4]);
					y=atoi(&readbuf[9]);
					switch (toupper(readbuf[2]))
					{
						case 'L':
							ADLeft[which]=x;
							DegLeft[which]=y;
						break;
						case 'R':
							ADRight[which]=x;
							DegRight[which]=y;
						break;					
						default:
							sprintf(writebuf,"ERROR: %s\r",readbuf);
 				  			Dwrite_z0(writebuf, strlen(writebuf));
				  		break;				
					}
		   	break;												

				case '?':
					//print some interesting information
					for(which=0;which<4;which++)
					{
						sprintf(writebuf,"r=%2d p1=%2d  p2=%2d  p3=%2d  save=%2d pos=%6d  set=%6d move=%2d\r",
							which+1,Preset[which][0],Preset[which][1],Preset[which][2],SavePreset[which],ad[which],setpoint[which],moving[which]);
	 			  		Dwrite_z0(writebuf, strlen(writebuf)+1);
			   	}
				break;

				case 'V':
					//print a/d values
					sprintf(writebuf,"ad1=%6d  ad2=%6d  ad3=%6d  ad4=%6d\r",ad[0],ad[1],ad[2],ad[3]);
 			  		Dwrite_z0(writebuf, strlen(writebuf));
				break;

				case 'R':
					//print rotor control bits
					sprintf(writebuf,"%x",RotorControl);
 			  		Dwrite_z0(writebuf, strlen(writebuf));
		   		Dz0send_prompt();
		   	break;

		   	//doesn't match any command
				default:
					sprintf(writebuf,"ERROR: %s\r",readbuf);
 			  		Dwrite_z0(writebuf, strlen(writebuf));
		  		break;				
			}
   		Dz0send_prompt();
   	}


		////////////////////////////////////////////////////////////////////
		//decide what to do with the inputs
		////////////////////////////////////////////////////////////////////


		//pick which preset to go to
		for(which=0;which<4;which++)
		{
			for(pre=0;pre<3;pre++)
			{
				//is a preset button pressed?
				if(Preset[which][pre])
				{
					//is the save preset button also pressed
					if(SavePreset[which])
					{
						//if so, save the current position as that preset
						setpointlist[which][pre]=AD2Deg(which);
						setpoint[which]=Deg2AD(setpointlist[which][pre],which);
						moving[which]=0;
					}
					else
					{
						//else, set the setpoint to that value and set moving flag
						setpoint[which]=Deg2AD(setpointlist[which][pre],which);
						moving[which]=1;
					}
				}
			}
		}


		////////////////////////////////////////////////////////////////////
		//core rotor control routine
		////////////////////////////////////////////////////////////////////


		
		//clear the output control bits
		RotorControl=0;

		for(which=3;which>=0;which--)
		{
			RotorControl <<= 2;
			
			//this is key to rotor control.  if the 'moving' flag is set to say we are
			//trying to position the antenna then figure out which way to move it
			if(moving[which])
			{
				//see if it is outside the dead band, if so figure out which
				//way to move it.  if not, clear the 'moving' flag to say we are done.
				if(abs(ad[which]-setpoint[which])>tolerance[which])
				{
					//set one bit or the other, use jp1 to programatically reverse
					//the control lines.
					if (ad[which]>setpoint[which])
						RotorControl|=ROTORCONTROL_1;
					else
						RotorControl|=ROTORCONTROL_2;
				}
				else
					moving[which]=0;
			}		
		}

		////////////////////////////////////////////////////////////////////
		//put together output control words and send to ports
		////////////////////////////////////////////////////////////////////
		
		// output all the bits together.
		outport(PIODA2,RotorControl);                 

	}
}