#include <stdio.h>
#include <string.h>
#include <errno.h>
#include <math.h>

#include <wiringPi.h>
#include <wiringSerial.h>

#include "Ax12.h"

Ax12::Ax12(FlowController& ComMode, SerialPort& Serial, unsigned char _ID) {
	_flowControlPin = ComMode.getFlowControlPin();
	if (!Serial.open()) {
		Serial.openSerial(1000000); // default baudrate for an ax12
	}
	_baudRate = Serial.getBaudrate();
	ID = _ID;
}
Ax12::Ax12(FlowController& ComMode, SerialPort& Serial, int baud, unsigned char _ID) {
	_flowControlPin = ComMode.getFlowControlPin();
	if (!Serial.open()) {
		Serial.openSerial(baud); // default baudrate for an ax12
	}
	_baudRate = Serial.getBaudrate();
	ID = _ID;
}
Ax12:: ~Ax12() {

}

void Ax12::recieveStatus(FlowController& ComMode, SerialPort& Serial) {
	delayMicroseconds(850); // Allow time for Ax-12 to reply
	ComMode.recieve();
	delayMicroseconds(875);
	if (ComMode.isRecieving())
	{
		unsigned char incomingData[15];
		int bufferSize = 0;
		_newPacket = false;
		_recieveError = false;
		while (serialDataAvail(Serial.getFileDescriptor()) > 0) {
			incomingData[bufferSize] = Serial.read();
			bufferSize++;
		}
		if (bufferSize > 4) {
			_bufferSize = bufferSize;
			if (validChecksum(incomingData)) {
				for (int i = 0; i < bufferSize; i++) {
					_statusRecieved[i] = incomingData[i];
				}
				_newPacket = true;
			}
			else {
				_rxErrors++;
				_recieveError = true;
			}
		}
	}
}
bool Ax12::validChecksum(unsigned char Packet[]) {
	unsigned char checksum = Packet[2];
	for (int i = 0; i < Packet[3]; i++) {
		checksum += Packet[i + 3];
	}
	checksum = ~checksum;
	checksum = checksum & 0xFF;
	if (Packet[_bufferSize - 1] == checksum) {
		return true;
	}
	else {
		return false;
	}
}

void Ax12::reset(FlowController& ComMode, SerialPort& Serial) {
	unsigned char checksum = (~(ID + 0x02 + 0x06)) & 0xFF;
	ComMode.transmit();
	unsigned char packet[7] = { 0xFF,0xFF,ID,0x02,0x06,checksum,'\0' };
	for (int i = 0; i < 6; i++){
	Serial.send(packet[i]);
	}
	recieveStatus(ComMode, Serial);
	if (_newPacket) {
		if (_statusRecieved[4] == 0x00) {
			printf("Reset was successfull!");
		}
		else {
			printf("Reset was not successfull!");
		}
		_newPacket = false;
	}
	else if (_recieveError) {
		printf("Reset Checksum Recieve Error has occured");
		_recieveError = false;
	}
}
void Ax12::ping(FlowController& ComMode, SerialPort& Serial) {
	unsigned char checksum = (~(ID + 0x02 + 0x01)) & 0xFF;
	ComMode.transmit();
	unsigned char packet[7] = { 0xFF,0xFF,ID,0x02,0x01,checksum,'\0' };
	for (int i = 0; i < 6; i++) {
		Serial.send(packet[i]);
	}
	if (_statusReturn) {
		recieveStatus(ComMode, Serial);
		if (_newPacket) {
			if (_statusRecieved[4] == 0x00) {
				printf("Ping was successfull!");
			}
			else {
				printf("Ping was not successfull!");
			}
			_newPacket = false;
		}
		else if (_recieveError) {
			printf("Ping Checksum Recieve Error has occured");
			_recieveError = false;
		}
	}
}

void Ax12::setID(FlowController& ComMode, SerialPort& Serial, unsigned char newID) {
	unsigned char checksum = (~(ID + 0x04 + 0x03 + 0x03 + newID)) & 0xFF; 
	ComMode.transmit();
	unsigned char packet[9] = { 0xFF,0xFF,ID,0x04,0x03,0x03,newID,checksum,'\0' };
	for (int i = 0; i < 8; i++) {
		Serial.send(packet[i]);
	}
	if (_statusReturn) {
		recieveStatus(ComMode, Serial);
		if (_newPacket) {
			if (_statusRecieved[4] == 0x00) {
				printf("SetID was successfull!");
				ID = newID;
			}
			else {
				printf("SetID was not successfull!");
			}
			_newPacket = false;
		}
		else if (_recieveError) {
			printf("SetID Checksum Recieve Error has occured");
			_recieveError = false;
		}
	}
}
void Ax12::setBaud(FlowController& ComMode, SerialPort& Serial, int baud) {
	unsigned char address4Value = (2000000 / baud) - 1;
	unsigned char checksum = (~(ID + 0x04 + 0x03 + 0x04 + address4Value)) & 0xFF; 
	ComMode.transmit();
	unsigned char packet[9] = { 0xFF,0xFF,ID,0x04,0x03,0x04,address4Value,checksum,'\0' };
	for (int i = 0; i < 8; i++) {
		Serial.send(packet[i]);
	}
	if (_statusReturn) {
		recieveStatus(ComMode, Serial);
		if (_newPacket) {
			if (_statusRecieved[4] == 0x00) {
				printf("setBaud was successfull!");
				_baudRate = baud;
			}
			else {
				printf("setBaud was not successfull!");
			}
			_newPacket = false;
		}
		else if (_recieveError) {
			printf("setBaud Checksum Recieve Error has occured");
			_recieveError = false;
		}
	}
}

void Ax12::move(FlowController& ComMode, SerialPort& Serial, double degrees) {
	int position = round(degrees / 300 * 1023);
	if (position > _cwLimit && position < _ccwLimit) {
		char positionH, positionL;
		positionH = position >> 8;
		positionL = position;
		unsigned char checksum = (~(ID + 0x05 + 0x03 + 0x1E + positionL + positionH)) & 0xFF;
		ComMode.transmit();
		unsigned char packet[10] = { 0xFF,0xFF,ID,0x05,0x03,0x1E,positionL,positionH,checksum,'\0' };
		for (int i = 0; i < 9; i++) {
			Serial.send(packet[i]);
		}
		if (_statusReturn) {
			recieveStatus(ComMode, Serial);
			if (_newPacket) {
				if (_statusRecieved[4] == 0x00) {
					printf("Move was successfull!");
					_position = position;
					_positionDeg = degrees;
				}
				else {
					printf("Move was not successfull!");
				}
				_newPacket = false;
			}
			else if (_recieveError) {
				printf("Move Checksum Recieve Error has occured");
				_recieveError = false;
			}
		}
	}
}
void Ax12::setSpeed(FlowController& ComMode, SerialPort& Serial, int speed) {
	char speedH, speedL;
	speedH = speed >> 8;
	speedL = speed;
	unsigned char checksum = (~(ID + 0x05 + 0x03 + 0x20 + speedL + speedH)) & 0xFF;
	ComMode.transmit();
	unsigned char packet[10] = { 0xFF,0xFF,ID,0x05,0x03,0x20,speedL,speedH,checksum,'\0' };
	for (int i = 0; i < 9; i++) {
		Serial.send(packet[i]);
	}
	if (_statusReturn) {
		recieveStatus(ComMode, Serial);
		if (_newPacket) {
			if (_statusRecieved[4] == 0x00) {
				printf("setSpeed was successfull!");
				_speed = speed;
			}
			else {
				printf("setSpeed was not successfull!");
			}
			_newPacket = false;
		}
		else if (_recieveError) {
			printf("setSpeed Checksum Recieve Error has occured");
			_recieveError = false;
		}
	}
} 
void Ax12::moveAtSpeed(FlowController& ComMode, SerialPort& Serial, int speed, double degrees) {
	int position = round(degrees / 300 * 1023);
	char positionH, positionL;
	positionH = position >> 8;
	positionL = position;
	char speedH, speedL;
	speedH = speed >> 8;
	speedL = speed;
	unsigned char checksum = (~(ID + 0x07 + 0x03 + 0x1E + positionL + positionH + speedL + speedH)) & 0xFF;
	ComMode.transmit();
	unsigned char packet[12] = { 0xFF,0xFF,ID,0x05,0x03,0x1E,positionL,positionH,speedL,speedH,checksum,'\0' };
	for (int i = 0; i < 11; i++) {
		Serial.send(packet[i]);
	}
	if (_statusReturn) {
		recieveStatus(ComMode, Serial);
		if (_newPacket) {
			if (_statusRecieved[4] == 0x00) {
				printf("Move at speed was successfull!");
				_position = position;
				_positionDeg = degrees;
				_speed = speed;
			}
			else {
				printf("Move at speed was not successfull!");
			}
			_newPacket = false;
		}
		else if (_recieveError) {
			printf("Move at speed Checksum Recieve Error has occured");
			_recieveError = false;
		}
	}
}

void Ax12::torqueEnable(FlowController& ComMode, SerialPort& Serial, bool Enable) {
	unsigned char checksum = (~(ID + 0x04 + 0x03 + 0x18 + Enable)) & 0xFF; 
	ComMode.transmit();
	unsigned char packet[9] = { 0xFF,0xFF,ID,0x04,0x03,0x18,Enable,checksum,'\0' };
	for (int i = 0; i < 8; i++) {
		Serial.send(packet[i]);
	}
	if (_statusReturn) {
		recieveStatus(ComMode, Serial);
		if (_newPacket) {
			if (_statusRecieved[4] == 0x00) {
				printf("torqueEnable was successfull!");
				_torqueEnabled = Enable;
			}
			else {
				printf("torqueEnable was not successfull!");
			}
			_newPacket = false;
		}
		else if (_recieveError) {
			printf("torqueEnable Checksum Recieve Error has occured");
			_recieveError = false;
		}
	}
}
void Ax12::ledOn(FlowController& ComMode, SerialPort& Serial, bool Enable) {
	unsigned char checksum = (~(ID + 0x04 + 0x03 + 0x19 + Enable)) & 0xFF; 
	ComMode.transmit();
	unsigned char packet[9] = { 0xFF,0xFF,ID,0x04,0x03,0x19,Enable,checksum,'\0' };
	for (int i = 0; i < 8; i++) {
		Serial.send(packet[i]);
	}
	if (_statusReturn) {
		recieveStatus(ComMode, Serial);
		if (_newPacket) {
			if (_statusRecieved[4] == 0x00) {
				printf("ledOn was successfull!");
				_ledOn = Enable;
			}
			else {
				printf("ledOn was not successfull!");
			}
			_newPacket = false;
		}
		else if (_recieveError) {
			printf("ledOn Checksum Recieve Error has occured");
			_recieveError = false;
		}
	}
}

void Ax12::setTempLimit(FlowController& ComMode, SerialPort& Serial, unsigned char Temp) {
	unsigned char checksum = (~(ID + 0x04 + 0x03 + 0x0B + Temp)) & 0xFF;
	ComMode.transmit();
	unsigned char packet[9] = { 0xFF,0xFF,ID,0x04,0x03,0x0B,Temp,checksum,'\0' };
	for (int i = 0; i < 8; i++) {
		Serial.send(packet[i]);
	}
	if (_statusReturn) {
		recieveStatus(ComMode, Serial);
		if (_newPacket) {
			if (_statusRecieved[4] == 0x00) {
				printf("setTempLimit was successfull!");
				_tempLimit = Temp;
			}
			else {
				printf("setTempLimit was not successfull!");
			}
			_newPacket = false;
		}
		else if (_recieveError) {
			printf("setTempLimit Checksum Recieve Error has occured");
			_recieveError = false;
		}
	}

}
void Ax12::setVoltageLimit(FlowController& ComMode, SerialPort& Serial, unsigned char LowVoltage, unsigned char HighVoltage) {
	LowVoltage = LowVoltage * 10;
	HighVoltage = HighVoltage * 10;

	unsigned char checksum = (~(ID + 0x05 + 0x03 + 0x0C + LowVoltage + HighVoltage)) & 0xFF;
	ComMode.transmit();
	unsigned char packet[10] = { 0xFF,0xFF,ID,0x04,0x03,0x0C,LowVoltage,HighVoltage,checksum,'\0'};
	for (int i = 0; i < 9; i++) {
		Serial.send(packet[i]);
	}
	if (_statusReturn) {
		recieveStatus(ComMode, Serial);
		if (_newPacket) {
			if (_statusRecieved[4] == 0x00) {
				printf("setVoltageLimit was successfull!");
				_lowVoltLimit = LowVoltage / 10;
				_highVoltLimit = HighVoltage / 10;
			}
			else {
				printf("setVoltageLimit was not successfull!");
			}
			_newPacket = false;
		}
		else if (_recieveError) {
			printf("setVoltageLimit Checksum Recieve Error has occured");
			_recieveError = false;
		}
	}

}
void Ax12::setAngleLimit(FlowController& ComMode, SerialPort& Serial, double CWDeg, double CCWDeg) {
	int cwLimit = round(CWDeg / 300 * 1023);
	int ccwLimit = round(CCWDeg / 300 * 1023);

	char CW_H, CW_L, CCW_H, CCW_L;
	CW_H = cwLimit >> 8;
	CW_L = cwLimit;               
	CCW_H = ccwLimit >> 8;
	CCW_L = ccwLimit;

	unsigned char checksum = (~(ID + 0x07 + 0x03 + 0x06 + CW_L + CW_H + CCW_L + CCW_H)) & 0xFF;
	ComMode.transmit();
	unsigned char packet[12] = { 0xFF, 0xFF ,ID ,0x07 ,0x03 ,0x06 ,CW_L ,CW_H ,CCW_L ,CCW_H ,checksum ,'\0' };
	for (int i = 0; i < 11; i++) {
		Serial.send(packet[i]);
	}
	if (_statusReturn) {
		recieveStatus(ComMode, Serial);
		if (_newPacket) {
			if (_statusRecieved[4] == 0x00) {
				printf("setAngleLimit was successfull!");
				_cwLimit = CWDeg;
				_ccwLimit = CCWDeg;
			}
			else {
				printf("setAngleLimit was not successfull!");
			}
			_newPacket = false;
		}
		else if (_recieveError) {
			printf("setAngleLimit Checksum Recieve Error has occured");
			_recieveError = false;
		}
	}

}
void Ax12::setTorqueLimit(FlowController& ComMode, SerialPort& Serial, unsigned char Torque) {
	if (Torque > 1023) {
		Torque = 1023;
	}
	char maxTorqueH, maxTorqueL;
	maxTorqueH = Torque >> 8;         
	maxTorqueL = Torque;

	unsigned char checksum = (~(ID + 0x05 + 0x03 + 0x0E + maxTorqueL + maxTorqueH)) & 0xFF;
	ComMode.transmit();
	unsigned char packet[10] = { 0xFF,0xFF,ID,0x05,0x03,0x0E,maxTorqueL,maxTorqueH,checksum ,'\0' };
	for (int i = 0; i < 9; i++) {
		Serial.send(packet[i]);
	}
	if (_statusReturn) {
		recieveStatus(ComMode, Serial);
		if (_newPacket) {
			if (_statusRecieved[4] == 0x00) {
				printf("setTorqueLimit ROM was successfull!");
				
			}
			else {
				printf("setTorqueLimit ROM was not successfull!");
			}
			_newPacket = false;
		}
		else if (_recieveError) {
			printf("setTorqueLimit ROM Checksum Recieve Error has occured");
			_recieveError = false;
		}
	}

	checksum = (~(ID + 0x05 + 0x03 + 0x22 + maxTorqueL + maxTorqueH)) & 0xFF;
	ComMode.transmit();
	unsigned char packet2[10] = { 0xFF,0xFF,ID,0x05,0x03,0x22,maxTorqueL,maxTorqueH,checksum,'\0' };
	for (int i = 0; i < 9; i++) {
		Serial.send(packet[i]);
	}
	if (_statusReturn) {
		recieveStatus(ComMode, Serial);
		if (_newPacket) {
			if (_statusRecieved[4] == 0x00) {
				printf("setTorqueLimit RAM was successfull!");

			}
			else {
				printf("setTorqueLimit RAM was not successfull!");
			}
			_newPacket = false;
		}
		else if (_recieveError) {
			printf("setTorqueLimit RAM Checksum Recieve Error has occured");
			_recieveError = false;
		}
	}

}

void Ax12::setLedAlarm(FlowController& ComMode, SerialPort& Serial, unsigned char AlarmType) {
	unsigned char checksum = (~(ID + 0x04 + 0x03 + 0x11 + AlarmType)) & 0xFF;
	ComMode.transmit();
	unsigned char packet[9] = { 0xFF,0xFF,ID,0x04,0x03,0x11,AlarmType,checksum,'\0' };
	for (int i = 0; i < 8; i++) {
		Serial.send(packet[i]);
	}
	if (_statusReturn) {
		recieveStatus(ComMode, Serial);
		if (_newPacket) {
			if (_statusRecieved[4] == 0x00) {
				printf("setLedAlarm change was successfull!");

			}
			else {
				printf("setLedAlarm change was not successfull!");
			}
			_newPacket = false;
		}
		else if (_recieveError) {
			printf("setLedAlarm change Checksum Recieve Error has occured");
			_recieveError = false;
		}
	}
}
void Ax12::setShutDownAlarm(FlowController& ComMode, SerialPort& Serial, unsigned char AlarmType) {
	unsigned char checksum = (~(ID + 0x04 + 0x03 + 0x12 + AlarmType)) & 0xFF;
	ComMode.transmit();
	unsigned char packet[9] = { 0xFF,0xFF,ID,0x04,0x03,0x12,AlarmType,checksum,'\0' };
	for (int i = 0; i < 8; i++) {
		Serial.send(packet[i]);
	}
	if (_statusReturn) {
		recieveStatus(ComMode, Serial);
		if (_newPacket) {
			if (_statusRecieved[4] == 0x00) {
				printf("setShutDownAlarm change was successfull!");

			}
			else {
				printf("setShutDownAlarm change was not successfull!");
			}
			_newPacket = false;
		}
		else if (_recieveError) {
			printf("setShutDownAlarm change Checksum Recieve Error has occured");
			_recieveError = false;
		}
	}

}
void Ax12::setStatusReturnLevel(FlowController& ComMode, SerialPort& Serial, unsigned char Response) {
	if (Response > 2) Response = 2;
	if (Response < 0) Response = 0;
	unsigned char checksum = (~(ID + 0x04 + 0x03 + 0x10 + Response)) & 0xFF;
	ComMode.transmit();
	unsigned char packet[9] = { 0xFF,0xFF,ID,0x04,0x03,0x10,Response,checksum,'\0' };
	for (int i = 0; i < 8; i++) {
		Serial.send(packet[i]);
	}
		recieveStatus(ComMode, Serial);
		if (_newPacket) {
			if (_statusRecieved[4] == 0x00) {
				printf("status Return Level change was successfull!");
				if (Response == 0 || Response == 1) _statusReturn = false;
				if (Response == 2) _statusReturn = true;
			}
			else {
				printf("status Return Level change was not successfull!");
			}
			_newPacket = false;
		}
		else if (_recieveError) {
			printf("status Return Level change Checksum Recieve Error has occured");
			_recieveError = false;
		}
	
}
void Ax12::setReturnDelay(FlowController& ComMode, SerialPort& Serial, unsigned char ReturnDelay) {

	unsigned char checksum = (~(ID + 0x04 + 0x03 + 0x05 + (ReturnDelay/2))) & 0xFF;
	ComMode.transmit();
	unsigned char packet[9] = { 0xFF,0xFF,ID,0x04,0x03,0x05,(ReturnDelay/2),checksum,'\0' };
	for (int i = 0; i < 8; i++) {
		Serial.send(packet[i]);
	}
	if (_statusReturn) {
	recieveStatus(ComMode, Serial);
		if (_newPacket) {
			if (_statusRecieved[4] == 0x00) {
				printf("Return Delay change was successfull!");

			}
			else {
				printf("Return Delay change was not successfull!");
			}
			_newPacket = false;
		}
		else if (_recieveError) {
			printf("Return Delay change Checksum Recieve Error has occured");
			_recieveError = false;
		}
	}

}

void Ax12::setCompMargin(FlowController& ComMode, SerialPort& Serial, unsigned char CWMargin, unsigned char CCWMargin) {
	unsigned char checksum = (~(ID + 0x05 + 0x03 + 0x1A + CWMargin + CCWMargin)) & 0xFF;
	ComMode.transmit();
	unsigned char packet[10] = { 0xFF,0xFF,ID,0x05,0x03,0x1A,CWMargin,CCWMargin,checksum,'\0' };
	for (int i = 0; i < 9; i++) {
		Serial.send(packet[i]);
	}
	if (_statusReturn) {
		recieveStatus(ComMode, Serial);
		if (_newPacket) {
			if (_statusRecieved[4] == 0x00) {
				printf("Compliance Margin change was successfull!");
			}
			else {
				printf("Compliance Margin change was not successfull!");
			}
			_newPacket = false;
		}
		else if (_recieveError) {
			printf("Compliance Margin change Checksum Recieve Error has occured");
			_recieveError = false;
		}
	}

}
void Ax12::setCompSlope(FlowController& ComMode, SerialPort& Serial, unsigned char CWSlope, unsigned char CCWSlope) {
	unsigned char checksum = (~(ID + 0x05 + 0x03 + 0x1C + CWSlope + CCWSlope)) & 0xFF;
	ComMode.transmit();
	unsigned char packet[10] = { 0xFF,0xFF,ID,0x05,0x03,0x1C,CWSlope,CCWSlope,checksum,'\0' };
	for (int i = 0; i < 9; i++) {
		Serial.send(packet[i]);
	}
	if (_statusReturn) {
		recieveStatus(ComMode, Serial);
		if (_newPacket) {
			if (_statusRecieved[4] == 0x00) {
				printf("Compliance Slope change was successfull!");

			}
			else {
				printf("Compliance Slope change was not successfull!");
			}
			_newPacket = false;
		}
		else if (_recieveError) {
			printf("Compliance Slope change Checksum Recieve Error has occured");
			_recieveError = false;
		}
	}

}
void Ax12::setPunch(FlowController& ComMode, SerialPort& Serial, int Punch) {
	char Punch_H, Punch_L;
	Punch_H = Punch >> 8;           
	Punch_L = Punch;
	unsigned char checksum = (~(ID + 0x05 + 0x03 + 0x30 + Punch_L + Punch_H )) & 0xFF;
	ComMode.transmit();
	unsigned char packet[10] = { 0xFF,0xFF,ID,0x05,0x03,0x30,Punch_L,Punch_H ,checksum,'\0' };
	for (int i = 0; i < 9; i++) {
		Serial.send(packet[i]);
	}
	if (_statusReturn) {
		recieveStatus(ComMode, Serial);
		if (_newPacket) {
			if (_statusRecieved[4] == 0x00) {
				printf("Punch was successfull!");
				_punch = Punch;
			}
			else {
				printf("Punch was not successfull!");
			}
			_newPacket = false;
		}
		else if (_recieveError) {
			printf("Punch Checksum Recieve Error has occured");
			_recieveError = false;
		}
	}
}

void Ax12::readAngleLimit(FlowController& ComMode, SerialPort& Serial) {
	char cwH, cwL, ccwH, ccwL;
	unsigned char checksum = (~(ID + 0x04 + 0x02 + 0x06 + 0x04)) & 0xFF;
	ComMode.transmit();
	unsigned char packet[9] = { 0xFF,0xFF,ID,0x04,0x02,0x06,0x04,checksum,'\0' };
	for (int i = 0; i < 8; i++) {
		Serial.send(packet[i]);
	}
	recieveStatus(ComMode, Serial);
	if (_newPacket) {
		if (_statusRecieved[4] == 0x00) {
			printf("readAngleLimit was successfull!");
			cwL = _statusRecieved[5];
			cwH = _statusRecieved[6];
			ccwL = _statusRecieved[7];
			ccwH = _statusRecieved[8];
			int rotation = cwH << 8;
			rotation = rotation + cwL;
			_cwLimit = rotation;

			rotation = ccwH << 8;
			rotation = rotation + ccwL;
			_ccwLimit = rotation;
		
		}
		else {
			printf("readAngleLimit was not successfull!");	
		}
		_newPacket = false;
	}
	else if (_recieveError) {
		printf("readAngleLimit Checksum Recieve Error has occured");
		_recieveError = false;
		
	}

}

int Ax12::readTemperature(FlowController& ComMode, SerialPort& Serial) {
	unsigned char checksum = (~(ID + 0x04 + 0x02 + 0x2B + 0x01)) & 0xFF;
	ComMode.transmit();
	unsigned char packet[9] = { 0xFF,0xFF,ID,0x04,0x02,0x2B,0x01,checksum,'\0' };
	for (int i = 0; i < 8; i++) {
		Serial.send(packet[i]);
	}
	recieveStatus(ComMode, Serial);
	if (_newPacket) {
		if (_statusRecieved[4] == 0x00) {
			printf("readTemperature was successfull!");
			_temperature = _statusRecieved[5];
			return _statusRecieved[5];
		}
		else {
			printf("readTemperature was not successfull!");
			return -111;
		}
		_newPacket = false;
	}
	else if (_recieveError) {
		printf("readTemperature Checksum Recieve Error has occured");
		_recieveError = false;
		return -111;
	}
}
int Ax12::readPosition(FlowController& ComMode, SerialPort& Serial) {
	char positionH, positionL;
	unsigned char checksum = (~(ID + 0x04 + 0x02 + 0x24 + 0x02)) & 0xFF;
	ComMode.transmit();
	unsigned char packet[9] = { 0xFF,0xFF,ID,0x04,0x02,0x24,0x02,checksum,'\0' };
	for (int i = 0; i < 8; i++) {
		Serial.send(packet[i]);
	}
	recieveStatus(ComMode, Serial);
	if (_newPacket) {
		if (_statusRecieved[4] == 0x00) {
			printf("readPosition was successfull! \n \r");
			positionL = _statusRecieved[5];
			positionH = _statusRecieved[6];
			printf("%d \n \r", positionL);
			printf("%d \n \r", positionH);
			int position = positionH << 8;
			position = position + positionL;
			_position = position;
			printf("%d \n \r", _position);
			_positionDeg = position / 1023 * 300;
			return _positionDeg;
		}
		else {
			printf("readPosition was not successfull!");
			return -1;
		}
		_newPacket = false;
	}
	else if (_recieveError) {
		printf("readPosition Checksum Recieve Error has occured");
		_recieveError = false;
		return -1;
	}

}
float Ax12::readVoltage(FlowController& ComMode, SerialPort& Serial) {
	unsigned char checksum = (~(ID + 0x04 + 0x02 + 0x2A + 0x01)) & 0xFF;
	ComMode.transmit();
	unsigned char packet[9] = { 0xFF,0xFF,ID,0x04,0x02,0x2A,0x01,checksum,'\0' };
	for (int i = 0; i < 8; i++) {
		Serial.send(packet[i]);
	}
	recieveStatus(ComMode, Serial);
	if (_newPacket) {
		if (_statusRecieved[4] == 0x00) {
			printf("readVoltage was successfull!");
			_voltage = _statusRecieved[5] / 10;
			return _voltage;
		}
		else {
			printf("readVoltage was not successfull!");
			return -1;
		}
		_newPacket = false;
	}
	else if (_recieveError) {
		printf("readVoltage Checksum Recieve Error has occured");
		_recieveError = false;
		return -1;
	}
}
int Ax12::readSpeed(FlowController& ComMode, SerialPort& Serial) {
	char speedH, speedL;
	unsigned char checksum = (~(ID + 0x04 + 0x02 + 0x26 + 0x02)) & 0xFF;
	ComMode.transmit();
	unsigned char packet[9] = { 0xFF,0xFF,ID,0x04,0x02,0x26,0x02,checksum,'\0' };
	for (int i = 0; i < 8; i++) {
		Serial.send(packet[i]);
	}
	recieveStatus(ComMode, Serial);
	if (_newPacket) {
		if (_statusRecieved[4] == 0x00) {
			printf("readSpeed was successfull!");
			speedL = _statusRecieved[5];
			speedH = _statusRecieved[6];
			int speed = speedH << 8;
			speed = speed + speedL;
			_speed = speed;
			return _speed;

		}
		else {
			printf("readSpeed was not successfull!");
			return -1;
		}
		_newPacket = false;
	}
	else if (_recieveError) {
		printf("readSpeed Checksum Recieve Error has occured");
		_recieveError = false;
		return -1;
	}

}
int Ax12::readLoad(FlowController& ComMode, SerialPort& Serial) {
	char loadH, loadL;
	unsigned char checksum = (~(ID + 0x04 + 0x02 + 0x28 + 0x02)) & 0xFF;
	ComMode.transmit();
	unsigned char packet[9] = { 0xFF,0xFF,ID,0x04,0x02,0x28,0x02,checksum,'\0' };
	for (int i = 0; i < 8; i++) {
		Serial.send(packet[i]);
	}
	recieveStatus(ComMode, Serial);
	if (_newPacket) {
		if (_statusRecieved[4] == 0x00) {
			printf("readLoad was successfull!");
			loadL = _statusRecieved[5];
			loadH = _statusRecieved[6];
			_loadDirCw = loadH >> 2;
			int load = (loadH & 0b00000011) << 8;
			load = load + loadL;
			_load = load;
			return load;
		}
		else {
			printf("readLoad was not successfull!");
			return -1;
		}
		_newPacket = false;
	}
	else if (_recieveError) {
		printf("readLoad Checksum Recieve Error has occured");
		_recieveError = false;
		return -1;
	}

}
bool Ax12::moving(FlowController& ComMode, SerialPort& Serial) {
	unsigned char checksum = (~(ID + 0x04 + 0x02 + 0x2E + 0x01)) & 0xFF;
	ComMode.transmit();
	unsigned char packet[9] = { 0xFF,0xFF,ID,0x04,0x02,0x2E,0x01,checksum,'\0' };
	for (int i = 0; i < 8; i++) {
		Serial.send(packet[i]);
	}
	recieveStatus(ComMode, Serial);
	if (_newPacket) {
		if (_statusRecieved[4] == 0x00) {
			printf("moving was successfull!");
			return _statusRecieved[5];
		}
		else {
			printf("moving was not successfull!");
			return false;
		}
		_newPacket = false;
	}
	else if (_recieveError) {
		printf("moving Checksum Recieve Error has occured");
		_recieveError = false;
		return false;
	}
}

int Ax12::getRxErrors(void) {
	return _rxErrors;
}
bool Ax12::getStatusReturn(void) {
	return _statusReturn;
}
int Ax12::getTemp(void) {
	return _temperature;
}
int Ax12::getPosition(void) {
	return _position;
}
double Ax12::getPositionDeg(void) {
	return _positionDeg;
}
int Ax12::getSpeed(void) {
	return _speed;
}
float Ax12::getVoltage(void) {
	return _voltage;
}
int Ax12::getPunch(void) {
	return _punch;
}
int Ax12::getloadMag(void) {
	return _load;
}
bool Ax12::isLoadCW(void) {
	return _loadDirCw;
}
bool Ax12::isTorqueEnabled(void) {
	return _torqueEnabled;
}
bool Ax12::isLedOn(void) {
	return _ledOn;
}
int Ax12::getCwLimit(void) {
	return _cwLimit;
}
int Ax12::getCcwLimit(void) {
	return _ccwLimit;
}
unsigned char Ax12::getID(void) {
	return ID;
}