/**********************************************
* ポータブル赤道儀  インターバルタイマー  極軸設定ドリフトモード 付き
* PENTAX KS1 用    V.1.01        2026.07.17
* PIC24FV32KA302 family                                      
* XC16 V2.10 
*    
* TFT LCD 320x240
* Steper Motor 17HS4023 maicro step 1/64 VREF=0.07 Ilimit=350mA V=7V
* Inerval Timer 255秒 x 255回
* Drift Mode
* KS1 IR Remocon 対応
     * 
*V1.01 光軌跡長係数（FWD%)の見直し
***********************************************/
// PIC24FV32KA302 Configuration Bit Settings

// 'C' source line config statements

// FBS
#pragma config BWRP = OFF               // Boot Segment Write Protect (Disabled)
#pragma config BSS = OFF                // Boot segment Protect (No boot program flash segment)

// FGS
#pragma config GWRP = OFF               // General Segment Write Protect (General segment may be written)
#pragma config GSS0 = OFF               // General Segment Code Protect (No Protection)

// FOSCSEL
#pragma config FNOSC = PRI               // Oscillator Select (8MHz FRC oscillator With Postscaler (FRCDIV))
#pragma config SOSCSRC = DIG            // SOSC Source Type 
#pragma config LPRCSEL = HP             // LPRC Oscillator Power and Accuracy (High Power, High Accuracy Mode)
#pragma config IESO = OFF                // Internal External Switch Over bit (Internal External Switchover mode enabled (Two-speed Start-up enabled))

// FOSC
#pragma config POSCMOD = EC           // Primary Oscillator Configuration bits (Primary oscillator disabled)
#pragma config OSCIOFNC = OFF           // CLKO Enable Configuration bit (CLKO output signal is active on the OSCO pin)
#pragma config POSCFREQ = HS            // Primary Oscillator Frequency Range Configuration bits (Primary oscillator/external clock input frequency greater than 8MHz)
#pragma config SOSCSEL = SOSCHP         // SOSC Power Selection Configuration bits (Secondary Oscillator configured for high-power operation)
#pragma config FCKSM = CSDCMD           // Clock Switching and Monitor Selection (Both Clock Switching and Fail-safe Clock Monitor are disabled)

// FWDT
#pragma config WDTPS = PS32768          // Watchdog Timer Postscale Select bits (1:32768)
#pragma config FWPSA = PR128            // WDT Prescaler bit (WDT prescaler ratio of 1:128)
#pragma config FWDTEN = OFF              // Watchdog Timer Enable bits (WDT enabled in hardware)
#pragma config WINDIS = OFF             // Windowed Watchdog Timer Disable bit (Standard WDT selected(windowed WDT disabled))

// FPOR
#pragma config BOREN = BOR3             // Brown-out Reset Enable bits (Brown-out Reset enabled in hardware, SBOREN bit disabled)
#pragma config LVRCFG = OFF             // Low Voltage Regulator Configuration bit (Low Voltage regulator is not available)
#pragma config PWRTEN = ON              // Power-up Timer Enable bit (PWRT enabled)
//#pragma config I2C1SEL = PRI            // Alternate I2C1 Pin Mapping bit (Use Default SCL1/SDA1 Pins For I2C1)
#pragma config BORV = V20               // Brown-out Reset Voltage bits (Brown-out Reset set to lowest voltage (2.0V))
#pragma config MCLRE = ON               // MCLR Pin Enable bit (RA5 input pin disabled,MCLR pin enabled)

// FICD
#pragma config ICS = PGx1               // ICD Pin Placement Select bits (EMUC/EMUD share PGC1/PGD1)

// FDS
#pragma config DSWDTPS = DSWDTPSF       // Deep Sleep Watchdog Timer Postscale Select bits (1:2,147,483,648 (25.7 Days))
#pragma config DSWDTOSC = LPRC          // DSWDT Reference Clock Select bit (DSWDT uses Low Power RC Oscillator (LPRC))
#pragma config DSBOREN = OFF             // Deep Sleep Zero-Power BOR Enable bit (Deep Sleep BOR enabled in Deep Sleep)
#pragma config DSWDTEN = OFF             // Deep Sleep Watchdog Timer Enable bit (DSWDT enabled)

// #pragma config statements should precede project file includes.
// Use project enums instead of #define for ON and OFF.




#include <xc.h>
#include <string.h>


#define FCY 12288000UL// libpic30.hの前にFosc/2を定義　__delay関数用
#include <libpic30.h> 
#include "Font9.h"
#include "Font12.h"





#define REA PORTBbits.RB14 //REinput A INT1
#define REB PORTBbits.RB13 //REinput B
#define RELEZOUT LATBbits.LATB15 //Relase out
#define TSET PORTAbits.RA0 //Time est
#define TSTART PORTAbits.RA1 //Interval timer start
#define MSTART PORTBbits.RB2 //PEQM start


#define LCD_CS LATBbits.LATB4
#define LCD_DC LATAbits.LATA4
#define LCD_RES LATBbits.LATB7
#define RLED LATBbits.LATB0
#define GLED LATBbits.LATB1
#define WLED LATBbits.LATB3

#define nEN LATBbits.LATB12
#define MODE1 LATBbits.LATB11
#define MODE2 LATBbits.LATB10
#define STBY LATAbits.LATA7
#define STEP LATBbits.LATB9
#define DIR LATBbits.LATB8

#define TMR2ON T2CONbits.TON //赤道儀スタート
#define TMR4ON T4CONbits.TON //インターバルタイマースタート


#define us unsigned

us char CNTstart,EQMstart,TIMEScnt,BOPENcnt,INTERVALcnt,DRIFTstart;
unsigned long int T2RP,T2RPX,curPR2;
us char EQtimecunt,BOPENset,lastBOPENset,INTERVALset,lastINTERVALset,TIMESset,lastTSET;
us char lastTIMESset,MMODE,lastINTERVALcnt,BKflg,lastBOPENcnt,RelezON;
us char mojicolor,backcolor;
unsigned int FX,FY;//フォント表示の開始座標

unsigned int F_col,F_row;
us char lcd_digi[10];
 
us char color[17][3];//18bit color data
unsigned char color0[16][3]= {//24bit color data RGB
	{0,0,0},//0 黒
	{255,127,0},//1　オレンジ
	{0,255,0},//2　緑
	{0,100,255},//3　青
	{255,255,0},//4　黄
	{0,255,255},//5　シアン
	{255,0,255},//6　紫
	{255,255,255},//7　白
	{0,0,120},//8　紺
	{120,0,0},//9　茶
	{145,255,0},//10黄緑
	{120,120,0},//11　
	{0,30,50},//12
	{120,0,120},//13
	{60,60,60},//14
	{255,70,0}//15
  
};
us char CWRatio[]={63,63,63,59,56,54,53,53,52,51,50};
us char CC0,CC1,CC2,CC3,CC4,CC5,MarkingTime,minWatingTime;
us char MicroStep = 64;
us char Motor_ON,DriftMode,DRIFTtimeset,PEQspeedset,FWDratio;


//関数宣言*****************************************************************************
void motorStandby(us char sw);
 
//*************************************************************************************

//データEEPROMへの読み書き
unsigned int eeRead(unsigned int address) {
  while(NVMCONbits.WR); // wait for any last write to complete
  TBLPAG = 0x7f; // eprom is a t 0x7ffe00
  return __builtin_tblrdl(0xFE00+address);
  }

 void eeWrite(unsigned int address, unsigned int data) {
   while(NVMCONbits.WR); // wait for last write to complete
   NVMCON = 0x4058; // Set up NVMCON to erase one word of data EEPROM
   TBLPAG = 0x7f; // eprom is a t 0x7ffe00
   __builtin_tblwtl(0xFE00+address, 0xffff);
   asm volatile ("disi #5");
    __builtin_write_NVM(); // Issue Unlock Sequence & Start Write Cycle
   while(NVMCONbits.WR); // wait for errase to complete
   NVMCON = 0x4004; // Set up NVMCON to write one word of data EEPROM
   __builtin_tblwtl(0xFE00+address, data); // Write EEPROM data to write latch
   asm volatile ("disi #5");
    __builtin_write_NVM();
   }



void lcd_CMD(unsigned char CM) {
//__delay_us(50) ;
    LCD_DC = 0;    
	LCD_CS = 0;
	
	SPI2BUF = CM;
    __delay_us(14) ;
	LCD_CS = 1;
}
void lcd_DATA(unsigned char DA) {
   // __delay_us(150) ;

    LCD_DC = 1;
	LCD_CS = 0;
	
	SPI2BUF = DA;
    __delay_us(14) ;
	LCD_CS = 1;

}
void init_LCD() {
	LCD_RES = 0;
	__delay_ms(120) ;

	LCD_RES = 1;
	__delay_ms(120) ;

    lcd_CMD(0x01);
   
	__delay_ms(120) ;
    
	lcd_CMD(0x11);//Sleep Out
    __delay_us(50) ;
    lcd_CMD(0x28);// display off
	__delay_ms(120) ;
	lcd_CMD(0x36);
     
	lcd_DATA(0b01100100);
    lcd_CMD(0x3A);
	lcd_DATA(0x66);

	lcd_CMD(0x2A);
	lcd_DATA(0x00);
	lcd_DATA(0x00);
	lcd_DATA(0x01);//    01
	lcd_DATA(0xEF);//319 3F
	lcd_CMD(0x2B);
	lcd_DATA(0x00);
	lcd_DATA(0x00);
	lcd_DATA(0x00);//     00
	lcd_DATA(0x3F);//239  EF

//	lcd_CMD(0x29);//Display ON
//	lcd_CMD(0x2C);
	__delay_ms(100) ;
	
}



void itostring(unsigned char digit, unsigned long int data,unsigned  char *buffer) {
	unsigned char i;
	buffer += digit;
	*buffer =0;					// 最後の数字位置
	for(i=digit; i>0; i--) {			// 変換は下位から上位へ
            buffer--;					// ポインター１
            *buffer = (data % 10) + '0';	// ASCIIへ
            data = data / 10;				// 次の桁へ
	}
}
void set_col_range(unsigned int start, unsigned int end) {//行範囲の指定
	lcd_CMD(0x002A);
	lcd_DATA(start >> 8);
	lcd_DATA(start);
	lcd_DATA(end >> 8);
	lcd_DATA(end);
	}
void set_row_range(unsigned int start, unsigned int end) {//列範囲の指定
	lcd_CMD(0x002B);
	lcd_DATA(start >> 8);
	lcd_DATA(start);
	lcd_DATA(end >> 8);
	lcd_DATA(end);
	}

void set_LCDcolor(unsigned char C) {//24bitRGBto16bitRGB
	unsigned int c0;
    us int cr,cg,cb;
    c0 = 0;
	cr =255- color0[C][0];//赤
//	cr = color0[C][0];//赤
	cg =255- color0[C][1];//緑 
//	cg = color0[C][1];//緑 
    
	
	cb =255- color0[C][2];//青
//	cb = color0[C][2];//青

	color[C][0] = cr;
	color[C][1] = cg;
    color[C][2] = cb;
}



void makecolorList(){
	us char i;
	for (i = 0;i <= 15;i++){
		set_LCDcolor(i);
	}
}
void write_color(unsigned char C) {
	lcd_DATA(color[C][0]);
	lcd_DATA(color[C][1]);
    lcd_DATA(color[C][2]);
}





void select_font_mk2(unsigned int F) {
    switch (F) {
        case 12:F_col=11;F_row=21;break;//実際のフォントドットから-1した値
        case  9:F_col=8;F_row=14;break;
    }
}
void mojidisp_mk2(us char f,us char ch,us char spn) {//BLOCK文字表示
	int k;
    const unsigned char *fc;
	int x,y,ma,mj;

	set_col_range(FX,FX+F_col);
 	set_row_range(FY,FY+F_row);
 	lcd_CMD(0x2C);//色データ書き込み

    switch (f) {
		case 9:fc = Font9_table[ch];break;
    	case 12:fc = Font12_table[ch];break;
     }

	mj = F_col;

 	for (y=0;y<=F_row;y++) {
 		mj = F_col;
 		for (x=0;x<=F_col;x++) {
 			ma = mj % 8;//ma:mjを8で割り算した余り
 			k=(*fc) & (1<< ma);
 				if (k>0) {
                    write_color(mojicolor);//文字色
 				} else {
 					write_color(backcolor);//背景色
					
 				}
                mj--;
                if (ma == 0) {if (x < F_col) {fc++;}}
 			}
                
 		fc++;
 	}
	FX=FX+F_col+spn;
}


void mojiretsudisp_mk2( us int x,us int y,us char fnt,  us char *str,us char spn) {//  文字列表示
    unsigned char i;
    char len;
	FX = x; FY =y;
	select_font_mk2(fnt);
	len = strlen(str);
	for (i=0;i<len;i++) {
        mojidisp_mk2(fnt,str[i],spn);
	}
}


void write_dots(int x,int y,int d,char c) {//x:ｘ座標　y:y座標　d:線幅　c:色番号
	int i,j;
	set_col_range(x,x+d);
 	set_row_range(y,y+d);
	lcd_CMD(0x2C);//色データ書き込み
	for (i=x;i<x+d;i++) {
 		for (j=y;j<=y+d;j++) {
 			write_color(c);//文字色
 		}
	}
}


void integerdisp(us int x, us int y,us char fnt,us char degi, us long int num,us char spn) {
    FX=x;FY=y;
    itostring(degi,num,lcd_digi);//ASCIIに変換
    mojiretsudisp_mk2(x,y,fnt,lcd_digi,spn);
}


void write_line(unsigned int xs,unsigned int ys,unsigned int xe,unsigned int ye,unsigned char c) {
    unsigned int i,j;
    set_col_range(xs,xe);
    set_row_range(ys,ye);
    lcd_CMD(0x2C);//色データ書き込み
    for (i=ys;i<=ye;i++) {
        for (j=xs;j<=xe;j++) {
            write_color(c);
        }
    }
}
void write_Rectangle(us int xs,us int ys,us int xe,us int ye,us int c) {
    us int i,j;
    write_line(xs,ys,xe,ys,c);
    write_line(xs,ys,xs,ye,c);
    write_line(xe,ys,xe,ye,c);
    write_line(xs,ye,xe,ye,c);
}

int get_colorcord(unsigned int R,unsigned int G,unsigned int B){
    int i,j;
    i=R>>3;
    i=i<<6;
    j=G>>2;
    i=i|j;
    i=i<<5;
    j=B>>3;
    i=i|j;
    return i;
}

void takelastdata(){
	BOPENset = eeRead(0);
	INTERVALset = eeRead(2);
	TIMESset = eeRead(4);
	DRIFTtimeset = eeRead(6);
	PEQspeedset = eeRead(8);
    FWDratio = eeRead(10);
    if (PEQspeedset > 60){PEQspeedset = 60;}
    if (FWDratio > 100){FWDratio = 100;}
}

void strset(us char *as, us char *buf) {//文字列をLCD表示用ASCII文字に変換
    while (*as) *buf++ = *as++;
    *buf = 0x00;
}


void dispBOPENset(us char c){
    mojicolor = c;
    integerdisp(69,110,12,3,BOPENset, 3);//FX,FY,Font番号,桁数,数値,スパン
}
void dispINTERVALset(us char c){
    mojicolor = c;
    integerdisp(159,110,12,3,INTERVALset, 3);//FX,FY,Font番号,桁数,数値,スパン
}
void dispTIMESset(us char c){
    mojicolor = c;
    integerdisp(249,110,12,3,TIMESset, 3);//FX,FY,Font番号,桁数,数値,スパン
}
void dispBOPENcnt(us char c){
    mojicolor = c;
    integerdisp(69,150,12,3,BOPENcnt, 3);//FX,FY,Font番号,桁数,数値,スパン
}
void dispINTERVALcnt(us char c){
    mojicolor = c;
    integerdisp(159,150,12,3,INTERVALcnt, 3);//FX,FY,Font番号,桁数,数値,スパン
}
void dispTIMEScnt(us char c){
    mojicolor = c;
    integerdisp(249,150,12,3,TIMEScnt, 3);//FX,FY,Font番号,桁数,数値,スパン
}
void dispsecdata(){
	dispBOPENset(CC1);
    dispINTERVALset(CC4);
    dispTIMESset(CC4);
    dispBOPENcnt(CC4);
    dispINTERVALcnt(CC4);
    dispTIMEScnt(CC4);
}
void dispBASIC(){
    us char c = 4;
//    write_Rectangle(1,30,319,212,c);
    write_line(1,70,1,182,c);
    write_line(319,70,319,182,c);
    write_line(1,70,319,70,c);
    write_line(1,102,319,102,c);
    write_line(1,142,319,142,c);
    write_line(1,182,319,182,c);
    write_line(46,70,46,182,c);
    write_line(132,70,132,182,c);
    write_line(230,70,230,182,c);
    mojicolor=5;backcolor = 0;
    mojiretsudisp_mk2(10,25,9,"PORTABLE EQUARTORIAL MOUNT",3);// FX,FY,Font,Moji.span
//    mojiretsudisp_mk2(10,45,9,"& INTERVAL TIMER SYSTEM",3);
    mojicolor=c;
//    mojiretsudisp_mk2(10,43,9,"MODE",3);// 
    mojiretsudisp_mk2(49,80,9," B-OPEN",3);
//    mojiretsudisp_mk2(139,80,9,"INTERVAL",3);
//    mojiretsudisp_mk2(233,80,9," TIMES",3);
    mojiretsudisp_mk2(10,114,9,"SET",3);// 
    mojiretsudisp_mk2(10,154,9,"CNT",3);// 
    //mojiretsudisp_mk2(10,190,9,"PEQM",3);// 
    dispsecdata();
}
void dispMODE(us char m){
    mojicolor = 4;
	mojiretsudisp_mk2(100,43,9,"MULTI TIMES",3);

}
void dispDrift(){
	mojicolor=5;backcolor = 0;
	mojiretsudisp_mk2(10,45,9,"& DRIFT CHECK SYSTEM   ",3);
    mojicolor=4;
    mojiretsudisp_mk2(139,80,9,"EQ SPEED",3);
    mojiretsudisp_mk2(233,80,9," FWD %",3);
}
void dispTimer() {
	mojicolor=4;backcolor = 0;
	mojiretsudisp_mk2(10,45,9,"& INTERVAL TIMER SYSTEM",3);
    mojiretsudisp_mk2(139,80,9,"INTERVAL",3);
    mojiretsudisp_mk2(233,80,9," TIMES",3);
}

char getTSETSw(){
 	char sw,last,cnt;
 	cnt=0;
 	last= TSET; //MODE スイッチの接続ポート
	while(cnt < 80){
		__delay_us(500);
 		sw= TSET;
 		if(last == sw){ // 前回の状態と比較
			cnt++; //安定状態
 			}else{
			cnt=0; //チャタリング発生中
 			}
 		last=sw; // 今の状態を前回の状態とする
 		}
	return sw;
 	}


	
void TSETcheck(){
	char sw;
	if (TSET == 1) {lastTSET = 1; return;}
	if (TSET == lastTSET) { return; }

	sw = getTSETSw(); // チャタリング除去済のスイッチ状態
    lastTSET = sw;
    if( (0 == sw) && (TSET == 0) ){ // スイッチが押されて、かつ現在ON
		MMODE++;
        if (MMODE > 2) {MMODE = 0;}
    	if ((DriftMode == 1) & (MMODE > 1)){MMODE = 0;}
        switch (MMODE) {
        	case 0: dispTIMESset(CC4);dispBOPENset(CC1);break;
        	case 1: dispBOPENset(CC4);dispINTERVALset(CC1);break;
        	case 2: dispINTERVALset(CC4);dispTIMESset(CC1);break;
            }


		}
	
	}
void DSETcheck(){
	char sw;
	if (TSET == 1) {lastTSET = 1; return;}
	if (TSET == lastTSET) { return; }

	sw = getTSETSw(); // チャタリング除去済のスイッチ状態
    lastTSET = sw;
    if( (0 == sw) && (TSET == 0) ){ // スイッチが押されて、かつ現在ON
		MMODE++;
        if (MMODE > 2) {MMODE = 0;}
    	if ((DriftMode == 1) & (MMODE > 1)){MMODE = 0;}
        switch (MMODE) {
        	case 0: dispINTERVALset(CC5);dispBOPENset(CC1);break;
        	case 1: dispBOPENset(CC5);dispINTERVALset(CC1);break;
                    //TIMESset = CWRatio[INTERVALset];dispTIMESset(CC5);
//        	case 2: dispINTERVALset(CC4);dispTIMESset(CC1);break;
            }
        

		}
	
	}

void EQMmotorstart(us char di){

    motorStandby(1);DIR = di;TMR2 = 0;TMR3 = 0;STEP = 0;RLED = 1;TMR2ON = 1;
}	
void EQMmotorstop(){
	while(STEP == 1);//STEPが０になるまで待つ
    TMR2ON = 0;motorStandby(0);STEP = 0;RLED = 0;

}
void PEQstartchack(){
    if (DRIFTstart == 1){return;}
	if ((MSTART == 0) && (EQMstart == 0)) {WLED=1;
        PR2 = curPR2 & 0x0000FFFF;PR3 = curPR2 >> 16;
        EQMmotorstart(1);
        EQMstart = 1;while(MSTART == 0);return;}
    if ((MSTART == 0) && (EQMstart == 1)) {WLED = 0;
        EQMmotorstop();
    	EQMstart = 0;while(MSTART == 0);return;}
}

void relez(){
	RELEZOUT = 0;
	__delay_ms(100);
	RELEZOUT = 1;
}
void MLTITimeBOPEN() {//通常モード
	if ((BOPENset == 0) || (TIMESset == 0)){return;}
	CNTstart = 1;
	switch (MMODE) {
		case 0: dispBOPENset(CC4);break;
		case 1: dispINTERVALset(CC4);break;
		case 2: dispTIMESset(CC4);break;
	}
	eeWrite(0,BOPENset);
	eeWrite(2,INTERVALset);
	eeWrite(4,TIMESset);

	while (TIMESset != TIMEScnt){
		MMODE = 0;
		relez();RelezON = 1;
		GLED = 1;TMR4ON = 1;//TIMER1 start
		PEQstartchack();
		while (BOPENset != BOPENcnt){
			if (lastBOPENcnt != BOPENcnt){dispBOPENcnt(CC4);lastBOPENcnt = BOPENcnt;}
			if ((TSTART == 0)&&(BOPENcnt > 1)){
				TMR4ON = 0;BKflg = 1;
				goto L001;}
			PEQstartchack();
		}
        if ((INTERVALset == 0) && (TIMESset != 0)){BOPENcnt =0;dispBOPENcnt(CC4);goto L002;}

		if (INTERVALset != 0){GLED = 0;TMR4ON = 0;}//TIMER1 stop
        
		if (INTERVALset != 0){
			relez();RelezON = 0;
			GLED = 0;TMR4ON = 1;//TIMER1 start
			BOPENcnt = 0;dispBOPENcnt(CC4);
			MMODE = 1;
			while (INTERVALset != INTERVALcnt){
				if (lastINTERVALcnt != INTERVALcnt){dispINTERVALcnt(CC4);lastINTERVALcnt = INTERVALcnt;}
				if ((TSTART == 0)&&(INTERVALcnt > 1)){TMR4ON = 0;BKflg = 1;goto L001;}
                PEQstartchack();
			}
			TMR4ON = 0;//TIMER1 stop
			INTERVALcnt = 0;dispINTERVALcnt(CC4);
		
		}
        L002:
		TIMEScnt++;dispTIMEScnt(CC4);
	}
L001:   
	if (RelezON != 0){relez();RelezON = 0;}
	GLED = 0;TMR4ON = 0;//TIMER4 stop
	TIMEScnt = 0;dispTIMEScnt(CC4);
	CNTstart = 0;
    if (BKflg == 1){BOPENcnt = 0;INTERVALcnt = 0;dispBOPENcnt(CC4);dispINTERVALcnt(CC4);}
    while(TSTART == 0);
}

void setMicroStep(us char MS) {
	switch (MS) {
		case 1: MODE1 = 0;MODE2 = 0;STEP = 0;DIR = 0;break;//1/1step
		case 2: MODE1 = 1;MODE2 = 0;STEP = 1;DIR = 0;break;//1/2step
		case 4: MODE1 = 0;MODE2 = 1;STEP = 0;DIR = 1;break;//1/4step	
		case 8: MODE1 = 1;MODE2 = 1;STEP = 1;DIR = 0;break;//1/8step	
		case 16: MODE1 = 1;MODE2 = 1;STEP = 1;DIR = 1;break;//1/16step
		case 32: MODE1 = 0;MODE2 = 1;STEP = 0;DIR = 0;break;//1/32step
		case 64: MODE1 = 1;MODE2 = 1;STEP = 0;DIR = 1;break;//1/64step
		case 128: MODE1 = 1;MODE2 = 0;STEP = 0;DIR = 0;break;//1/128step
		default: MODE1 = 0;MODE2 = 1;STEP = 0;DIR = 1;break;//1/4step	
    }
}
	
void motorStandby(us char sw){//0:STBY有効 1:STBY解除
    
	if (sw == 0){STBY = 0;return;//STBY
		} else {
        Motor_ON = 1;
		setMicroStep(MicroStep);
		__delay_ms(10);
        STBY = 1;//STBY MODE 解除
        __delay_ms(90);
		}
}


void initMotorDriver(){
	STEP = 0;
	DIR = 0;//CW回転
    nEN = 0;
    STBY = 1;//STBY解除
 	__delay_ms(100);
    STBY = 0;//motor stand-by
	setMicroStep(MicroStep);
	__delay_ms(10);
    STBY = 1;//STBY MODE 解除
    __delay_ms(100);
}

void DRIFTstartcheck() {

	if ((TSTART == 0) && (DRIFTstart == 0)) {
    //    if (EQMstart == 1){EQMmotorstop();}
		DRIFTstart = 1;
        
        eeWrite(6,BOPENset);
        eeWrite(8,INTERVALset);
        eeWrite(10,TIMESset);

        while(TSTART == 0);return;}
    if ((TSTART == 0) && (DRIFTstart == 1)) {
    	EQMmotorstop();DRIFTstart = 0;
    	PR2 = curPR2 & 0x0000FFFF;PR3 = curPR2 >> 16;while(TSTART == 0);
        if (EQMstart == 1){EQMmotorstart(1);return;}
	    }
}

void DRIFTMODEStart(){//ドリフトモード
    us int INVtime,waitetime;
    us long int T2RPTMP;
    us char tempBOPN;
    MarkingTime = 5;
    TMR2ON = 0;MMODE = 0;
    INVtime = TIMESset * BOPENset/100;
    TMR2ON = 1;
	dispBOPENset(CC5);dispINTERVALset(CC5);dispTIMESset(CC5);
    TIMEScnt = 0;
	relez();RelezON = 1;
	GLED = 1;TMR4ON = 1;
	while (BOPENcnt < MarkingTime){
        if (lastBOPENcnt != BOPENcnt){dispBOPENcnt(CC4);lastBOPENcnt = BOPENcnt;}
        DRIFTstartcheck();
        if (DRIFTstart == 0){goto L003;}
    }
	BOPENcnt = 0;
    T2RPTMP = curPR2 / INTERVALset;
	PR2 = T2RPTMP & 0x0000FFFF;PR3 = T2RPTMP >> 16;
	EQMmotorstart(1);
//    GLED = 1;TMR4ON = 1;//TIMER4 start
	while (INVtime > BOPENcnt){
			if (lastBOPENcnt != BOPENcnt){dispBOPENcnt(CC4);lastBOPENcnt = BOPENcnt;}
	//		if ((TSTART == 0)&&(BOPENcnt > 1)){TMR4ON = 0;goto L003;}
			DRIFTstartcheck();
            if (DRIFTstart == 0){goto L003;}
		}
	EQMmotorstop();__delay_ms(100);
    PR2 = T2RPTMP & 0x0000FFFF;PR3 = T2RPTMP >> 16;
	EQMmotorstart(0);
        

	while (BOPENset > BOPENcnt){
			if (lastBOPENcnt != BOPENcnt){dispBOPENcnt(10);lastBOPENcnt = BOPENcnt;}//colorcord10:黄緑
//			if ((TSTART == 0)&&(BOPENcnt > 1)){TMR4ON = 0;BKflg = 1;goto L003;}
			DRIFTstartcheck();
            if (DRIFTstart == 0){goto L003;}
		}
	L003:
    
    BOPENcnt = 0;
	if (RelezON != 0){relez();RelezON = 0;}
	GLED = 0;TMR4ON = 0;//TIMER4 stop
    TMR2ON = 0;STEP = 0;RLED = 0;
	EQMmotorstop();__delay_ms(100);
     if (EQMstart == 1){
        PR2 = curPR2 & 0x0000FFFF;PR3 = curPR2 >> 16;
        EQMmotorstart(1);}
    BOPENcnt = 0;waitetime = minWatingTime;
    tempBOPN = BOPENset;BOPENset = waitetime;dispBOPENset(10);
    TMR4ON = 1;
    if (DRIFTstart == 1){
    while(BOPENcnt < waitetime){
        if (lastBOPENcnt != BOPENcnt){dispBOPENcnt(10);lastBOPENcnt = BOPENcnt;}
        DRIFTstartcheck();
        if (DRIFTstart == 0){goto L004;}
    }};
    BOPENset = tempBOPN;
    L004:
    TMR4ON = 0;BOPENcnt = 0;dispBOPENcnt(CC4);
    while (TSTART == 0);
    DRIFTstart = 0;
   
}


////////////////////////////////////////////////////////
//割り込み処理

void __attribute__((__interrupt__,__no_auto_psv__)) _T3Interrupt(void){
 	IFS0bits.T3IF = 0;
 	IEC0bits.T3IE = 0;

	STEP = ~STEP;
 	TMR2 = 0;TMR3 = 0;
    EQtimecunt++;
	if (EQtimecunt > 30){RLED = ~RLED;EQtimecunt = 0;}

 	IEC0bits.T3IE = 1;

}

void __attribute__((__interrupt__,__no_auto_psv__)) _T5Interrupt(void){
 	IFS1bits.T5IF = 0;
 	IEC1bits.T5IE = 0;

 	TMR4 = 0;TMR5 = 0;
	if (MMODE == 0){BOPENcnt++;}
	if (MMODE == 1){INTERVALcnt++;}

 	IEC1bits.T5IE = 1;

}

void __attribute__((__interrupt__,__no_auto_psv__)) _INT1Interrupt(void){

 IFS1bits.INT1IF = 0;
 IEC1bits.INT1IE = 0;

    if (REB == 1){
            if(MMODE == 0){
                lastBOPENset = BOPENset;BOPENset++;}
            if (MMODE == 1){
                lastINTERVALset = INTERVALset;INTERVALset++;
                if (DriftMode == 1){if (INTERVALset > 10){INTERVALset = 2;}}
            }
            if ((MMODE == 2)& (DriftMode == 0)){
                lastTIMESset = TIMESset;TIMESset++;
 //               if (DriftMode == 1){if (TIMESset > 100){TIMESset = 1;}}
            }
        }
     if (REB == 0){
            if (MMODE == 0){
                lastBOPENset = BOPENset;BOPENset--;}
            if (MMODE == 1){
                lastINTERVALset = INTERVALset;INTERVALset--;
                if (DriftMode == 1){if (INTERVALset < 2){INTERVALset = 10;}}
            }
            if ((MMODE == 2) & (DriftMode == 0)){
                lastTIMESset = TIMESset;TIMESset--;
 //               if (DriftMode == 1){if (TIMESset == 0){TIMESset = 100;}}
            }
     }
  
 IEC1bits.INT1IE = 1;

}


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

 int main()  {
 unsigned int i,j;
 OSCCON = 0b0010001000000000; //Fosc Xtal 24.576MHz
 CLKDIV = 0x0000;
 ANSA = 0b00000000000000000;//ANなし
 TRISA =0b0000000001101111; //ポートA　入出力設定
 PORTA = 0x0000;
 ANSB = 0x0000;
 TRISB = 0b0110000000000100;//ポートB　入出力設定
 LATB = 0x8000;//LATBbits.LATB15 = 1
 
 SPI2CON1 = 0b0000000001111011;//マスターモード クロック second 1/2 primaly 1/1 (clk=約6MHz)
 SPI2CON2 = 0x0000;
 SPI2STATbits.SPIROV = 0;
 SPI2STATbits.SPIEN = 1;//SPI2有効化
 
 
 	
 INTCON1 = 0x0000;
 INTCON2 = 0x0000;//INT1 立下りで割り込み
 IPC5bits.INT1IP = 2;
 IFS1bits.INT1IF = 0;
 IEC1bits.INT1IE = 1;//INT1 割り込み可
 
 // 赤道儀回転タイマー
 T2CON = 0b0000000000001000;//Timer2,3：プリスケーラー1/1 32bit クロック：fosc/2  
 PR2 = 0x0940;PR3 = 0x0015;//仮設定
 TMR2 = 0;TMR3 = 0;//T2,3レジスタクリア
 IPC2bits.T3IP = 4;//t2,3 割込み優先度　4
 IFS0bits.T3IF = 0;//t2,3 割込みフラグクリア
 IEC0bits.T3IE = 1;//t2,3 割込み許可


// インターバルタイマー
 T4CON = 0b0000000000001000;//Timer4,5 プリスケーラー1/1 32bit クロック：fosc/2  
 PR4 = 0x8000;PR5 = 0x00BB;//T4,5 カウント 1秒ごとに割り込み発生
 TMR4 = 0;TMR5 = 0;//T4,5レジスタクリア
 IPC7bits.T5IP = 3;//t4,5 割込み優先度　3
 IFS1bits.T5IF = 0;//t4,5 割込みフラグクリア
 IEC1bits.T5IE = 1;//t4,5 割込み許可

 	MicroStep = 64;
 	switch (MicroStep) {
 	case 16: T2RP = 2757248;T2RPX = 45954;break;//ustep 1/16の時の60倍
 	case 32: T2RP = 1378624;T2RPX = 22977;break;//ustep 1/32の時の60倍
 	case 64: T2RP = 689312;T2RPX = 11488;break;//ustep 1/64の60倍
 	case 128: T2RP = 344656;T2RPX = 5744;break;//ustep 1/128の60倍
 	default: T2RP = 11028992;T2RPX = 183816;break;//ustep 1/4の時の60倍
 	}
 	if (MSTART == 0) {PR2 = T2RPX & 0x0000FFFF;PR3 = T2RPX >> 16;curPR2 = T2RPX;} else {
        
        PR2 = T2RP & 0x0000FFFF;PR3 = T2RP >> 16;curPR2 = T2RP;}
 	if (TSET == 0){DriftMode = 1;} else {DriftMode = 0;}

 CC0 = 0;CC1 = 1;CC2 =2;CC3 = 3;CC4 = 4;CC5 = 5;	
 minWatingTime = 2;
 RelezON = 0;
 motorStandby(0);//STBY有効
 initMotorDriver();//Motor Driver 初期設定
 makecolorList();
 init_LCD();//LCD初期設定

//***********************************画面クリアー（0番色(黒)で全面塗りつぶし）
lcd_CMD(0x002C);//色データ書き込み

    lcd_CMD(0x29);//Display ON
 	write_line(0,0,319,239,0);//Xs,Ys,Xe,Ye,Color 
	takelastdata();

    dispBASIC();
 	if (DriftMode == 0){dispTimer();} else {dispDrift();goto L101;}
 	
    MMODE = 0;//Interval Timer Mode
	while(TSET == 0);

    CNTstart = 0;

    while(1){

    	TSETcheck();// Timer set check

    	if ((lastBOPENset != BOPENset) && (MMODE == 0)){dispBOPENset(CC1);}

    	if ((lastINTERVALset != INTERVALset) && (MMODE == 1)){dispINTERVALset(CC1);}
		if ((lastTIMESset != TIMESset) && (MMODE == 2)){dispTIMESset(CC1);}
    	

    	if ((TSTART == 0) && (CNTstart == 0)){MLTITimeBOPEN();}
    	
    	PEQstartchack();

    }
 	
 	L101:
 	
 	MMODE = 0;//Drift Mode
 	TIMESset = FWDratio;
 	INTERVALset = PEQspeedset;
 	BOPENset = DRIFTtimeset;
 	dispINTERVALset(CC5);dispTIMESset(5);
	while(MSTART == 0);

    CNTstart = 0;

    while(1){//Drift check Mode

    	DSETcheck();// Timer set check

    	if ((lastBOPENset != BOPENset) && (MMODE == 0)){
            dispBOPENset(CC1);}

    	if ((lastINTERVALset != INTERVALset) && (MMODE == 1)){dispINTERVALset(CC1);}
		if ((lastTIMESset != TIMESset) && (MMODE == 2)){dispTIMESset(CC1);}
    	DRIFTstartcheck();

//    	if ((TSTART == 0) && (CNTstart == 0)){DRIFTMODEStart();}
    	if (DRIFTstart == 1){DRIFTMODEStart();}
    	PEQstartchack();
        if (INTERVALset != lastINTERVALset) {TIMESset = CWRatio[INTERVALset];dispTIMESset(CC5);}
    }
}