#include <reg52.h>
#include "main.h"
#include "stdio.h"
#include <math.h>
#include "LCD1602.h"

//显存
u8 Disp1[16] ={0};
u8 Disp2[16] ={0};

sbit DIR1 = P1^0;
sbit CLK1 = P1^1;
sbit DIR2 = P1^2;
sbit CLK2 = P1^3;
sbit DIR3 = P1^4;
sbit CLK3 = P1^5;
sbit DIR4 = P1^6;
sbit CLK4 = P1^7;

sbit KEY1 = P3^2;
sbit KEY2 = P3^3;
sbit KEY3 = P3^4;

#define X_AXIS 0
#define Y_AXIS 1
#define Z_AXIS 2
#define _RADIUS       46.188   //半径
#define _DIAGONAL_ROD 500      //推杆长mm
//是否复位状态
u8   reset_state = 0;
//总长850mm,一圈是10Π
#define MAX_LENGTH 10828
//走一步的距离
#define step_value 0.0785f

u16    T1_Length=0,T2_Length=0,T3_Length=0;
//点的状态
u8     point_states = 0;
//目标点坐标，单位mm
float  aim_cartesian_arr[2][3] = {{100,20,50},
								  {-60,70,30},};
//目标点坐标转换为T1、T2、T3长度
float  aim_length_arr_temp[2][3] = {0};
//将长度转换成步进电机步进
u16    aim_length_arr[2][3] = {0};

float delta_diagonal_rod_2; //推杆长的平方
float delta_tower1_x;       //是左前柱的x坐标值，是由radius这个参数算出来的
float delta_tower1_y;       //是左前柱的y坐标值，是由radius这个参数算出来的
float delta_tower2_x;       //是右前柱的x坐标值，是由radius这个参数算出来的
float delta_tower2_y;       //是右前柱的y坐标值，是由radius这个参数算出来的
float delta_tower3_x;       //是后中柱的x坐标值，是由radius这个参数算出来的
float delta_tower3_y;       //是后中柱的y坐标值，是由radius这个参数算出来的

//打印机打点的初始化
void PrinterInit();
//现在状态到目标状态运动
void NowToAim(u16 aim_length[3]);
//比较现在状态和目标状态
u8   CompareNowAndAim(u16 aim_length[3]);
//按键扫描，扫描限位开关
void KeyScan(void);	
//复位电机运动
void ResetPro(void);
//平方
float sq(float value);
//初始化计算坐标所需参数
void recalc_delta_settings(float radius, float diagonal_rod);
//坐标转换为T1、T2、T3的计算
void calculate_delta(float delta[3],float cartesian[3]);
							 
void Timer0Init(void)		//5毫秒@12.000MHz
{	
	TMOD = 0x01;		//设置定时器模式
	TL0  = 0x78;		//设置定时初始值
	TH0  = 0xEC;		//设置定时初始值
	TF0  = 0;		    //清除TF0标志
	TR0  = 1;		    //定时器0开始计时
	ET0  = 1;
	EA   = 1;
}

void Timer0() interrupt 1
{
	static int counts = 0;
	TL0 = 0x78;		    //设置定时初始值
	TH0 = 0xEC;		    //设置定时初始值
	counts++;
	if(reset_state == 0)//在复位状态中，时刻扫描限位开关
	{
		KeyScan();
	}
	if(counts>1)//每隔1*5*2ms电机转动一个步距
	{
		if(reset_state == 0)//复位状态
		{				
			ResetPro();//所有电机正转，将滑块往上移
			if(T1_Length == MAX_LENGTH && T2_Length == MAX_LENGTH && T3_Length == MAX_LENGTH)//所有限位开关触发
				reset_state = 1;//复位完成
		}else if(reset_state == 1)
		{
			NowToAim(aim_length_arr[point_states]);//移动到目标位置函数
			if(CompareNowAndAim(aim_length_arr[point_states])==1)//假如移动到目标位置换下一个点
				point_states++;
			if(point_states>1)//两个点走完了，归零
				point_states = 0;
		}
		counts=0;
	}
}

void main()
{
	PrinterInit();//打印机打点的初始化
	LcdInit();
	Timer0Init();	
	while(1)
	{
		sprintf((char *)Disp1,"T1:%5dT2:%5d",T1_Length,T2_Length);
		LcdShowStr(0, 0, Disp1);
		sprintf((char *)Disp2,"T3:%5dP :%5d",T3_Length,(u16)point_states);
		LcdShowStr(0, 1, Disp2);
	}
}

void PrinterInit()
{
	int    i,j;
	recalc_delta_settings(_RADIUS,_DIAGONAL_ROD);//初始化计算坐标所需参数
	for(i=0;i<2;i++)//坐标转换为T1、T2、T3的计算
	{
		calculate_delta(aim_length_arr_temp[i],aim_cartesian_arr[i]);
	}
	for(i=0;i<2;i++)//将长度转换成步进电机步进
	{
		for(j=0;j<3;j++)
			aim_length_arr[i][j] = (u16)(aim_length_arr_temp[i][j]/step_value);
	}
}

void ResetPro()
{
	if(T1_Length!=MAX_LENGTH)//限位开关未触发，一直正转
	{
		DIR1 = 1;
		CLK1 = !CLK1;
	}else//限位开关触发，停止
	{
		CLK1 = 0;
	}
	
	if(T2_Length!=MAX_LENGTH)//限位开关未触发，一直正转
	{
		DIR2 = 1;
		CLK2 = !CLK2;
	}else
	{
		CLK2 = 0;
	}
	
	if(T3_Length!=MAX_LENGTH)//限位开关未触发，一直正转
	{
		DIR3 = 1;
		CLK3 = !CLK3;
	}else
	{
		CLK3 = 0;
	}
}

void KeyScan()
{
	static int keycount = 0;
	static int keystate = 0;
	if(KEY1==0&&keystate==0)//按键按下
	{
		keycount++;
		if(keycount>2&&KEY1==0&&keystate==0)//加两次类似延迟10ms，不好解释
		{
			T1_Length = MAX_LENGTH;//将现在值设为最大
			keystate=1;
		}			
	}else if(KEY2==0&&keystate==0)
	{
		keycount++;
		if(keycount>2&&KEY2==0&&keystate==0)
		{
			T2_Length = MAX_LENGTH;
			keystate=1;
		}	
	}else if(KEY3==0&&keystate==0)
	{
		keycount++;
		if(keycount>2&&KEY3==0&&keystate==0)
		{
			T3_Length = MAX_LENGTH;
			keystate=1;
		}	
	}
	else if(KEY1==1&&KEY2==1&&KEY3==1&&keystate==1)//当所有按键都处于抬起状态，状态刷新
	{
		keycount=0;
		keystate=0;
	}
}
//比较现在位置和目标位置
u8 CompareNowAndAim(u16 aim_length[3])
{
	if(T1_Length == aim_length[0] && T2_Length == aim_length[1] && T3_Length == aim_length[2])
		return 1;
	else 
		return 0;
}

void NowToAim(u16 aim_length[3])
{
	//第一个
	if(T1_Length<aim_length[0])//假如T1_Length小于目标的，就正转
	{
		DIR1 = 1;
		CLK1 = !CLK1;
		if(CLK1==1)
			T1_Length+=1;
	}else if(T1_Length>aim_length[0])
	{
		DIR1 = 0;
		CLK1 = !CLK1;
		if(CLK1==1)
			T1_Length-=1;
	}else
	{
		CLK1=0;
	}
	//第二个
	if(T2_Length<aim_length[1])
	{
		DIR2 = 1;
		CLK2 = !CLK2;
		if(CLK2==1)
			T2_Length+=1;
	}else if(T2_Length>aim_length[1])
	{
		DIR2 = 0;
		CLK2 = !CLK2;
		if(CLK2==1)
			T2_Length-=1;
	}else
	{
		CLK2=0;
	}
	//第三个
	if(T3_Length<aim_length[2])
	{
		DIR3 = 1;
		CLK3 = !CLK3;
		if(CLK3==1)
			T3_Length+=1;
	}else if(T3_Length>aim_length[2])
	{
		DIR3 = 0;
		CLK3 = !CLK3;
		if(CLK3==1)
			T3_Length-=1;
	}else
	{
		CLK3=0;
	}
	//第四个
	DIR4 = 1;
	CLK4 = !CLK4;	
}
//平方
float sq(float value)
{
    return value*value;
}
//初始化计算坐标所需参数
void recalc_delta_settings(float radius, float diagonal_rod)
{
    delta_tower1_x = -0.866 * radius;  //-SIN_60 * radius // front lefttower
    delta_tower1_y = -0.5 * radius;    //-COS_60 * radius
    delta_tower2_x = 0.866 * radius;   //SIN_60 * radius // front right tower
    delta_tower2_y = -0.5 * radius;    //-COS_60 * radius
    delta_tower3_x = 0.0;              // back middle tower
    delta_tower3_y = radius;
    delta_diagonal_rod_2 = sq(diagonal_rod);
}
//将坐标转化为T1,T2,T3长度
//delta是T1,T2,T3长度，cartesian是实际坐标
void calculate_delta(float delta[3],float cartesian[3])
{
    delta[X_AXIS] = sqrt(delta_diagonal_rod_2 - sq(delta_tower1_x - cartesian[X_AXIS]) - sq(delta_tower1_y - cartesian[Y_AXIS])) + cartesian[Z_AXIS];
    delta[Y_AXIS] = sqrt(delta_diagonal_rod_2 - sq(delta_tower2_x - cartesian[X_AXIS]) - sq(delta_tower2_y - cartesian[Y_AXIS])) + cartesian[Z_AXIS];
    delta[Z_AXIS] = sqrt(delta_diagonal_rod_2 - sq(delta_tower3_x - cartesian[X_AXIS]) - sq(delta_tower3_y - cartesian[Y_AXIS])) + cartesian[Z_AXIS];
}