/*
VP·SQUARED - THE VERTICAL PLOTTER
Vic Aprea & Paul Grzymkowski
EE 476 - Spring 2001 - Cornell University
http://www.paulspageofpain.com/vp2

Revision: 6.0
Date: 4/30/01
*/

#include <90s8515.h>
#include <Stdio.h>
#include <Stdlib.h>
#include <delay.h>                                                              
#include <math.h> 	
#asm
    .equ __lcd_port=0x15	;LCD = PORT C
#endasm
#include <lcd.h>

#define prescale0 3
#define begin {
#define end }
#define steps_per_inch 48/1.5	//coefficient to adjust line scaling  
#define D 36 			//length between motors in inches.
#define max_points 10		//max points allowed as input
#define maxkeys 18		//max valid keystrokes
#define kt 30 			//poll keypad every 30 milliseconds
#define uarton 0		//flag to turn UART on or off

//state defintions
#define sget_x 1		
#define sget_y 2
#define splotting 3
#define spoint_count 4
#define scalibrate 5   
#define stop_menu 6
#define sresolution 7

#define base_x D/2		//start at initial x and y that will form an
#define base_y 0.866*D		//equliateral triangle for calibration
#define nobut 99		//declare invalid keystrokes
#define num_options 3		//number of options in the TOP menu

//function initializations     
void initialize(void);
void calculate_da(void);
void calculate_db(void); 
void calculate_dy(void);
void btick_once_R(void);
void ftick_once_R(void);
void btick_once_L(void);
void ftick_once_L(void);
void steptestL(void);
void steptestR(void); 
void calculate_change(void);
void key_press(void);          
void point_count(void);   
void get_x(void);
void get_y(void);
void state_table(void);
void plotting(void);
void step_size(void);
void calibrate(void);
void get_next(void);
void top_menu(void);

//table of valid keypad mappings
flash unsigned char keytbl[maxkeys]={0xd7, 0xee, 0xde, 0xbe, 0xed, 0xdd, 
0xbd, 0xeb, 0xdb, 0xbb, 0x7e, 0x7d, 0x7b, 0x77, 0xe7, 0xb7, 206, 205};
//table of valid menu options
flash unsigned char menu_options[num_options]={scalibrate, sresolution, 
spoint_count};   
               
//(x,y) point queue to be filled by user or function generator
float x_coords[max_points];
float y_coords[max_points];
                     
//multidimentional declarations
int entered_char [5];
char outstring[10];
char lcd_buffer[3];  

//position variables
float init_y, init_x, current_x, current_y, current_a, current_b, da, db, 
length_x, length_y, dx, dy;
float slope, y_intercept, x_dest, y_dest, resolution;  

//stepper motor variables
unsigned int stepcountR, stepcountL, condition;
unsigned char stepR, stepL, flagR, flagL, Rsteps_left, Lsteps_left, substeps;

//state variables
unsigned char num_points, entered_points, i, detour, echo, menu_choice, 
menu_past, offset, state, db_larger, da_funct; 
   
//misc variables
unsigned char reload, key,butnum, prevbut, keytime, num_keys;
                                                
//TIMER0 ISR - Overflows every 1 msec.  Keytime is used to poll
//the keypad, drive the motors, and schedule all other operations
interrupt [TIM0_OVF] void timer0_overflow(void)
begin   
  TCNT0=reload;
  if (keytime>0) --keytime;
end  

//INITIALIZATION - Sets everything up.  Ensures the state machine
//enters into the correct state intitially, and that all assumed variables
//are set.
void initialize(void)
begin

  DDRC=0xFF; 		//make port C output used for LCD
  PORTD = 0;
  
  DDRB=0xFF; 		//make port B output LED's on STK board
  PORTD = 0x00;                       
  
  DDRA = 0xFF;  	//port A trivially set to output, will be used by keypad
  PORTD = 0x00; 
              
  DDRD = 0xFF; 		//port D is output used for stepper motors
  PORTD = 1+2+16+32;
  
  //UART setup
  //will interfere with motors on PORT D if on, so toggle on only 
  //when simulating, and not actually driving physical motors
  if (uarton)		
  begin     
  	UCR = 0x10 + 0x08 ;
  	UBRR = 25 ;  
  end
  	
  //set up timer 0     
  //62.5 x (64x.25) microSec = 1.0 mSec, so prescale 64, and count 62 times.
  reload=256-62; 		//value for 1 Msec  
  TCNT0=reload;	 		//preload timer 1 so that is interrupts after 1 mSec.
  TCCR0=prescale0;		//prescalar to 64
  TIMSK=2;	   		//turn on timer 0 overflow ISR  
  
  //init the task timer
  keytime=kt; 
  
  //init stepper counters
  stepR=0;  
  stepL=0;
  stepcountR=0;
  stepcountL=0;            
  flagL = 0;
  flagR = 0;

  //init position counters
  length_x =0;
  length_y =0;	
  da_funct=-1;
  current_a = D;
  current_b = D;
  current_y = base_y; 
  current_x = base_x;
  init_y = current_y; 
  init_x = current_x; 
  x_coords[0] = 0; 
  
  //init state stuff
  num_keys = 0; 
  entered_points = 0;
  state = stop_menu;
  echo=0;
  offset = 3;
  resolution=.1;
  menu_choice = 0;
  substeps = 0;
  
  //initialize the LCD for 16 char wide
  lcd_init(16); 
  lcd_clear();
  lcd_putsf("CALIBR?");
 
  //crank up the ISRs
  #asm
  	sei
  #endasm 
end
   
//MAIN - every 30 msec provide entry into the state table
//echo PORTD to PORTB for debuggin purposes
void main(void)
begin
   initialize();
   while(1)
   begin                                               
    if (keytime == 0)
   	begin
   	   	keytime=kt;
   	   	state_table();
   	   	PORTB=PORTD;
   	end
  end
end    	
    
//STATE TABLE - depending on value of STATE, jump to a different
//part of the state machine
void state_table(void)
begin
	if(state == spoint_count)
		point_count();
	if(state == sget_x)
		get_x();
	if(state == sget_y)
		get_y();
	if(state == splotting)
		plotting();
	if(state == scalibrate)
		calibrate();	    
	if(state == stop_menu)
		top_menu();
	if(state == sresolution)
		step_size();           
end
 
//TOP MENU - entry point for user.  Provides functionality
//for calibrating the plotter, changing the step size (resolution)
//and plotting a user defined number of points.
//Provides entry into CALIBRATE, POINT COUNT, and STEP SIZE.
void top_menu(void)
begin
	key_press();
	num_keys=0;

	if (entered_char[0] == 10)
	begin
		if (menu_choice < num_options-1)
			menu_choice++;
	 	else menu_choice = 0;
        
		lcd_clear();
		if (menu_options[menu_choice]==scalibrate)
			lcd_putsf("CALIBR?");
		else if (menu_options[menu_choice]==spoint_count)
			lcd_putsf("POINTS?");
		else if (menu_options[menu_choice]==sresolution)
			lcd_putsf("STPSIZE?");	
	end	

	if (entered_char[0] == 13)
	begin
		state = menu_options[menu_choice];
		num_keys=0;
  		lcd_clear();
  				
		if (state == scalibrate)
		begin
			echo=0;
  	 	   	lcd_putsf("CALIBRATE");
		end
		
		if (state == spoint_count)
		begin
			echo=1;
			lcd_putsf("#P=");
		end
		
		if (state == sresolution)
		begin
			echo=1;
			offset=5;
			lcd_putsf("SS/8=");
		end
	end  
	entered_char[0]=0;	
end
                    
//STEP SIZE - changes dx resolution based on users' fancy.
//ultimately effects how smooth or jagged the plotted line looks
//a lower resolution will require fewer calculations, but it will be a worse
//approximation to the actual desired shapes.  When done return to TOP
void step_size(void)
begin
	if(entered_char[num_keys - 1] != 13 || num_keys==0)
	   	key_press(); 
	else
	begin
    		resolution=0;    
	
       		for(i = 0; i < (num_keys - 1); i++)
			resolution = entered_char[i]+ resolution*10;                                                                       
       		resolution = resolution/8;
		
       		lcd_clear();
		lcd_putsf("STEP=");
       		ftoa(resolution,3,lcd_buffer);
       		lcd_puts(lcd_buffer);
       		delay_ms(1000);
		
		
       		num_keys = 0;
       		entered_char[0]=0;
       		state = stop_menu;
       		echo=0;
       		offset=3;
       		menu_choice = 0;
  		lcd_clear();
       		lcd_putsf("CALIBR?");
       	end
end
    
//POINT COUNT - allows user to enter the desired number of 
//discrete points they want to plot, then procedes to GET X
//to allow them to input the points.
void point_count(void)
begin
	if(num_keys == 0)
	begin
		key_press();     
	end	
	else if(num_keys ==1)
	begin
		delay_ms(500);
		num_points = entered_char[0];
		entered_points=0;
		x_coords[0]=0;
		y_coords[0]=0;
		entered_char[0]=0;
		num_keys=0;
  		lcd_clear();
	 	lcd_putsf("X0=");
	  	lcd_gotoxy(3,0);
	   	state = sget_x;
	   	echo=1;
	end
end
    
//GET X - allows user to enter the x-coordinate of their
//desired point.  This function gets called repeatedly
//depending on how many points the user entered, each time
//formatting the input and storing it into the x_coord queue,
//When a value is entered, it moves on to GET Y.
void get_x(void)
begin
	if(entered_char[num_keys - 1] != 13 || num_keys==0)
	   	key_press(); 
	else
	begin
   	   	if(entered_char[0]==14)
		begin
   	   		for(i = 1; i < (num_keys - 1); i++)
				x_coords[entered_points] = entered_char[i]+ x_coords[entered_points]*10;
			x_coords[entered_points] = -x_coords[entered_points] + base_x;	
		end
		else
		begin 
			for(i = 0; i < (num_keys - 1); i++) 
			begin
				x_coords[entered_points] = entered_char[i]+ x_coords[entered_points]*10;
			end                                                                      
			x_coords[entered_points] = x_coords[entered_points] + base_x;	
   		end                                         
   		
		num_keys = 0;  
		lcd_clear();
   		lcd_buffer[0]=48+entered_points;
   		lcd_buffer[1]=0x0;
		lcd_putsf("Y");
		lcd_gotoxy(1,0);
		lcd_puts(lcd_buffer);
		lcd_gotoxy(2,0);
		lcd_putsf("=");
		lcd_gotoxy(3,0);
		echo=1;  
		state = sget_y;
		y_coords[entered_points]=0;
	end
end     
                                                                           
//GET Y - allows user to enter the y-coordinate of their
//desired point.  This function gets called repeatedly
//depending on how many points the user entered, each time
//formatting the input and storing it into the y_coord queue,
//When an y value is entered, and there are no more points
//to enter, it goes on to PLOTTING, otherwise it goes back to 
//GET X to get more points.
void get_y(void)
begin
	if(entered_char[num_keys - 1] != 13 || num_keys == 0)
		key_press(); 
	else    
	begin
		if(entered_char[0] == 14)
		begin
			for(i = 1; i< num_keys-1; i++)
			begin
				y_coords[entered_points] = entered_char[i]+y_coords[entered_points]*10;	
			end
			y_coords[entered_points] = base_y + y_coords[entered_points];
		end
		else
		begin
			for(i = 0; i< num_keys-1; i++)
			begin
				y_coords[entered_points] = entered_char[i]+y_coords[entered_points]*10;	
			end
			y_coords[entered_points] = base_y - y_coords[entered_points];
		
		end
   		num_keys = 0;      
   		entered_points++; 
   		
   		if (entered_points<num_points)
   		begin                                
   			entered_char[0]=0;
   			lcd_buffer[0]=48+entered_points; 
   			lcd_buffer[1]=0x0;
   			lcd_clear();
	 		lcd_putsf("X");
	  		lcd_gotoxy(1,0);
	  		lcd_puts(lcd_buffer);
	  		lcd_gotoxy(2,0);
	   		lcd_putsf("=");
	   		lcd_gotoxy(3,0);
	   		state = sget_x;
	   		x_coords[entered_points]=0;
	   		echo=1; 
		end
		else
		begin
		   	state = splotting;
		   	entered_points = 0;
		   	echo=0;
		   	lcd_clear();        
		   	lcd_putsf("PLOTTING!"); 
		   	                
      			da_funct=0;
	 		stepR=0;  
			stepL=0;
  			stepcountR=0;
  			stepcountL=0; 
			get_next();
		end
	end
end     
         
//CALIBRATE - allows the user to manually control
//each of the stepper motors individually, or at the same time via 
//tbe numbers 1, 2, 4, & 5 on the keypad.  Presumably, the user will
//use this feature to place the cursor at the correct starting point
//to ensure proper output.
//When done calibrating, return to TOP
void calibrate(void)
begin
    key_press();
    num_keys=0;
    
    if (entered_char[0]!=13)
    begin

	if (butnum == 1)
 		if (stepL > 0)
  			stepL--;
  		else stepL = 3;
    	if (butnum == 4)
    		if (stepL < 3)
			 stepL++;
		else stepL = 0;
	if (butnum == 2)
    		if (stepR < 3)
			 stepR++;
		else stepR = 0;    
	if (butnum == 5)
    		if (stepR > 0)
  	 		stepR--;
  		else stepR = 3;
  			
 	if(butnum == 16)	//both up
 	begin
  		if (stepL > 0)
  			stepL--;
  		else stepL = 3;
  		    
  		if (stepR < 3)
			stepR++;
		else stepR = 0; 
  	end          
  		
  	if(butnum == 17)	//both down
  	begin
		if (stepL < 3)
			stepL++;
		else stepL = 0;
		
		if (stepR > 0)
  			stepR--;
  		else stepR = 3;
  	end                	
	steptestL();
	steptestR();
    end             
    else
    begin
       	state = stop_menu;
       	menu_choice=0;
       	lcd_clear();
  		lcd_putsf("CALIBR?");
       	echo=0;
    end
    
    entered_char[0]=0;
end    
 
//STEP TEST LEFT - maps the four possible "states" of the LEFT motor
//onto appropriate outputs for PORTD.  Logic is include to ensure 
//output to one half of the Port does not disrupt the other half.   
void steptestL(void)
begin
  	if (stepL == 0) PORTD = 16+32+(PORTD&0x0F);
	if (stepL == 1) PORTD = 32+64+(PORTD&0x0F);
	if (stepL == 2) PORTD = 64+128+(PORTD&0x0F);
	if (stepL == 3) PORTD = 128+16+(PORTD&0x0F);
end

//STEP TEST RIGHT - same procedure, this time for the RIGHT motor
void steptestR(void)
begin
	if (stepR == 0) PORTD = 1+2+(PORTD&0xF0);
	if (stepR == 1) PORTD = 2+4+(PORTD&0xF0);
	if (stepR == 2) PORTD = 4+8+(PORTD&0xF0);
	if (stepR == 3) PORTD = 8+1+(PORTD&0xF0);
end

//LEFT BACK TICK - function to make the LEFT motor tick BACKWARDS once
void btick_once_L(void)
begin
   	if (stepL > 0)
   	begin					
   		stepL--;
   	end
   	else stepL = 3;	 
   	stepcountL++;   
   	steptestL();
end

//LEFT FORWARD TICK - function to make the LEFT motor tick FORWARDS once
void ftick_once_L(void)
begin
   	if (stepL < 3)
   	begin					
   		stepL++;
   	end
   	else stepL = 0;
   	
   	stepcountL++;
   	steptestL();
end           

//RIGHT BACK TICK - function to make the RIGHT motor tick BACKWARDS once
void btick_once_R(void)
begin
	if (stepR < 3)
	begin					
		stepR++;
	end
	else stepR = 0;  
      
   	stepcountR++;
	steptestR();
end 
   
//RIGHT FORWARD TICK - function to make the RIGHT motor tick FORWARD once
void ftick_once_R(void)
begin
	if (stepR > 0)
	begin					
		stepR--;
	end
	else stepR = 3;  

	stepcountR++;
	steptestR();
end 

//CALCULATE dA - calculate the change in da as outlined in "High Level Design"
void calculate_da(void)
begin
	da = ((current_x / current_a) * dx) + ((current_y / current_a) * dy);
end

//CALCULATE dB - calculate the change in db as outlined in "High Level Design"
void calculate_db(void)
begin
	db = (((current_x - D)/ (current_b)) * dx) + ((current_y / current_b) * dy);
end               
    
//CALCULATE CHANGE - determine where the desintation is based on the initial
//coordinates and the new desired lengths
void calculate_change(void)
begin
	y_dest = init_y + length_y;
	x_dest = init_x + length_x;
end                              

//PLOTTING - takes values from the x and y coordinate queues and negotiates how to 
//step the left and right motors for each set of coordinates.  when it completes one set of 
//coordinates, it gets a new set and repeates the process.  when there are no more coordinates
//to plot, it returns to the TOP MENU
void plotting(void)
begin
	key_press();     		  
    	calculate_change(); 
    
	if(dx>0)
		if (dy>0)
		    condition = (current_y + resolution < y_dest) || (current_x + 
resolution)< x_dest;	  		
		else
			condition = (current_y  - resolution > y_dest) || (current_x + resolution 
< x_dest);	  					
	else
		if (dy>0)
			condition = (current_y  + resolution < y_dest) || (current_x - resolution 
> x_dest);	  		
		else
			condition = (current_y  - resolution > y_dest) || (current_x - resolution 
> x_dest);	  		

	if(condition==1)
	begin
			
		Rsteps_left = stepcountR < fabs(steps_per_inch*db*flagR);
		Lsteps_left = stepcountL < fabs(steps_per_inch*da*flagL);
        
	   	if(uarton) //for debugging purposes
		begin	 
			printf("\n\rstepcountL:%i", stepcountL);
			printf("stepcountR:%i", stepcountR);
		end        
         
		if(Rsteps_left)
			if(db<0) 
				btick_once_R();
			else
				ftick_once_R();
	    	else 
	    	begin
	     		stepcountR = 0;
	     		flagR=0;
	    	end	   
	    
		if(Lsteps_left)
			if(da<0)
				btick_once_L();
			else
				ftick_once_L();	
	  	else
	  	begin
	  		stepcountL = 0;
	  		flagL=0;
	    	end
	    
		if(!Lsteps_left && !Rsteps_left) 
		begin
			current_y = current_y + dy;
			current_x = current_x + dx;
			current_a = current_a + da;   
			current_b = current_b + db;
        	
        		calculate_da();
        		calculate_db();
			flagR = 1;
			flagL = 1;
			substeps=0; 
			//delay_ms(1000); //more debugging		
		end			
	end 
	
	else
	begin
		da_funct++; 
		
		if(da_funct>=num_points)
		begin
			entered_points=0;
			entered_char[0]=0;
			da_funct=0;
			num_keys = 0; 
 			state = stop_menu;
 			echo=0;
			menu_choice = 0;
  			lcd_clear();
  			lcd_putsf("CALIBR?");
		end
		else
		begin
			get_next();    	
	  		delay_ms(1000);
	  		
	  		//display on LCD what last y value was.
	  		//for some reason, when this is included
	  		//PLOTTING fails to grab a second set of coordinates
	  		/*lcd_clear();
	  		lcd_putsf("Y=");
	  		ftoa(current_y,3,lcd_buffer);
	  		lcd_puts(lcd_buffer);
	  		 */	     
 	    end
	end    
end
       
//GET NEXT - gets the next set of coordinates that need to be plotted
//called as a subroutine of PLOTTING after the previous set of coordinates
//have been completed 
void get_next(void)
begin
	length_x = x_coords[da_funct] - current_x;
	length_y = y_coords[da_funct] - current_y; 

   	if (fabs(length_y)<resolution) dy = 0;
	else if (fabs(length_x) <resolution) dy = length_y*fabs(resolution/length_y);
		else dy = length_y*fabs(resolution/length_x);
	if (fabs(length_x)<resolution) dx = 0;
	else dx = length_x*fabs(resolution/length_x); 
	   		
	if (uarton) //massive debugging section
	begin
			   	ftoa(current_x, 5, outstring);	   		
	   			printf("\n\rcurrent_x = %s", outstring);	   		
	   	   		ftoa(current_y, 5, outstring);
				printf("current_y = %s", outstring);
				ftoa(x_coords[da_funct], 5, outstring);	   		
	   			printf("\n\rx_coord = %s", outstring);	   		
	   			ftoa(y_coords[da_funct], 5, outstring);
				printf("y_coord = %s", outstring);
	            		ftoa(length_x, 5, outstring);	   		
	   			printf("\n\rlength_x = %s", outstring);
	   			ftoa(length_y, 5, outstring);	   		
	   			printf("length_y = %s", outstring);

				ftoa(dx, 5, outstring);	   		
	   			printf("\n\rdx = %s", outstring);	   		
	   			ftoa(dy, 5, outstring);
				printf("dy = %s", outstring);
				//printf("num_points=%i", num_points);
				//printf("\n\rda_funct=%i", da_funct);
				
	end
	   			
	init_y = current_y;
	init_x = current_x;
	calculate_da();
 	calculate_db(); 
	flagL = 1;
 	flagR = 1; 
end

//KEY PRESS - semi-standard keypad scanning routine.  Scans one half,
//then the other.  then looks up the mapping in a flash table and assigns
//the mapping a number.  if it's invalid, then assign it the designated
//invalid number.  Also has ability to soft reset the MCU, and recognize
//the "*" symbol as a negative sign.
void key_press(void)
begin
	 //get lower nibble
  	 DDRA = 0x0f;
  	 PORTA = 0xf0;
  	 delay_us(5);
  	 key = PINA;

  	 //get upper nibble
  	 DDRA = 0xf0;
  	 PORTA = 0x0f;
  	 delay_us(5);
  	 key = key | PINA;
  	 prevbut = butnum;

  	 //find matching keycode in keytable
  	 if (key != 0xff)
  	 begin
  	    for (butnum=0; butnum<maxkeys; butnum++)
  	        if (keytbl[butnum]==key)  
  	        	break;
  	   	if (butnum==maxkeys) butnum=nobut;
  	 end
  	 else butnum=nobut;
  	 
  	 if(butnum!=nobut && prevbut==nobut)
  	 begin
  	   	
  	   	if(butnum == 11) //mmm... soft reset
  	   	begin
  	   		#asm
  	   		rjmp 0;
  	   		#endasm
  	   	end
  	   	
  	   	entered_char[num_keys] = butnum;
  	   	num_keys++;
        
  		if (echo)
  	   	begin
  	   		for(i = 0; i<num_keys; i++)
       			begin
       				if (entered_char[i] == 14)
       			       		lcd_buffer[i]='-';
       				else lcd_buffer[i]=48+entered_char[i];
	   		end
	   	lcd_buffer[num_keys] = 0x0;
       		lcd_gotoxy(offset,0);
       		lcd_puts(lcd_buffer);
      		end
	end	 
end