vince/vincecontrol.cpp

#include "vincecontrol.h"
#include"QSerialPortInfo"
#include "QDebug"
#include "QThread"
union MyUnion
{
	int i;
	unsigned char a[4];
};
VinceControl::VinceControl(ProTools proto)
{
	protools = proto;
	initme();
}



VinceControl::~VinceControl()
{
	if (protools == RS232 || protools == RS485)
	{
		serial->close();
		IsMotorInit = false;
	}

}

bool VinceControl::serialconnect(QString comname, QString bandrate)
{
	if (protools == RS232 || protools == RS485)
	{
	}
	else
	{
		return false;
	}
	QSerialPortInfo info;
	QList<QSerialPortInfo> infos = QSerialPortInfo::availablePorts();
	int i = 0;
	foreach(info, infos) {
		if (info.portName() == comname) break;
		i++;
	}
	if (i != infos.size())
	{
		serial->close();
		serial->setPort(info);
		bool aa = serial->open(QIODevice::ReadWrite);
		qint32 b = bandrate.toInt();
		serial->setBaudRate(b);		
		IsMotorInit = true;
	}
	else
	{
		serial->close();
		IsMotorInit = false;
	}
	return true;
}



void VinceControl::Handshacke(QString motorid)
{

	SendCommandtoMotor("dev\n", motorid);
	QByteArray buf;

	GetCommonRetrun(buf);
	if (unsigned char(buf[0])==0xff)
	{
		if (motorid == "non")
		{
			motorid = Motorlist[0];
		}
		buf.remove(0, 3);
		buf.remove(buf.length() - 3, 4);

		QString str=motorid+":";
		str.append(buf);
		SendLog(str);
	}
	initMotor(motorid);
}

void VinceControl::SetRS485ID(QString id)
{
	RS485ID = id;
}

void VinceControl::EnableMotro(QString id )
{
	QString str = "ena\n";
	if (id =="non")
	{
		
		SendCommandtoMotor(str);
	}
	else
	{
		SendCommandtoMotor(str, id);
	}
	QByteArray buf;
	GetCommonRetrun(buf,id);
	

	ByteBack Camback = TranslateBytedata(buf,id);

	




}

void VinceControl::DisableMotro(QString id )
{
	QString str = "off\n";
	SendCommandtoMotor(str,id);
}

void VinceControl::SendCommandtoMotor(QString str)
{
	if (protools == RS232)
	{
		QByteArray buf;
		buf.append(str);
		serial->write(buf);
		serial->waitForBytesWritten(1000);
	}
	else if(protools==RS485)
	{
		QString str2 = RS485ID + " " + str;
		QByteArray buf;
		buf.append(str2);
		serial->write(buf);
		serial->waitForBytesWritten(1000);

	}
	else if (protools==NETTCP)
	{
		if (tcpSocket.length() == 0)
		{
			return;
		}
		else
		{
			tcpSocket[0]->write(str.toUtf8().data());
		}
		//tcpSocket[0]->waitForBytesWritten(1000);
	}
}

void VinceControl::SendCommandtoMotor(QString str, QString modor)
{
	if (modor == "non")
	{
		SendCommandtoMotor(str);
		return;

	}
	
	int lenth = tcpSocket.length();
	int lenthstring = Motorlist.length();
	if (lenthstring!=lenth)
	{
		return;
	}
	for (size_t i = 0; i < lenth; i++)
	{
		if (Motorlist.at(i)==modor)
		{
			tcpSocket[i]->write(str.toUtf8().data());
			tcpSocket[i]->waitForBytesWritten(1000);
			return;
		}

	}
}

void VinceControl::MoveSetDistance(long distance,QString motornetid )
{
	QString commonstr = "rmv " + QString::number(distance) + "\n";
	SendCommandtoMotor(commonstr, motornetid);

}

void VinceControl::MoveMotar(bool direction, QString motornetid )
{
	if (motornetid == "non")
	{
		if (protools != NETTCP)
		{
			if (!SpeedisSet)
			{
				return;
			}
			if (direction)
			{
				SettingSpeedByThis(abs(Speednow));
			}
			else
			{
				SettingSpeedByThis(abs(Speednow)*-1);
			}
		}
		if (protools == NETTCP&&Motorlist.length() > 0)
		{

			if (!isSettingSpeedlist[0])
			{
				return;
			}
			if (direction)
			{
				SettingSpeedByThis(abs(Speedlist[0]), motornetid);;
			}
			else
			{
				SettingSpeedByThis(abs(Speedlist[0])*-1, motornetid);
			}
			
		}

	}
	else
	{

		int lenthstring = Motorlist.length();

		for (size_t i = 0; i < lenthstring; i++)
		{
			if (Motorlist.at(i) == motornetid)
			{

				if (!isSettingSpeedlist[i])
				{
					return;
				}
				if (direction)
				{
					SettingSpeedByThis(abs( Speedlist[i]), motornetid);;
				}
				else
					
				{
					int a = abs(Speedlist[i]);
					SettingSpeedByThis(abs(Speedlist[i])*-1, motornetid);
				}
				break;
				
			}

		}
	}

	QThread::msleep(100);
	QString commendtosend = "mov\n";
	SendCommandtoMotor(commendtosend, motornetid);

}

void VinceControl::SettingSpeed(unsigned long Speed, QString motornetid /*= "non"*/)
{
	SettingSpeedByThis(Speed, motornetid);
	//speed
}

void VinceControl::GetCommonRetrun(QByteArray &buf, QString id/*="non"*/)
{
	if (id == "non")
	{
		if (protools==NETTCP)
		{
			buf.clear();
			tcpSocket[0]->waitForReadyRead(10000);
			buf = tcpSocket[0]->readAll();
		}
	}
}

void VinceControl::SettingSpeedByThis(  long Speed, QString motornetid)
{
	if (motornetid=="non")
	{
		if (protools==NETTCP&&Motorlist.length()>0)
		{
			isSettingSpeedlist[0] = true;
			Speedlist[0] = Speed;
		}
		if (protools!=NETTCP)
		{
			Speednow = Speed;
			SpeedisSet = true;

		//	TranslateBytedata(buf);
		}
	
	}
	else
	{
		int lenthstring = Motorlist.length();

		for (size_t i = 0; i < lenthstring; i++)
		{
			if (Motorlist.at(i) == motornetid)
			{
				
				isSettingSpeedlist[i] = true;
				Speedlist[i] = Speed;

			}

		}
	}
	QString commentosend = "cfg spd=" + QString::number(Speed) + "\n";
	SendCommandtoMotor(commentosend, motornetid);
	QByteArray buf;
	GetCommonRetrun(buf, motornetid);
	TranslateBytedata(buf);

}

ByteBack VinceControl::TranslateBytedata(QByteArray buf, QString motornetid)// = "non")
{
	ByteBack backdatt;
	backdatt.Speed = -1000000;
	if (buf.length()!=21)
	{
		return backdatt;
	}
	if (unsigned char(buf[0]) == 0xff)
	{

		buf.remove(0, 3);
		buf.remove(buf.length() - 3, 3);
		 int location;
		unsigned char a[15];
		memcpy(a, buf.data(), 15);
		location = ((a[0] & 0x0f) << 28) | ((a[1] & 0x7f) << 21) | ((a[2] & 0x7f) << 14) | ((a[3] & 0x7f) << 7) | ((a[4] & 0x7f));
		float speed;
		
		memcpy(&speed, &location, 4);
		backdatt.Speed = speed;
		int loc;
		loc = ((a[5] & 0x0f) << 28) | ((a[6] & 0x7f) << 21) | ((a[7] & 0x7f) << 14) | ((a[8] & 0x7f) << 7) | ((a[9] & 0x7f));
		backdatt.Location = loc;
		backdatt.Speed = speed;
		location = ((a[10] & 0x0f) << 28) | ((a[11] & 0x7f) << 21) | ((a[12] & 0x7f) << 14) | ((a[13] & 0x7f) << 7) | ((a[14] & 0x7f));
		backdatt.Stata = unsigned int(location);

		if (motornetid == "non")
		{
			motornetid = Motorlist[0];
		}
		//memcpy(buf.data() + 6, &location, 4);
		QString str1 = motornetid + ": Speed " + QString::number(backdatt.Speed, 'f', 2) + " location " + QString::number(backdatt.Location);

		SendLog(str1);

		return backdatt;
		//memcpy(buf.data() + 6, &location, 4);

	}
	return backdatt;
}

void VinceControl::initme()
{

	IsMotorInit = false;
	RS485ID = "0";
	Speednow = 0;
	SpeedisSet = false;

}

void VinceControl::initMotor(QString motornetid /*= "non"*/)
{
		QByteArray buf;
	//<2F><><EFBFBD><EFBFBD>s1<73>½<EFBFBD>ʱֹͣ<CDA3><D6B9>ת
	QString commonstr = "cfg s1f=2\n";
	SendCommandtoMotor(commonstr, motornetid);
	GetCommonRetrun(buf, motornetid);

	commonstr = "cfg zmd=1\n";//<2F><><EFBFBD>㳢<EFBFBD>Դ<EFBFBD><D4B4><EFBFBD>
	SendCommandtoMotor(commonstr, motornetid);
	GetCommonRetrun(buf, motornetid);
	commonstr = "cfg snr=0\n";//<2F><><EFBFBD>㴫<EFBFBD><E3B4AB><EFBFBD><EFBFBD>
	SendCommandtoMotor(commonstr, motornetid);
	GetCommonRetrun(buf, motornetid);
	commonstr = "cfg osv=0\n";//<2F><><EFBFBD>㴫<EFBFBD><E3B4AB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD>ƿΪ<C6BF>͵<EFBFBD>ƽ
	SendCommandtoMotor(commonstr, motornetid);
	GetCommonRetrun(buf, motornetid);
	commonstr = "cfg zsd=3000\n";//<2F><><EFBFBD>㴫<EFBFBD><E3B4AB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD>ƿΪ<C6BF>͵<EFBFBD>ƽ
	SendCommandtoMotor(commonstr, motornetid);
	GetCommonRetrun(buf, motornetid);
	//<2F><><EFBFBD>ø<EFBFBD><C3B8><EFBFBD><EFBFBD>޴<EFBFBD><DEB4><EFBFBD><EFBFBD><EFBFBD>
    commonstr = "cfg msr=1\n";
	SendCommandtoMotor(commonstr, motornetid);
	GetCommonRetrun(buf, motornetid);
	//<2F><><EFBFBD>ü<EFBFBD><C3BC>޴<EFBFBD><DEB4><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD>͵<EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD>
	commonstr = "cfg msv=0\n";
	SendCommandtoMotor(commonstr, motornetid);
	GetCommonRetrun(buf, motornetid);


	//<2F><><EFBFBD><EFBFBD>s2<73>½<EFBFBD>ʱֹͣ<CDA3><D6B9>ת
	commonstr = "cfg s2f=2\n";
	SendCommandtoMotor(commonstr, motornetid);
	GetCommonRetrun(buf, motornetid);
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>޴<EFBFBD><DEB4><EFBFBD><EFBFBD><EFBFBD>
	commonstr = "cfg psr=2\n";
	SendCommandtoMotor(commonstr, motornetid);
	GetCommonRetrun(buf, motornetid);
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>޴<EFBFBD><DEB4><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD>͵<EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD>
	commonstr = "cfg psv=0\n";
	SendCommandtoMotor(commonstr, motornetid);
	GetCommonRetrun(buf, motornetid);

}

void VinceControl::MovetoZero(QString motornetid )
{
	MoveToLocation(0, motornetid);

}

long VinceControl::GetLocationNow(QString motornetid)
{
	ByteBack back = GetState(motornetid);
	return back.Location;

}

void VinceControl::MoveToLocation(long Location, QString motornetid)
{
	QString commonstr = "pos "+QString::number(Location)+"\n";
	SendCommandtoMotor(commonstr, motornetid);
	QByteArray buf;
	GetCommonRetrun(buf, motornetid);
	TranslateBytedata(buf, motornetid);

}

void VinceControl::SettingUpandDownSpeed(int addspeed, int downspeed, QString motornetid)
{
	//<2F><><EFBFBD>ü<EFBFBD><C3BC>ٶ<EFBFBD>
	QString commonstr = "cfg acc=" + QString::number(addspeed) + "\n";
	SendCommandtoMotor(commonstr, motornetid);
	QByteArray buf;
	GetCommonRetrun(buf, motornetid);
	TranslateBytedata(buf, motornetid); 
	//<2F><><EFBFBD>ü<EFBFBD><C3BC>ٶ<EFBFBD>
	 commonstr = "cfg dec=" + QString::number(addspeed) + "\n";
	SendCommandtoMotor(commonstr, motornetid);
	GetCommonRetrun(buf, motornetid);
	TranslateBytedata(buf, motornetid);
}

void VinceControl::StopMotormove(QString motornetid )
{
	QString commonstr = "stp\n";
	SendCommandtoMotor(commonstr, motornetid);
	QByteArray buf;
	GetCommonRetrun(buf,motornetid);
	TranslateBytedata(buf,motornetid);
	
}



ByteBack VinceControl::GetState(QString motorid /*= "non"*/)
{
	QString commonstr = "cts\n";
	SendCommandtoMotor(commonstr, motorid);
	QByteArray buf;
	GetCommonRetrun(buf,motorid);
	ByteBack back = TranslateBytedata(buf, motorid);
	return back;
}

void VinceControl::SettingZeroLocation(QString motorid /*= "non"*/)
{
	QString commonstr = "zero start\n";
	SendCommandtoMotor(commonstr, motorid);
	//
	QByteArray buf;
	GetCommonRetrun(buf, motorid);

}

VinceControl::VinceControl(ProTools proto, int port)
{
	//VinceControl(proto);
	initme();
	if (proto != NETTCP)
	{
		return;
	}
	protools = proto;
	//Motor1IpAddress.append("192.168.1.1");

	//<2F><><EFBFBD><EFBFBD><EFBFBD>׽<EFBFBD><D7BD><EFBFBD>,ָ<><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Զ<EFBFBD><D4B6><EFBFBD><EFBFBD>տռ<D5BF>
	tcpServer = new QTcpServer();

	//setWindowTitle("<22><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>6666");
	tcpServer->listen(QHostAddress::Any, port);
	connect(tcpServer, &QTcpServer::newConnection, this, &VinceControl::onNewTcpClinetConnet);


}
void VinceControl::onNewTcpClinetConnet()
{
	if (protools != NETTCP)
	{
		return;
	}
	QTcpSocket *tcpsocket1;

	tcpSocket.append(tcpServer->nextPendingConnection());
//	connect(tcpSocket[tcpSocket.length() - 1], &QTcpSocket::readyRead, this, &VinceControl::onReciveFromClinet);

	QString ip = tcpSocket[tcpSocket.length() - 1]->peerAddress().toString().split("::ffff:")[1];
	Motorlist.append(ip);
	bool statofmotor = false;
	isSettingSpeedlist.append(statofmotor);
	int speed = 200;
	Speedlist.append(speed);
	qint32 port = tcpSocket[tcpSocket.length() - 1]->peerPort();
	QString str11 = ip + " Connected The Port Number is "+QString::number(port)+"\n";
	SendLog(str11);
	Handshacke(Motorlist[Motorlist.length()-1]);
	qDebug() << ip << ":" << port;
	IsMotorInit = true;
}

void VinceControl::onReciveFromClinet(QString motornetid)// = "non")
{
	//QTcpSocket* sc = dynamic_cast<QTcpSocket*>(sender());
	//QString ip = sc->peerAddress().toString().split("::ffff:")[1];

	//<2F><>ȡ<EFBFBD>Է<EFBFBD><D4B7><EFBFBD><EFBFBD>͵<EFBFBD><CDB5><EFBFBD><EFBFBD><EFBFBD>
	if (motornetid=="non")
	{
		if (protools==NETTCP)
		{
			QByteArray buf;
			tcpSocket[0]->waitForReadyRead(1000);
			buf = tcpSocket[0]->readAll();
			if (buf.length()==0)
			{
				return;
			}
			QString str = Motorlist[0] + ":";
			str.append(buf);
			SendLog(str);
			
		}
	}
#ifdef LOGOUT
//	QByteArray array = sc->readAll();
//	QString strofrecive;
//	strofrecive.append(array);
	//QString strtosend = "sender " + ip + ":" + strofrecive;
//	SendLog(strtosend);

#endif		



}

void VinceControl::onClinetDisConnet()
{
	QTcpSocket* sc = dynamic_cast<QTcpSocket*>(sender());
	QString ip = sc->peerAddress().toString().split("::ffff:")[1];
	int lenth = Motorlist.length();
	for (size_t i = 0; i < lenth; i++)
	{
		if (Motorlist[i]==ip)
		{
			Motorlist.removeAt(i);
			isSettingSpeedlist.removeAt(i);
			Speedlist.removeAt(i);
			delete tcpSocket[i];
			tcpSocket.removeAt(i);
			return;
		}

	}
}

void VinceControl::SendLog(QString str)
{
#ifdef LOGOUT
	emit SendLogToCallClass(str);
#endif // LOGOUT

}