/*********************************  servo.c  *******************************
 * $Source: /home/cvs/ESP/gen2/software/msp430/test/servo/servo.c,v $
 *  Copyright (C) 2003 MBARI
 *
 *  MBARI Proprietary Information. All rights reserved.
 * $Id: servo.c,v 1.17 2004/06/10 16:12:34 alana Exp $
 *
 * Mainline initialization and background loop for the "sleepy"
 * dwarf microcontroller of the gen2 ESP.
 *
 *****************************************************************************/

// Clock rates
#define CPURATE   4000000 //target CPU clock in hz (may be adjusted)
#define BAUDRATE  115200  //desired USART baud rate
#define DCOUPRATE 1*TIMERHZ  //adjust the DCO every second

#include "dwarftop.h"     //headers required for later inclusion of dwarfmain.h
#include "TrapServo.h"
#include "heater.h"
#include "serial.h"

//void startmove(long int relative,MotorChannel* mc);

static MotorChannel mc1;
static Heater h1;
static int16 setPoint=0;

static unsigned char gMISCout = 0x1f;
static unsigned char gHConfig = 0x0;
static unsigned char gPWRctrl = 0x0;
static unsigned char gSensCtrl=0x0;
//ignore partial command sequence from serial input 
static char endCmd=0;
//flag to toggle position data output
static char encFlag=0;
static int16 heaterCounter=0;
static char tempSenseSwitch=0;
static char tempFlag=0;
static char SendHeaterPWM=false;

// Software timers (each decriments TIMERHZ counts/second)
enum timerIndex {   //allocate and name interval timers
  dcoTimer,         //adjust DCO frequency to compensate for temperature change
  servoTimer,
  heaterTimer,
//define more here...  
  NTIMERS   //number of interval timers
};

// declare FIFOs and Handshaking thresholds
static FIFO (toHost, 400);      //bytes bound for host machine
static FIFO (fromHost, 400);    //bytes received from host machine
#define CTSstop      150   //cancel CTS if fromHost has <= this many bytes free
#define CTSresume      200   //reassert CTS when fromHost fifo has this much room


//a complete description of the command message parser
static parserInstance servoCmdParser = {
  &fromHost, &toHost, kickstartXmit  //parser "object"
};

#include "dwarfcore.h"

void homed0(MotorChannel* mc)
{
writeDWRF(DWRFIFG,0x00);
gPWRctrl &= ((unsigned char)(~(DCOR0HOME5)));
writeDWRF(DCORENCPWR,gPWRctrl);
gHConfig &= ((unsigned char)(~DCOR0HMZERO));
writeDWRF(DCORENCCTL,gHConfig);
gMISCout=0x1f;
mc->relative=0;
startmove(mc->relative,mc);	
}
static inline void serviceDwarfIRQ (byte pending)
/*
  Service Pending Dwarf Interrupts
  Global Interrupts are enabled on entry.
*/
{
//add more CPLD interrupt handler calls here...
//each MUST clear the source of the interrupt or mask it before exit
//  e.g. if (pending & DWRFI2CHELD) I2CgateCkHeld (&gateway);
if (pending & DCOR0HOMED) homed0(&mc1);

}

// set framework's parameters
#define dwarfTic() 
#define dwarfInResume() 
#define dwarfOutResume() 

//// BEGIN Framework
#include "dwarfmain.h"   //generic dwarf application "framework"

interrupt[ADC_VECTOR] void ADC12ISR(void)
{
char temp;
 	switch(ADC12IV)
 	{
 		case 6:
 		mc1.motorCurrent=ADC12MEM0;
 		if(mc1.motorCurrent>2048)
 		{
 		gMISCout=0x1f;
		gMISCout &= (unsigned char)(~(DCORMOTORPWR));
		writeDWRF(DCOROUT,gMISCout);
		mc1.relative=0;
		startmove(mc1.relative,&mc1);
		convwritebyte(0xee,&servoCmdParser);
		}
 		break;
 		case 16:
 		
 		if(tempSenseSwitch==1)
 		{
 		h1.tempSense1=ADC12MEM5;
 		gSensCtrl|=(DCORTEMPSL+DCORTEMPEN);
		writeDWRF(DCORSENSOR,gSensCtrl);
 		tempSenseSwitch=0;
 		}
 		else
 		{
 		h1.tempSense2=ADC12MEM5;
 		gSensCtrl&=(unsigned char)(~(DCORTEMPSL));
		gSensCtrl|=DCORTEMPEN;
		writeDWRF(DCORSENSOR,gSensCtrl);
 		tempSenseSwitch=1;
 		}
 		break;
 		case 18:
 		h1.thermoCouple=ADC12MEM6;
 		break;
 		default:break;
 		
 	}
}

void initAdc(void)
{
	
	P6SEL=0xff;	//disable I/O on all port 6 pins (see note on ADC port pg17-5 in users manual)
	P6DIR=0;	//Set all port 6 pins to be inputs
	
	//set ADC control register 0
	ADC12CTL1=SHP+ADC12SSEL_3+CONSEQ_1; //use SMCLK as source, pulse mode sampling, sequence of conversions mode
	ADC12MCTL0=SREF_1+INCH_0;
	ADC12MCTL5=SREF_1+INCH_5; 
	ADC12MCTL6=SREF_1+INCH_6+EOS;
	ADC12IE = 0x61;                     // Enable ADC12IFG.0 (motor current) and 5 (temp sense) 6 (thermo couple)       
	ADC12CTL0=SHT0_15+SHT1_15+MSC+REF2_5V+REFON+ADC12ON+ENC; //ADC config 
}

//initServo sets calls PWM(), clears break int, 
//turns power on to motor and initializes a new motor channel
void initServo(void)
{
//set port1 as input
// P1DIR = 0x0;
 //make sure port functions are high first
// P3OUT = 0x15;
 //set port 3 functions as outputs 
// P3DIR |= 0x35;
// P3OUT = 0x15;
// P3OUT = 0x35;

 //turn encoders on
 gPWRctrl |=DCOR0ENC5;
 writeDWRF (DCORENCPWR, gPWRctrl);//gPWRctrl |= Enc51;//writereg(PWRctrl,gPWRctrl);
 writeDWRF(DCOROUT,0x1f);//writereg(MISCout, gMISCout);
 
 //PWM to the motors are on channel PWM1 and PWM2.
 //this is so that CCR0 can be used with the timer up/down mode
 //define the two PWM's on 4.1 and 4.2 as outptus and second fucntion
 DCORPWMDIR|=(DCOR1PWM|DCOR2PWM|DCOR3PWM);
 DCORPWMSEL|=(DCOR1PWM|DCOR2PWM|DCOR3PWM);

 //need to define the brake and DIR pins as outputs.
 DCORMOTOROUT =(DCOR0DIR|DCOR1DIR);
 DCORMOTORDIR|=0x0f;

 //Now set timer B for PWM mode using SMCLK
 //first stop and clear the counter AND USE smclk
 TBCTL = (TBSSEL1 | TBCLR);
 //Then set what to count up to.
 TBCCR0 = 100;  //Which makes timer B divided by 500
 //Then set up compare fucntions
 TBCCTL1 = 0x0240;
 TBCCTL2 = 0x0240;
 TBCCTL3 = 0x0240;
 //Then the width of the pulse SET TO 1/4
 TBCCR1 = 0;
 //Then the width of the pulse SET TO 3/4
 TBCCR2 = 0;
 //Set starting pulse width to 0
 TBCCR3 = 0;
 //Now startup Timer B IN UP/DOWN MODE
 TBCTL |= (MC1 | MC0);
 set8(DCORMOTOROUT,DCOR0BRAKE);//P5OUT |= 0x02;
 //clear interrupt
 disableDWRFirqs (DCOR0BRAKE);//do we need this?
 //release the brake
 DCORMOTOROUT &=0xfd;
 clear8(DCORMOTOROUT,DCOR0BRAKE); //P5OUT &= 0xfD;

 gMISCout |= (byte)((DCORMOTORPWR));
 writeDWRF(DCOROUT,gMISCout);

 //writeDWRF(DWRFIFG,0x00);
 enableDWRFirqs (DCOR0HOMED);
	
 configMotorChannel(&mc1);
 _EINT();

}

static void runServo (struct timer *array, unsigned index, struct timer *timer)
{
	//start ADC conversion
	ADC12CTL0|=ADC12SC;
	getNewPos(&mc1);
	if(encFlag){sendSerialData(mc1.position,&servoCmdParser);}
 	trapServoLoop(&mc1);
}
static void runHeater(struct timer *array, unsigned index, struct timer *timer)
{
	HeaterServoLoop(setPoint, h1.clHeatFlag, &h1);
	if(heaterCounter>31)
	{
		if(tempFlag){
		sendSerialData((int32)h1.thermoCouple,&servoCmdParser);
		parserPutByte(&servoCmdParser,0x20);
		sendSerialData((int32)h1.currentTemperature,&servoCmdParser);
		parserPutByte(&servoCmdParser,0x20);
		sendSerialData((int32)h1.tempSense1,&servoCmdParser);
		parserPutByte(&servoCmdParser,0x20);
		sendSerialData((int32)h1.tempSense2,&servoCmdParser);
	//parserPutByte(&servoCmdParser,0x0a);
		parserPutByte(&servoCmdParser,0x0d);
		}
		heaterCounter=0;
	}
	else heaterCounter++;
	if(SendHeaterPWM==true){
	sendSerialData((int32)h1.heaterPWM,&servoCmdParser);}

}


// Servo test command parser
void parseTestCmd (parserInstance *parser)
{
#define putString(s)  parserPutString(parser,(s))
#define getByte()     parserGetByte(parser)
#define putByte(byte) parserPutByte(parser,(byte))

 char temp;
 unsigned char comm_bytes[4]={0,0,0,0};
 unsigned long int pos_holder;
 int i;
 unsigned char intreg;

  putString ("\r\nESP2 ServoTest v1.0\r\n");
 

  while (true) 
  {
  temp=getByte();
	  switch (temp)
	  {
	case 'H'://turn on power to home sensor
		gPWRctrl |=DCOR0HOME5 ;//gPWRctrl |= Home51;
		writeDWRF(DCORENCPWR,gPWRctrl);
		writeDWRF(DWRFIFG,0x00);
		break;
	case 'h'://turn off power to home sensor
		gPWRctrl &= ((unsigned char)(~(DCOR0HOME5)));//gPWRctrl &= ((unsigned char)(~(Home51)));
		writeDWRF(DCORENCPWR,gPWRctrl);
		break;
	case 'r'://read
		intreg=readDWRF(DWRFIFG);
		convwritebyte(intreg,&servoCmdParser);break;
	case 'l':
		intreg=readDWRF(DCORLIMITS);
		convwritebyte(intreg,&servoCmdParser);break;
	case 'Z'://enable homing
		gHConfig |= DCOR0HMZERO;//gHConfig |= EnableHome1;
		writeDWRF(DCORENCCTL,gHConfig);
		break;
	case 'z'://disable homing
		gHConfig &= ((unsigned char)(~DCOR0HMZERO));
		writeDWRF(DCORENCCTL,gHConfig);
		break;
	case 'b':
		mc1.relative=0x00010000;
		startmove(mc1.relative,&mc1);
		putByte(0x0a);
		putByte(0x0d);
		break;
	case 'B':
		mc1.relative=-0x00010000;
		startmove(mc1.relative,&mc1);
		putByte(0x0a);
		putByte(0x0d);
		break;
	 case 'j':	//JOG +dir
		for(i=0;i<4;i++)
		{
	   	comm_bytes[i]=convreadbyte(getByte(),endCmd);
	   	}
	  if(endCmd==1)
	   {endCmd=0;break;}
	   else{
	   	pos_holder=0;
		pos_holder+=comm_bytes[3];
		pos_holder|=(comm_bytes[2]<<4);
		pos_holder|=(comm_bytes[1]<<8);
		pos_holder|=(comm_bytes[0]<<12);
		
		mc1.relative=pos_holder;
		startmove(mc1.relative,&mc1);}
		putByte(0x0a);
		putByte(0x0d);
		break;
	case 'J':	//JOG -dir
		for(i=0;i<4;i++)
		{
	   	comm_bytes[i]=convreadbyte(getByte(),endCmd);
	   	}
	if(endCmd==1)
	   {endCmd=0;break;}
	   else{
	   pos_holder=0;
		pos_holder+=comm_bytes[3];
		pos_holder|=(comm_bytes[2]<<4);
		pos_holder|=(comm_bytes[1]<<8);
		pos_holder|=(comm_bytes[0]<<12);
		mc1.relative=0;
		mc1.relative=pos_holder;
		startmove(-(mc1.relative),&mc1);}
		putByte(0x0a);
		putByte(0x0d);
		break;
	case 'g':	//Go to absolute pos
		for(i=0;i<4;i++)
		{
	    comm_bytes[i]=convreadbyte(getByte(),endCmd);
	    }
		if(endCmd==1)
	   {endCmd=0;break;}
	   else{
	  	pos_holder=0;
		pos_holder+=comm_bytes[3];
		pos_holder|=(comm_bytes[2]<<4);
		pos_holder|=(comm_bytes[1]<<8);
		pos_holder|=(comm_bytes[0]<<12);
		mc1.relative=0;
		mc1.relative=pos_holder-mc1.position;
		startmove(mc1.relative,&mc1);}
		putByte(0x0a);
		putByte(0x0d);
		break;
	case 'M'://Turn motor on
		gMISCout=0x1f;
		gMISCout |= (unsigned char)((DCORMOTORPWR));
		writeDWRF(DCOROUT,gMISCout);
		_NOP();
		break;
	case 'm'://Turn motor off
		gMISCout=0x1f;
		gMISCout &= (unsigned char)(~(DCORMOTORPWR));
		writeDWRF(DCOROUT,gMISCout);
		mc1.relative=0;
		startmove(mc1.relative,&mc1);
		_NOP();
		break;	
	case 'P'://Spit out one position record
		putByte(0x0a);
		putByte(0x0d);
		sendSerialData((int32)mc1.position,&servoCmdParser);
		break;
    case 'D'://stream out position records
    	encFlag=1;
		break;
    case 'd'://stop streaming position records
    	encFlag=0;
		break;
	case 'i'://spit out current to motor 0
		convwritebyte((unsigned char)((mc1.motorCurrent>>8)&0x000000ff),&servoCmdParser);
 		convwritebyte((unsigned char)(mc1.motorCurrent&0x000000ff),&servoCmdParser);
 		putByte(0x0a);
		putByte(0x0d);
		break;
	case 'I'://spit out temp sensor measurement
		convwritebyte((unsigned char)((h1.currentTemperature>>8)&0x000000ff),&servoCmdParser);
 		convwritebyte((unsigned char)(h1.currentTemperature&0x000000ff),&servoCmdParser);
 		putByte(0x0a);
		putByte(0x0d);
		//sendMotorPos(&mc1,6);
		break;
	case 'f':
		sendSerialData(h1.sum,&servoCmdParser);
		putByte(0x0a);
		putByte(0x0d);
		break;
	case 't'://command a set heater PWM
		for(i=0;i<2;i++)
		{
	    comm_bytes[i]=convreadbyte(getByte(),endCmd);
	    }
		if(endCmd==1)
	    {
	    endCmd=0;break;
	    }
	    else
	    {
	  	h1.fixedPWM=0;
		h1.fixedPWM+=comm_bytes[1];
		h1.fixedPWM|=(comm_bytes[0]<<4);
		if (h1.fixedPWM>90)h1.fixedPWM=90;
		h1.clHeatFlag=false;
		}
		putByte(0x0a);
		putByte(0x0d);
		break;
	case 'T'://command a set temperature for closed loop heater control
		for(i=0;i<3;i++)
		{
	    comm_bytes[i]=convreadbyte(getByte(),endCmd);
	    }
		if(endCmd==1)
	    {
	    endCmd=0;break;
	    }
	    else
	    {
	  	setPoint=0;
		setPoint+=comm_bytes[2];
		setPoint|=(comm_bytes[1]<<4);
		setPoint|=(comm_bytes[0]<<8);
		if(setPoint>4095)setPoint=4095;
		calculateSlope(&h1,setPoint);
		h1.sum=0;
		h1.clHeatFlag=true;
		}
		putByte(0x0a);
		putByte(0x0d);
		break;
	case 'S':
		for(i=0;i<1;i++)
		{
	    comm_bytes[i]=convreadbyte(getByte(),endCmd);
	    }
		if(endCmd==1)
	    {
	    endCmd=0;break;
	    }
	    else
	    {
		  	if(comm_bytes[0]==1)
		  	{
		  	gSensCtrl|=DCORTEMPSL;
		  	}
		  	else
		  	{
		  	gSensCtrl&=(unsigned char)(~(DCORTEMPSL));
		  	}
		  	
		}
		putByte(0x0a);
		putByte(0x0d);
		gSensCtrl|=DCORTEMPEN;
		writeDWRF(DCORSENSOR,gSensCtrl);
		break;
	case 's':
		gSensCtrl&=(unsigned char)(~(DCORTEMPEN));
		writeDWRF(DCORSENSOR,gSensCtrl);
		break;	
	case 'w'://scroll out heater PWM values- toggles on and off
		if(SendHeaterPWM==true)
		{
		SendHeaterPWM=false;
		}
		else
		{SendHeaterPWM=true;}
		putByte(0x0a);
		putByte(0x0d);
		break;
	case 'W'://scroll out heater PWM values- toggles on and off
		if(tempFlag==true)
		{
		tempFlag=false;
		}
		else
		{tempFlag=true;}
		putByte(0x0a);
		putByte(0x0d);
		break;
	default: 
		putByte(temp);
		break;
	  }
    
  }
}


//  Application's initialization and background loop

void main(void)
{
  dwarfMainInit();             // initialize framework
  //Set up servo timer
  itimer[servoTimer].countdown = itimer[servoTimer].period = TIMERHZ/32; 
  itimer[servoTimer].action = runServo;
  //set up heater timer
  itimer[heaterTimer].countdown = itimer[heaterTimer].period = TIMERHZ/32; 
  itimer[heaterTimer].action = runHeater;
  
  
//	P5DIR |= 0x30;
//	P5SEL |= 0x30;
	//set up ADC for temp and current sense
	initAdc();
	//set up PWM channels
	initServo();
	initHeater(&h1);
	
 
  if (setjmp (restartEnv)) dwarfMainRestart();
		_EINT();
  parseTestCmd (&servoCmdParser);   //parse commands
  RESET();  //just reset when the parser exits
}