//7 Go Forward DIRECTION
void Robot::BasicDriveForward(double v, double t){
///values needed to calculate teh new angular velocities of the wheels
double r0 = Robot::m_wheel[0].GetRadius();
double r1 = Robot::m_wheel[1].GetRadius();
double wl = Robot::m_wheel[0].GetAngularVelocity();
double wr = Robot::m_wheel[1].GetAngularVelocity();
double v_old = Robot::GetLinearVelocity();
std::cout<<"I have driven with the velocity "<<v<<" for time "<<t<<std::endl;
double ds = v*t;
double da=0.0;
Robot::SetPosition(ds,da);
int vel = static_cast<int>(250*v); //bo najfajniej się wtedy rusza
int tm = static_cast<int>(80*t);
int dir;
if(vel>0) dir=16;
else {dir=1; vel=vel*(-1);}
SendFrame(7, dir, vel, tm);
bl = ReceiveFrame(7);
emit _BasicDriveForward(v,t);
//emit _BasicDriveForward((double)bl[2]/250,(double)bl[3]/80);
this->SetLinearVelocity(v);
socket->flush();
bl = ReceiveFrame(7);
Robot::m_wheel[0].SetAngularVelocity(v/(2*PI*r0));
Robot::m_wheel[1].SetAngularVelocity(v/(2*PI*r1));
}
//5 Arm Position
void Robot::BasicArmPositionChange(POSITION pos)
{
SendFrame(6,0, (int)pos, 0 );
emit _BasicArmPositionChange(pos);
bl=ReceiveFrame(6);
this->m_arm.SetPosition(static_cast<POSITION>(bl[2]));
bl=ReceiveFrame(6);
}
//8 Turn DIRECTION
void Robot::BasicTurn(double angle, double t){
double w = Robot::GetAngularVelocity();
double rl = Robot::m_wheel[0].GetRadius();
double rr = Robot::m_wheel[1].GetRadius();
double d = Robot::GetWheelTrack();
double wl = Robot::m_wheel[0].GetAngularVelocity();
double wr = Robot::m_wheel[1].GetAngularVelocity();
///check the angle and then decide which angular velocity changes
if (angle>0) {
///calculate the value
//Robot::m_wheel[0].SetAngularVelocity(w*d/rr+wr+a/t);
Robot::m_wheel[0].SetAngularVelocity(angle/t/100);
}
else {
///calculate the value
//Robot::m_wheel[1].SetAngularVelocity(-wl+w*d/rl+a/t);
Robot::m_wheel[1].SetAngularVelocity(-angle/t/100);
}
///temporary velocity while turning - (v_left + v_right)/2
double ds = (wl * rl + wr * rr)/2*t;
double da=angle;
///change position
Robot::SetPosition(ds,da);
std::cout<<"I have turned to an angle "<<angle<<std::endl;
//0-100
int vel = (int)(angle*8);
int tm = (int)(t*80);
int dir;
if(vel>0){ dir=16;}
else {dir=1; vel=vel*(-1);}
SendFrame(8,dir, vel, tm);
socket->flush();
bl = ReceiveFrame(12);
//emit _BasicTurn((double)bl[2]/8, (double)bl[3]/80);
emit _BasicTurn(angle,t);
bl=ReceiveFrame(12);
Robot::m_wheel[1].SetAngularVelocity(0);
Robot::m_wheel[0].SetAngularVelocity(0);
socket->flush();
}
//3 Driving Steer Engine
void Robot::BasicEngineDrivingSteer(int i, double w){
int val = (int)(w);
int dir;
switch(i){
case 1: {
if(w>0)
dir=52;
else
dir=56;
break;}
case 2: {
if(w>0)
dir=68;
else
dir=72;
break;
}
}
if(w<0) val=val*(-1);
SendFrame(3, dir, val,0);
bl = ReceiveFrame(2);
switch(bl[2]){
case 52: {m_wheel[0].SetEngineSpeed((double)bl[3]); m_wheel[0].SetEngineDirection(0); break;}
case 56: {m_wheel[0].SetEngineSpeed((double)bl[3]); m_wheel[0].SetEngineDirection(1); break;}
case 68: {m_wheel[1].SetEngineSpeed((double)bl[3]); m_wheel[0].SetEngineDirection(0); break;}
case 72: {m_wheel[1].SetEngineSpeed((double)bl[3]); m_wheel[0].SetEngineDirection(1); break;}
}
this->m_wheel[i].SetEngineSpeed((double)bl[3]);
emit _BasicEngineDrivingSteer(i, double(bl[3]));
}
bool GPSSensorUBX::GetPosition(int32_t *lat, int32_t *lon, int32_t *alt, uint32_t *hAcc, uint32_t *vAcc) {
//DBG("TX NAVPOS");
PollCmd(UBXMsgClassNAV, UBXMsgIdNAV_POSLLH);
//DBG("RX NAVPOS");
if(ReceiveFrame(10000)){
if((recvBuff[0] == 0xb5) && (recvBuff[1] == 0x62)) {
//DBG("There is a Frame in buffer");
if((recvBuff[2] == 0x01) && (recvBuff[3] == 0x02)) {
DBG("OK this is a POSLLH frame");
UBXNAV_POSLLH *pPosLlH = (UBXNAV_POSLLH*)(recvBuff+6);
*lat = pPosLlH->lat;
*lon = pPosLlH->lon;
*alt = pPosLlH->height;
*hAcc = pPosLlH->hAcc;
*vAcc = pPosLlH->vAcc;
DBG("%08x %08x %08x %08x %08x",*lat,*lon,*alt,*hAcc,*vAcc);
DBG("%ld %ld %ld %ld %ld",*lat,*lon,*alt,*hAcc,*vAcc);
if((*lat != 0) & (*lon != 0)) {
DBG("VALID POS");
fixed = true;
return true;
}
}
}
}
return false;
}
//39 Arm to go
void Robot::MiningArmPosition1(){
SendFrame(39,0,0,0);
bl = ReceiveFrame(39);
m_arm.SetPosition(a);
emit _MiningArmPosition1();
this->SetState(BASIC);
}
BOOL CGatewayInfoteamSerialToI::ReceiveFrameRepeated(CInterfaceManagerBase* pInterfaceManager,HANDLE hI_Handle,HANDLE hTransactionHandle, DWORD p_ulMaxPackageSize, DWORD* pdRetPackageSize,DWORD* pdRetChecksum,void** ppRetDataBuffer,DWORD* pdRetDataBufferLength,DWORD dTimeout,CErrorInfo* pErrorInfo)
{
const DWORD k_MaxReceivingCount = 3;
CErrorInfo errorInfo;
DWORD dReceivingCount = 0;
BOOL oSendRepAck = TRUE;
while(dReceivingCount < k_MaxReceivingCount)
{
dReceivingCount++;
if(dReceivingCount == k_MaxReceivingCount) oSendRepAck = FALSE;
if(ReceiveFrame(pInterfaceManager,hI_Handle,hTransactionHandle, p_ulMaxPackageSize, pdRetPackageSize,pdRetChecksum,ppRetDataBuffer,pdRetDataBufferLength,dTimeout,oSendRepAck,&errorInfo))
{
return TRUE;
}
else
{
if(!oSendRepAck || (errorInfo.GetErrorCode() != k_Error_InfoteamSerial_BadDataSizeReceived) || (errorInfo.GetErrorCode() != k_Error_InfoteamSerial_BadCrcReceived))
{
if(pErrorInfo) *pErrorInfo = errorInfo;
return FALSE;
}
}
}
if(pErrorInfo) *pErrorInfo = errorInfo;
return FALSE;
}
ssize_t RawSocketComm::RecvData(void* buff, size_t len, int flags, SA* from, socklen_t* from_len) {
size_t remained_len = len;
size_t received_bytes = 0;
char* ptr = (char*)buff;
int bytes;
while (remained_len > 0) {
if ( (bytes = ReceiveFrame(recv_frame.frame_buffer)) < 0) {
return bytes;
}
if (IsMyPacket()) {
int data_len = bytes - ETH_HLEN;
recv_frame.payload_size = data_len;
memcpy(ptr, recv_frame.data, data_len);
// cout << "One RAW frame received." << endl;
// cout << "Source Addr: " << this->GetMacAddrString(recv_frame.src_addr) << endl;
// cout << "Dest Addr: " << this->GetMacAddrString(recv_frame.dst_addr) << endl;
// cout << "Payload Size: " << recv_frame.payload_size << endl;
received_bytes += data_len;
ptr += data_len;
remained_len -= data_len;
return received_bytes;
}
}
return len;
}
//34 Calibration
void Robot::MiningCalibration(){
SendFrame(34,0,0,0);
bl = ReceiveFrame(34);
//started
emit _MiningCalibration();
}
//32 Arm Position 4
void Robot::MiningArmPosition4(){
SendFrame(32,0,0,0);
bl = ReceiveFrame(32);
m_arm.SetPosition(d);
emit _MiningArmPosition4();
}
//33 Cylinder start
void Robot::MiningCylinderStart(){
SendFrame(33,0,0,0);
bl = ReceiveFrame(33);
//started
emit _MiningCylinderStart();
}
//38 Mass measurement
void Robot::MiningTensometerMass(){
SendFrame(38,0,0,0);
bl = ReceiveFrame(38);
if(bl==NULL) return;
double mass = bl[1];
m_cylinder.SetWeight(mass);
emit _MiningTensometerMass();
}
//35 Lower to ground
void Robot::MiningCylinderToGround(double w){
int val = (int)(w*255/5);
SendFrame(35,0,val,0);
bl = ReceiveFrame(35);
//started
emit _MiningCylinderToGround(w);
}
//31 Cylinder check
void Robot::MiningCylinderState(bool opened){
SendFrame(31,0,0,0);
bl = ReceiveFrame(30);
//opened
emit _MiningCylinderState(opened);
}
//46 Cylinder rotate //rotate
void Robot::UnloadCylinderRotate(double angle, double w){
int val1 = (int)(angle);
int val2 = (int)(w);
SendFrame(46,0,val1,val2);
bl = ReceiveFrame(46);
emit _UnloadCylinderRotate(angle,w);
//emit _UnloadCylinderRotate((double)bl[2], (double)bl[3]);
}
//lewy, prawy
void Robot::BasicTurnArc(bool dir1, bool dir2, double w1, double w2){
//w2- czas
//if(dir1)
//SendFrame(9, dir,(int)(w1*255/5),(int)(w2*255/5));
bl = ReceiveFrame(9);
}
//4 0 Cylinder
void Robot::BasicCylinderSetToZero(double w){
int val = (int)(w);
SendFrame(4,0,w,0);
bl=ReceiveFrame(4);
emit _BasicCylinderSetToZero(w);
}
//6 Electromagnets
void Robot::BasicElectromagnetSet(bool on) {
//emit Electromagnet(On);
SendFrame(6, 0, (int)on, 0);
bl= ReceiveFrame(6);
this->m_cylinder.SetElectromagnet((bool)bl[2]);
emit _BasicElectromagnetSet((bool)bl[2]);
}
//36 Power Control
void Robot::MiningPowerControl(double U, double I){
int u = (int)(U);
int i = (int)(I);
SendFrame(36,0,i,u);
bl = ReceiveFrame(36);
SetMaxCurrentVoltage(U,I);
emit _MiningPowerControl((double)bl[2],(double)bl[3]);
}
//105 `Autonomy //wat
void Robot::SecurityAutonomy(){
SendFrame(105,0,0,0);
bl = ReceiveFrame(105);
if(bl==NULL) return;
if(bl[2]!=(char)1)
emit _SecurityAutonomy();
this->SetState(AUTONOMY);
}
//102 Stahp all driving eng.
void Robot::SecurityDrivingEnginesStop(){
SendFrame(102,0,0,0);
bl = ReceiveFrame(102);
for(int i=0; i<2; ++i){
m_wheel[i].Stop();
}
emit _SecurityDrivingEnginesStop();
}
//30 Initiate DOALL
void Robot::MiningInitiate(){
SendFrame(30,0,0,0);
bl = new char[4];
bl = ReceiveFrame(30);
emit _MiningInitiate();
this->SetState(MINING);
}
//48 Arm pos check //wat
void Robot::UnloadArmPositionCheck(){
SendFrame(48,0,8,0);
bl = ReceiveFrame(48);
if (bl==NULL) return;
if(bl[2]!= (char)1)
BasicArmPositionChange(b);
emit _UnloadArmPositionCheck();
this->SetState(BASIC);
}
//101 Stop all engines
void Robot::SecurityAllEnginesStop(){
SendFrame(101,0,0,0);
bl = ReceiveFrame(101);
for(int i=0; i<2; ++i){
m_wheel[i].Stop();
}
m_arm.Stop();
m_cylinder.Stop();
emit _SecurityAllEnginesStop();
}
void AmsConnection::Recv()
{
AmsTcpHeader amsTcpHeader;
AoEHeader aoeHeader;
for ( ; ownIp; ) {
Receive(amsTcpHeader);
if (amsTcpHeader.length() < sizeof(aoeHeader)) {
LOG_WARN("Frame to short to be AoE");
ReceiveJunk(amsTcpHeader.length());
continue;
}
Receive(aoeHeader);
if (aoeHeader.cmdId() == AoEHeader::DEVICE_NOTIFICATION) {
ReceiveNotification(aoeHeader);
continue;
}
auto response = GetPending(aoeHeader.invokeId(), aoeHeader.targetPort());
if (!response) {
LOG_WARN("No response pending");
ReceiveJunk(aoeHeader.length());
continue;
}
ReceiveFrame(response->frame, aoeHeader.length());
switch (aoeHeader.cmdId()) {
case AoEHeader::READ_DEVICE_INFO:
case AoEHeader::READ:
case AoEHeader::WRITE:
case AoEHeader::READ_STATE:
case AoEHeader::WRITE_CONTROL:
case AoEHeader::ADD_DEVICE_NOTIFICATION:
case AoEHeader::DEL_DEVICE_NOTIFICATION:
case AoEHeader::READ_WRITE:
break;
default:
LOG_WARN("Unkown AMS command id");
response->frame.clear();
}
response->errorCode = aoeHeader.errorCode();
response->Notify();
}
}
//2 Steer Engine Arm/Cylinder //1-cylinder, 2-arm, 3,4-wheels
void Robot::BasicEngineSteer(int i, double w){
int dir;
switch(i){
case 1: {
if(w>0) dir=20;
else dir = 24;
break;}
case 2: {
if(w>0) dir=36;
else dir=40;
break;}
case 3: {
if(w>0) dir=52;
else dir=56;
break;}
case 4: {
if(w>0) dir = 68;
else dir=72;
break;}
}
int val = (int)(w);
if(val<0) val=val*(-1);
SendFrame(2, dir, val,0);
bl = ReceiveFrame(2);
if(bl!=NULL) std::cout<<"OK";
switch(bl[2]){
case 20: {i=1; m_cylinder.SetEngineSpeed((double)bl[3]); m_cylinder.SetEngineDirection(0); break;}
case 24: {i=1;m_cylinder.SetEngineSpeed((double)bl[3]); m_cylinder.SetEngineDirection(1); break;}
case 36: {i=2;m_arm.SetEngineSpeed((double)bl[3]); m_arm.SetEngineDirection(0); break;}
case 40: {i=2;m_arm.SetEngineSpeed((double)bl[3]); m_arm.SetEngineDirection(1); break;}
case 52: {i=3;m_wheel[0].SetEngineSpeed((double)bl[3]); m_wheel[0].SetEngineDirection(0); break;}
case 56: {i=3;m_wheel[0].SetEngineSpeed((double)bl[3]); m_wheel[0].SetEngineDirection(1); break;}
case 68: {i=4;m_wheel[1].SetEngineSpeed((double)bl[3]); m_wheel[0].SetEngineDirection(0); break;}
case 72: {i=4;m_wheel[1].SetEngineSpeed((double)bl[3]); m_wheel[0].SetEngineDirection(1); break;}
}
emit _BasicEngineSteer(i,(double)bl[3]);
}
bool GPSSensorUBX::GetTime(uint32_t *date,uint32_t *time) {
DBG("TX TIMEUTC");
PollCmd(UBXMsgClassNAV, UBXMsgIdNAV_TIMEUTC);
DBG("RX TIMEUTC");
if(ReceiveFrame(10000)){
if((recvBuff[0] == 0xb5) && (recvBuff[1] == 0x62)) {
DBG("There is a Frame in buffer");
if((recvBuff[2] == 0x01) && (recvBuff[3] == 0x21)) {
DBG("OK This is a TIMEUTC Frame");
UBXNAV_TIMEUTC *pTimeUTC= (UBXNAV_TIMEUTC*)(recvBuff+6);
*date = (pTimeUTC->year>2000)?(pTimeUTC->year-2000):pTimeUTC->year;
*date += (pTimeUTC->month * 100);
*date += (pTimeUTC->day * 10000);
*time = pTimeUTC->sec;
*time *= pTimeUTC->min;
*time *= pTimeUTC->hour;
return true;
}
}
}
return false;
}
int SendPacket(char dest, char* Spacket) {
/*
Packet length = 122 => max 6 frames
split packet into frames, send frame, await acknowledgement
*/
put_string("\r\nstring passed to DLL: ");
put_string(Spacket);
struct frame data[FRAMECOUNT];
int no_frames;
no_frames = makeframe(&data, dest, Spacket, 0);
put_string("\r\n\nnumber of frames generated: ");
put_number(no_frames);
uint8_t *bufptr;
char temp[50];
int i,k, send_complete;
struct frame ack;
unsigned long time;
for(i = 0; i < no_frames; i++) {
send_complete = 0;
if((uint8_t)dest != (uint8_t)BROADCAST) {
put_string("\r\nSend message and await acknowledgement: ");
while(!send_complete) {
///////////////////send//////////////////////
put_frame(data[i]);
SendFrame(data[i].frame);
time = millis() + 1000;
while((millis() != time)) {
///////////check for acknowledgemt/////////////////
//put_char(':');
if(ReceiveFrame()) {
//send_complete = 1;
ReceiveFrame(temp);
// bufptr = rfm12_rx_buffer();
// for(k = 0; k < (rfm12_rx_len()); k++) {
// temp[k] = bufptr[k];
// }
// temp[rfm12_rx_len()] = '\0';
// rfm12_rx_clear();
// put_string("\r\nRECEIVED: ");
// put_string(temp);
// put_string("\r\n\r\n");
////////////////check if acknowledgemnt valid////////////////
int check_ack = decode_frame(&ack, temp);
if((check_ack & (1<<1)) && !(check_ack & 1<<2)) {
//if(strcmp(ack.data, data[i].data)) {
put_string("\r\nSend Complete!\r\n");
send_complete = 1;
break;
//}
}
}
}
if(!send_complete) {
put_string("\r\ntimeout\r\n");
}
}
}
else {
put_frame(data[i]);
put_string("\n\rBROADCAST frame now : ");
SendFrame(data[i].frame);
_delay_ms(400);
}
}
/*
for frame in frames:
send_complete = 0;
while not send complete:
send frame
start timer
while timer:
if acknowledgement:
send_complete = 1;
*/
return 0;
}
int RecievePacket(char* Rpacket) {
/*
see http://www.hansinator.de/rfm12lib/ for rfm12b libray details
receive frame, send acknowledgement
if received a frame:
de-bytestuff
check crc
if recipient:
acknowledge
pass to network
*/
int i = 0;
put_string("\r\n>>>Begin receive packet: \r\n");
if (ReceiveFrame()) { //Status complete 1 - passes this section
put_char('.');
uint8_t* bufptr;
char Rframe[50], ackstr[50];
struct frame ack, decode;
struct frame Nrframe[FRAMECOUNT];
struct frame ackarr[FRAMECOUNT];
int Received_Final_frame = 0;
unsigned long timeout = millis() + 20000;
while(!Received_Final_frame && (millis() < timeout)){ //never passes this while statement
//Also maybe add RFM12B tick??
//int Rframe_len;
put_string("Trying to receive data");
if (ReceiveFrame()) { //Status complete 2 - why?
ReceiveFrame(Rframe);
// bufptr = rfm12_rx_buffer();
// put_string("Raw data received: ");
// put_string((char*)bufptr);
// for(uint8_t k = 0; k < (rfm12_rx_len()); k++) {
// Rframe[k] = bufptr[k];
// }
// Rframe[rfm12_rx_len()] = '\0';
// rfm12_rx_clear();
put_string("\r\nRframe: ");
put_string(Rframe);
//strcpy(ackstr, Rframe);
decode = Nrframe[i];
int Rframe_check = decode_frame(&decode, Rframe);
Nrframe[i] = decode;
put_string("\r\nReceived Data: ");
put_frame(Nrframe[i]);
if(Rframe_check & (1<<1)) {
if(Rframe_check & 1<<4) {
Received_Final_frame = 1;
}
put_string("\r\nFrame Number; ");
put_number(i);
/*
frame received, frame for me
acknowledge
*/
if(!(Rframe_check & 1<<5)) {
put_string("\n\r>>>>START MAKE AND SEND ACKNOWLEDGEMENT\r\n");
ackarr[0] = Nrframe[i];
int framecount = makeframe(&ackarr, Nrframe[i].address[0], Nrframe[i].data, 1, 1);
put_string("\r\nNumber of acknowledgement frames generated: ");
put_number(framecount);
put_string("\r\nacknowledgement: ");
put_frame(ackarr[0]);
//_delay_ms(100);
SendFrame(ackarr[0].frame);
// rfm12_tx(strlen(ackarr[0].frame), 0, (uint8_t*)ackarr[0].frame);
// for (uint8_t j = 0; j < 100; j++)
// {
// //put_string(". ");
// rfm12_tick();
// _delay_us(500);
// }
}
i++;
timeout = millis() + 20000;
}
else if(!Rframe_check) {
;
}
else {
break;
}
}
}
if(i && i < FRAMECOUNT) {
put_string("\r\nPacketComplete\n\r");
strcpy(Rpacket, Nrframe[0].data);
for(int l = 1; l<i; l++) {
strcat(Rpacket, Nrframe[l].data);
}
strcat(Rpacket, "\0");
put_string("\r\nReceived packet: ");
put_string(Rpacket);
}
}
put_string("\r\n>>>End ReceivePacket\r\n");
return i;
}
//1 Update Values
void Robot::BasicChangeValues(){
//BATTERY
SendFrame(1,0, 1,0);
bl = ReceiveFrame(1);
if(bl==NULL) return;
this->battery->SetVoltage((double)bl[3]); //setvoltage
this->battery->SetCurrent((double)bl[2]); //setcurrent
std::cout<<"I answered!\n";
socket->flush();
//CYLINDER CURRENT
SendFrame(1,0,2,0);
bl = ReceiveFrame(1);
if(bl==NULL) return;
this->m_cylinder.SetCurrent((int)bl[2]);
socket->flush();
//TENSOMETERS-MASS //wartosc x1, wartosc x2???
SendFrame(1,0,3,0);
bl= ReceiveFrame(1);
if(bl==NULL) return;
this->m_cylinder.SetWeight((int)(bl[2]));
socket->flush();
//TENSOMETERS-VALUES
SendFrame(1,0,4,0);
bl= ReceiveFrame(1);
if(bl==NULL) return;
this->SetTensometer((int)bl[2]);
socket->flush();
//ARM POSITION
SendFrame(1,0,5,0);
//
bl = ReceiveFrame(1);
if(bl==NULL) return;
this->m_arm.SetPosition(static_cast<POSITION>(bl[1]));
socket->flush();
//ENGINE nr+dir+val
for(int i=0; i<2; ++i){
///1 4 bity - silnik
SendFrame(1,0,6,i);
bl = ReceiveFrame(1);
if(bl[0]==0) {std::cout<<":("; return;}
int dir=bl[1];
//1-cylinder, 2-arm, 3,4-wheels
switch(dir){
case 20:{
this->m_cylinder.SetEngineSpeed((double)bl[2]);
this->m_cylinder.SetEngineDirection(1);
break;}
case 24:{
this->m_cylinder.SetEngineSpeed((double)bl[2]);
this->m_cylinder.SetEngineDirection(0);
break;}
case 36:{
this->m_arm.SetEngineSpeed((double)bl[2]);
this->m_arm.SetEngineDirection(0);
break;}
case 40:{
this->m_arm.SetEngineSpeed((double)bl[2]);
this->m_arm.SetEngineDirection(1);
break;}
case 52:{
this->m_wheel[0].SetEngineSpeed((double)bl[2]);
this->m_wheel[0].SetEngineDirection(0);
break;}
case 56:{
this->m_wheel[0].SetEngineSpeed((double)bl[2]);
this->m_wheel[0].SetEngineDirection(1);
break;}
case 68:{
this->m_wheel[1].SetEngineSpeed((double)bl[2]);
this->m_wheel[1].SetEngineDirection(0);
break;}
case 72:{
this->m_wheel[1].SetEngineSpeed((double)bl[2]);
this->m_wheel[1].SetEngineDirection(1);
break;}
}
}
// Wysterowany 1bit- lewy silnik w lewo, 3bit- lewy w prawo, 5bit- prawy w lewo, 7bit- prawy w lewo
socket->flush();
//std::cout<<"Agreed\n";
//CYLINDER dir+val
SendFrame(1,0,7,0);
bl = ReceiveFrame(1);
if(bl==NULL) return;
if(bl[2]==20){this->m_cylinder.SetEngineSpeed((int)bl[3]); this->m_cylinder.SetEngineDirection(0);}
if(bl[2]==24){this->m_cylinder.SetEngineSpeed((int)bl[3]); this->m_cylinder.SetEngineDirection(1);}
socket->flush();
//ENGINE IMPULSES
for(int i=0; i<4; ++i){
SendFrame(1,0,8,i);
bl = ReceiveFrame(1);
if(bl==NULL) return;
///impulsy??
socket->flush();
}
//ELECTROMAGNET
SendFrame(1,0,9,0);
bl = ReceiveFrame(1);
bool val = true;
if(val==1)
this->m_cylinder.SetElectromagnet(false);
else if (val==2)
this->m_cylinder.SetElectromagnet(true);
else return;
socket->flush();
//CYLINDER OPENED
SendFrame(1,0,10,0);
bl = ReceiveFrame(1);
int vl = bl[1];
if(vl==1) this->m_cylinder.Open(false);
if (vl==2) this->m_cylinder.Open(true);
//else return;
socket->flush();
emit _BasicChangeValues();
}
