// ride_rev2.c 

#ifndef iogp32
#define iogp32
#include <iogp32.h>
#endif

#include "stdio.h"
#include "string.h"

#define DEBUG	   1

// Timer channel register settings for the various states of motor control
// spin motor
#define SPIN_SLOWH 	    0x0D
#define SPIN_SLOWL		0xC0

#define SPIN_FASTH		0x0D
#define SPIN_FASTL		0x71

//#define SPIN_OFFH		0xC0
//#define SPIN_OFFL		0x00

// rotate motor settings
#define ROT_CWH		    0x0E		   
#define ROT_CWL			0x41

#define ROT_CCWH        0x0E
#define ROT_CCWL		0x00

//#define ROT_OFFH		0xC0
//#define ROT_OFFL		0x00

// LIFT motor settings
#define LIFT_UPH		0x0E
#define LIFT_UPL		0x30

#define LIFT_DOWNH		0x0e
#define LIFT_DOWNL		0x50

#define LIFT_IDLEH		0x0e
#define LIFT_IDLEL		0x48

// definition of the timer registers that are associated with each motor
#define mtrLIFTH        T1CH0H
#define mtrLIFTL        T1CH0L
#define mtrROTATEH      T2CH0H
#define mtrROTATEL      T2CH0L
#define mtrSPINH        T2CH1H
#define mtrSPINL        T2CH1L

// define the limit switch inputs on port a
#define LIMIT_CCW		(PTA & 0x40)   // portA bit 6
#define LIMIT_CW		(PTA & 0x20)   // portA bit 5
#define LIMIT_UP		(PTA & 0x10)   // portA bit 4
#define LIMIT_DOWN      (PTA & 0x08)   // portA bit 3

void init_timers (void);

#pragma interrupt_handler isr_porta_keybd 

void SCI_Setup(void);
void wait_1ms (unsigned int msecs);
void init_porta (void);

void spin_slow (void);
void spin_fast (void);
void spin_off (void);

void rotate_cw (void);
void rotate_ccw (void);
void rotate_off (void);

void lift_up (void);
void lift_down (void);
void lift_idle (void);
void lift_off (void);

void servos_ch1_on (void);
void servos_ch1_off (void);
void servos_ch2_on (void);
void servos_ch2_off (void);

// state functions are combos of lift, rotate settings
void state1 (void);
void state2 (void);
void state3 (void);
void state4 (void);
void state5 (void);
void state6 (void);


void main(void)
{
    // declare the limit variables that are negative logic, thus the "n" in 
	// the name.  nlim_up = 0 when the up limit switch is actuated. nlim_up = 1
	// when it is not actuated, etc
    unsigned char nlim_up, nlim_down, nlim_cw, nlim_ccw;

    SCI_Setup ();       // set up the serial out port
    init_timers ();     //set up timers for servo control
	init_porta ();
    
    //state0 ();          // init the ride
	printf ("\r\rin main");
		
	// start up routine: this ensures a known starting postition of the rotate
	// and lift motors.  In case of a power failure or of a reset, the motors
	// could be in any position.  This routine uses the limit switches feedback
	// to index the motors.  Note that the spin motor can be in any position 
	// without worry
	servos_ch1_off ();
	servos_ch2_off ();
	
	lift_idle ();
	rotate_off ();
	spin_off ();
	
	servos_ch1_on ();
	servos_ch2_on ();
    
	// if the limit up switch is not actuated (= 1) then move the lift motor
	// down until the switch is actuated
	nlim_up = LIMIT_UP;
	nlim_down = LIMIT_DOWN;
	if (nlim_down) lift_down ();
	{
	   while ( nlim_down )
	   {
	 	  nlim_down = LIMIT_DOWN;
	   }
	}

	lift_idle ();
	
	// if the limit CW switch is not actuated (= 1) then move the lift motor
	// down until the switch is actuated.  If the ccw limit switch is set, then 
	// rotate to set the CW limit
	nlim_cw = LIMIT_CW;
	nlim_ccw = LIMIT_CCW;
    if (nlim_cw) rotate_cw ();
	{
	   while ( nlim_cw )
	   {
	 	  nlim_cw = LIMIT_CW;
	   }
	}
	
	rotate_off ();
	
	wait_1ms (1000);
	// at this point the rotate arm should be in far CW position (CW limit switch
	// is acutated) and the lift arm should be in the down position (DOWN limit
	// switch is actuated).
	
	while (1)
	{
 
	  lift_idle ();
	  spin_off ();
	  rotate_off ();
	  servos_ch2_off ();
	  
	  
	  rotate_off ();
	  //lift_move (LIFT_50,LIFT_0);
	  
	  // load sequence
	  spin_slow ();
	  servos_ch2_on ();
	  wait_1ms (2000);
	  spin_off ();
	  servos_ch2_off ();
	  wait_1ms (2000);
	  
	  spin_slow ();
	  servos_ch2_on ();
	  wait_1ms (2000);
	  spin_off ();
	  servos_ch2_off ();
	  wait_1ms (2000);
	  
	  spin_slow ();
	  servos_ch2_on ();
	  wait_1ms (2000);
	  spin_off ();
	  servos_ch2_off ();
	  wait_1ms (2000);
	  
	  spin_slow ();
	  servos_ch2_on ();
	  wait_1ms (2000);
	  spin_off ();
	  servos_ch2_off ();
	  wait_1ms (2000);
	  
	  // lift up a bit
	  servos_ch1_on ();
	  lift_up ();
	  wait_1ms (2500);
	  lift_idle ();
	  	  
	  // get the rotate arm off of the switch before turning on interrupts
	  rotate_ccw();
	  spin_fast ();
	  servos_ch2_on ();
	  wait_1ms (500);
	  
	  asm ("cli");    // interrupts on to protect from over rotation
	  
	  // start main sequence
	  lift_up ();
	  wait_1ms (2000);
	  lift_idle ();
	  wait_1ms (5000);
	  spin_fast ();
	  
	  state1 ();
	  wait_1ms (10000);
	  
	  state3 ();
	  wait_1ms (8000);
	  
	  state2 ();
	  wait_1ms (12000);
	  
	  state5 ();
	  wait_1ms (7000);
	  
	  state1 ();
	  wait_1ms (6000);
	  
	  state4 ();
	  wait_1ms (10000);
	  
	  state6 ();
	  wait_1ms (3000);
	  
	  state1 ();
	  wait_1ms (10000);
	  
	  state3 ();
	  wait_1ms (7000);
	  
	  state4 ();
	  wait_1ms (5000);
	  
	  state6 ();
	  wait_1ms (3000);
	  
	  state2 ();
	  wait_1ms (1000);
	  
	 	  
	  // start the stopping sequence
	  lift_idle ();
	  spin_slow ();
	  rotate_cw ();
	  asm ("sei");           // turn off ints and just poll the sensor
	  
	  while (PTA & 0x20) ;   // keep rotating cw until the switch fires
	  
	  rotate_off ();
	  // rotate arm is in stop position, now lower the lift to unload position	  
	  // if the limit up switch is not actuated (= 1) then move the lift motor
	  // down until the switch is actuated
	  nlim_up = LIMIT_UP;
	  nlim_down = LIMIT_DOWN;
	  if (nlim_down) lift_down ();
	  {
	     while ( nlim_down )
	     {
	 	   nlim_down = LIMIT_DOWN;
	     }
	  }

	  lift_idle ();
      // now the lift arm is in unload position
	  
	  wait_1ms (2000);
	  spin_off ();
	  servos_ch2_off ();
	  wait_1ms (1000);
	  	  
	  spin_slow ();
	  servos_ch2_on ();
	  wait_1ms (2700);
	  spin_off ();
	  servos_ch2_off ();
	  wait_1ms (2000);
	  
	  spin_slow ();
	  servos_ch2_on ();
	  wait_1ms (2700);
	  spin_off ();
	  servos_ch2_off ();
	  wait_1ms (2000);
	  
	  spin_slow ();
	  servos_ch2_on ();
	  wait_1ms (2700);
	  spin_off ();
	  servos_ch2_off ();
	  wait_1ms (2000);
	  
	  spin_slow ();
	  servos_ch2_on ();
	  wait_1ms (2700);
	  spin_off ();
	  servos_ch2_off ();
	  wait_1ms (2000);
	  
	  wait_1ms (15000);
  } 
  
	
}


    

// ******************************************************
// ************** servo motor control stuff

// function: lift motor up - sets rotation of the lifting motor for CCW
// inputs: none - settings are predetermined
// outputs: none, timer registers are updated
void lift_up (void)
{
    T1SC0  = 0x1A;          //unbuf output,toggle on overflow,set out on compare
	
    mtrLIFTH = LIFT_UPH;
    mtrLIFTL = LIFT_UPL;
}


// function: lift motor down - sets rotation of the lifting motor for CW
// inputs: none - settings are predetermined
// outputs: none, timer registers are updated
void lift_down (void)
{
    T1SC0  = 0x1A;          //unbuf output,toggle on overflow,set out on compare
	
    mtrLIFTH = LIFT_DOWNH;
    mtrLIFTL = LIFT_DOWNL;
}

// function: lift motor idle.  This keeps the motor powered but motionless
// or at least very close to motionless.  Note that the idle value is 
// an ideal "center" signal and should result in no motion, but as the temperature
// of the motor changes, the feedback resistor values will vary and thus the 
// servo may start rotating slightly.  This is okay.  The idea here is to keep
// the motor energized so that the lift doesn't collapse.
// inputs: none - settings are predetermined
// outputs: none, timer registers are updated
void lift_idle (void)
{
    T1SC0  = 0x1A;          //unbuf output,toggle on overflow,set out on compare
	
    mtrLIFTH = LIFT_IDLEH;
    mtrLIFTL = LIFT_IDLEL;
}


// *************************************************
// function: rotate_off - kills timer channel for the
// lift motor.
// inputs: none - settings are predetermined
// outputs: none, timer registers are updated
void lift_off (void)
{
    T2SC0 = (T2SC0 & 0xE3);   // set D4 to port control
	DDRD = (DDRD | 0x10);		// set D4 as output
	PTD = (PTD | 0x10);		// drive out constant hi
}



// *************************************************
// function: rotate_cw - sets up rotation in the arm
// to rotate clockwise
// inputs: none - settings are predetermined
// outputs: none, timer registers are updated
void rotate_cw (void)
{
    T2SC0  = 0x1A;          //unbuf output,toggle on overflow,set out on compare
	
    mtrROTATEH = ROT_CWH;
    mtrROTATEL = ROT_CWL;
}


// *************************************************
// function: rotate_cw - sets up rotation in the arm
// to rotate clockwise
// inputs: none - settings are predetermined
// outputs: none, timer registers are updated
void rotate_ccw (void)
{
    T2SC0  = 0x1A;          //unbuf output,toggle on overflow,set out on compare
	
    mtrROTATEH = ROT_CCWH;
    mtrROTATEL = ROT_CCWL;
}



// *************************************************
// function: rotate_off - kills timer channel for the
// rotate motor.
// inputs: none - settings are predetermined
// outputs: none, timer registers are updated
void rotate_off (void)
{
    T2SC0 = (T2SC0 & 0xE3);   // set D6 to port control
	DDRD = (DDRD | 0x40);		// set D6 as output
	PTD = (PTD | 0x40);		// drive out constant hi
}



// *************************************************
// function: spin_off - sets up spin for the basket
// to move at off rate. only one direction possible
// inputs: none - settings are predetermined
// outputs: none, timer registers are updated
void spin_off (void)
{
    T2SC1 = (T2SC1 & 0xE3);   // set D7 to port control
	DDRD = (DDRD | 0x80);		// set D7 as output
	PTD = (PTD | 0x80);		// drive out constant hi}
}


// *************************************************
// function: spin_fast - sets up spin for the basket
// to move at fast rate. only one direction possible
// inputs: none - settings are predetermined
// outputs: none, timer registers are updated
void spin_fast (void)
{
    T2SC1  = 0x1A;          //unbuf output,toggle on overflow,set out on compare
    mtrSPINH = SPIN_FASTH;
    mtrSPINL = SPIN_FASTL;
}

// *************************************************
// function: spin_slow - sets up spin for the basket
// to move at off rate. only one direction possible
// inputs: none - settings are predetermined
// outputs: none, timer registers are updated
void spin_slow (void)
{
    T2SC1  = 0x1A;          //unbuf output,toggle on overflow,set out on compare
	
    mtrSPINH = SPIN_SLOWH;
    mtrSPINL = SPIN_SLOWL;
}



void servos_ch2_off (void)
{
   T2SC   = 0x20;          //stop TIM2 counter 
}

void servos_ch2_on (void)
{
   T2SC = 0x00;
}


void servos_ch1_off (void)
{
   T1SC   = 0x20;          //stop TIM2 counter
}

void servos_ch1_on (void)
{
   T1SC = 0x00;
}

// state definitions
void state1 (void)
{
   lift_up ();
   rotate_ccw ();
}  

void state2 (void)
{
   lift_down ();
   rotate_ccw ();
}

void state3 (void)
{
    lift_up ();
	rotate_cw ();
}

void state4 (void)
{
    lift_down ();
	rotate_cw ();
}		   

void state5 (void)
{
   lift_idle ();
   rotate_cw ();
}

void state6 (void)
{
   lift_idle ();
   rotate_ccw ();
}      


// ************************************************
// function: wait_1ms - this sets up a delay for 1 ms
// inputs: number of ms to wait
// outputs: none
void wait_1ms (unsigned int msecs)
{ 
   int i,j = 0;
   for (j=0; j<msecs; j++)
   {
      for (i=0;i<60;i++)
	  {
	  }
   }
}



// ******************************************************
// function: SCI_setup - initializes the serial communication
// interface.  used by the printf statements
void SCI_Setup(void)
{
     SCC1 = 0x40;
     SCC2 = 0x0C;
     CONFIG1 = 0x01;	/* disable COP */
     CONFIG2 = 0x01;	/* Internal clock for SCI */
}



void init_timers (void)
{
   // **************** timer 1 setup for sonar
    T1SC = 0X20;    //stop timer1
	T1SC = 0x30;	// keep stopped, reset
	T1MODH = 0xC0;	// set for 50 Hz freq
	T1MODL = 0x00;
    T1SC0 = 0x1A;   //unbuf output,toggle on overflow,set out on compare
	T1SC1 = 0x1A;   //unbuf output,toggle on overflow,set out on compare
	
    // ****************  timer 2 for servos
    T2SC   = 0x20;          //stop TIM2 counter
    T2SC   = 0x30;          //keep stopped, and reset counter
    T2MODH = 0xC0;          //modulus high of
    T2MODL = 0x00;          //modulus low set
    
    T2SC0  = 0x1A;          //unbuf output,toggle on overflow,set out on compare
    T2SC1  = 0x1A;          //unbuf output,toggle on overflow,set out on compare
	
}	

void init_porta (void)
{
   DDRA = 0x00;        // port a is all input
   PTAPUE = 0xFF;      // all port a pull ups enabled

   INTKBSCR = 0x02;    // ints masked, falling edge triggering
   INTKBIER = 0x78;    // enable ints on bits 6,5,4 and 3
   INTKBSCR = 0x06;    // write a 1 to ACKK to clear a pending int
   INTKBSCR = 0x04;    // leave on ACKK , ints now unmasked
   
}   
   
void isr_porta_keybd (void)
{
   char pta6,pta5,pta4,pta3;
   
   pta6 = PTA & 0x40;
   pta5 = PTA & 0x20;
   pta4 = PTA & 0x10;
   pta3 = PTA & 0x80;
   
   if (!pta5)
   {
      printf ("\rA5 clear, CW limit switch actuated, back to CCW");
	  rotate_ccw ();
	  pta5 = PTA & 0x20; 
	  while (!pta5) 
	  {
	     pta5 = PTA & 0x20;
	     printf ("\rCW limit switch still actuated");
      }
   }
   
   else if (!pta6)
   {
      printf ("\rA6 clear, CCW limit switch actuated, back to CW");
	  rotate_cw ();
	  pta6 = PTA & 0x40;
	  while (!pta6) 
	  {
	     printf ("\rCCW limit switch still actuated");
		 pta6 = PTA & 0x40;
      }
   }
   
   if (!pta4)
   {
      printf ("\rA4 clear, UP limit switch actuated, back to Down");
	  lift_down ();
	  pta4 = PTA & 0x10; 
	  while (!pta4) 
	  {
	     pta4 = PTA & 0x10;
	     printf ("\rUP limit switch still actuated");
      }
   }
   
   else if (!pta3)
   {
      printf ("\rA3 clear, Down limit switch actuated, back to Up");
	  lift_up ();
	  pta3 = PTA & 0x08; 
	  while (!pta3) 
	  {
	     pta3 = PTA & 0x08;
	     printf ("\rUP limit switch still actuated");
      }
   }
   
   wait_1ms (100);
	  
   INTKBSCR = INTKBSCR | 0x04;   // write a 1 to ACKK bit to acknowledge int
   
}