/* SendThreadFun function is called by send-thread, it is used to * * send messages to the server. * * if it sends a temperature message, it will set haverecvACK = 0 * * (to tell recv thread that he should recv an ACK from server * * saying "got it"). it will always send the temperature message * * until recv-thread receive an ACK message */ void SendThreadFun(void *ptr) { printf("start send thread...\n"); char frame[MAX_FRAME_SIZE]; char ackframe[7] = "*00ACK/"; show_state(); while(stop != 1) { //if sendACK == 1, it means that system has receive a control message, so you should send an ACK, then set sendACK back to 0// //if(needsendACK == 1) //{ //printf("\nsending ACK frame....\n"); //SendFrame(fd, ackframe, 7); //needsendACK = 0; //show_state(); //} //get a temp and send // GetFrame(frame); printf("\nsending %s, fre: %d s\n", frame, fre); SendFrame(fd, frame, MAX_FRAME_SIZE); /*reset haverecvACK to 0, make sure that new frame with new ACK*/ pthread_mutex_lock(&have_recv_ack_mutex); haverecvACK = 0; pthread_mutex_unlock(&have_recv_ack_mutex); show_state(); sleep(fre); // usleep(500000); while(haverecvACK != 1 ) //ACK { //if(needsendACK == 1) //{ // printf("\nsending ACK frame....\n"); // SendFrame(fd, ackframe, 7); // needsendACK = 0; // show_state(); //} if(stop == 1) break; // stop printf("no recv ACK, will send again: %s, fre: %d\n", frame, fre); SendFrame(fd, frame, MAX_FRAME_SIZE); show_state(); sleep(fre); //usleep(500000); } } }
//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])); }
void ADSamplTask(void *p_arg) { int16_t buff[4]; uint8_t i; while(TRUE_P) { __memset(buff, 0, 8); for (i = 0; i < 4; i++) { if (SysAppCfg.ad_chnnl_use[i]) { buff[i] = ADSampl(i); //OS_Trace("AD Value = %d\n",buff[i]); } } SendFrame(FRM_CMD_AD_V, (uint8_t *)buff, 8); //8即8个字节 OSTimeDlyHMSM(0, 0, 0, 300); } }
// 按IP地址查询尾包时刻帧 void MakeInstrTailTimeQueryFramebyIP(m_oTailTimeFrameStruct* pTailTimeFrame, m_oConstVarStruct* pConstVar, unsigned int uiInstrumentIP, m_oLogOutPutStruct* pLogOutPut) { ASSERT(pLogOutPut != NULL); ASSERT(pTailTimeFrame != NULL); ASSERT(pConstVar != NULL); unsigned short usPos = 0; EnterCriticalSection(&pTailTimeFrame->m_oSecTailTimeFrame); // 仪器IP地址 pTailTimeFrame->m_pCommandStructSet->m_uiDstIP = uiInstrumentIP; // 查询命令 pTailTimeFrame->m_pCommandStructSet->m_usCommand = pConstVar->m_usSendQueryCmd; // 查询命令字内容 // 交叉站交叉线尾包接收时刻 pTailTimeFrame->m_pbyCommandWord[usPos] = pConstVar->m_byCmdLAUTailRecTimeLAUX; usPos ++; // 交叉站大线尾包接收时刻 pTailTimeFrame->m_pbyCommandWord[usPos] = pConstVar->m_byCmdLineTailRecTimeLAUX; usPos ++; // 尾包接收/发送时刻 pTailTimeFrame->m_pbyCommandWord[usPos] = pConstVar->m_byCmdTailRecSndTime1; usPos ++; // 查询命令字个数 pTailTimeFrame->m_usCommandWordNum = usPos; MakeInstrFrame(pTailTimeFrame->m_pCommandStructSet, pConstVar, pTailTimeFrame->m_cpSndFrameData, pTailTimeFrame->m_pbyCommandWord, pTailTimeFrame->m_usCommandWordNum); SendFrame(pTailTimeFrame->m_oTailTimeFrameSocket, pTailTimeFrame->m_cpSndFrameData, pConstVar->m_iSndFrameSize, pTailTimeFrame->m_pCommandStructSet->m_usAimPort, pTailTimeFrame->m_pCommandStructSet->m_uiAimIP, pLogOutPut); LeaveCriticalSection(&pTailTimeFrame->m_oSecTailTimeFrame); }
//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::SendFrameRepeated(CInterfaceManagerBase* pInterfaceManager,HANDLE hI_Handle,HANDLE hTransactionHandle,DWORD dPackageSize,WORD wChunkSize,BYTE uLastChunkFlag,DWORD* pdChecksum,void* pDataBuffer,DWORD dDataBufferLength,CErrorInfo* pErrorInfo) { const DWORD k_MaxSendingCount = 3; CErrorInfo errorInfo; DWORD dSendingCount = 0; while(dSendingCount < k_MaxSendingCount) { dSendingCount++; if(SendFrame(pInterfaceManager,hI_Handle,hTransactionHandle,dPackageSize,wChunkSize,uLastChunkFlag,pdChecksum,pDataBuffer,dDataBufferLength,&errorInfo)) { return TRUE; } else { if(errorInfo.GetErrorCode() != k_Error_InfoteamSerial_RepAckReceived) { if(pErrorInfo) *pErrorInfo = errorInfo; return FALSE; } } } if(pErrorInfo) *pErrorInfo = errorInfo; return FALSE; }
//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(); }
// Called when thread is started void* CaptureThread::Entry() { while (true) { // check to see if the thread should exit if (TestDestroy() == true) { break; } if (capturing == CAPTURE) { // get a new image CaptureFrame(); } else if (capturing == PREVIEW) { // get a new image and show it on screen CaptureFrame(); SendFrame(imageQueue.back()); } else if (capturing == IDLE) { Sleep(10); } else if (capturing == STOP) { break; } Yield(); } return NULL; }
//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)); }
u8 Icar_SendRead(u8 type,u8* Strp) { u8 buffer[16],CANflag=1; u16 len; u8 *pFrm=buffer; u32 iClick=OSTime; u8 FRAMES[]="\x30\x00\x00\x00\x00\x00\x00\x00"; Icar_TranToSendBuf(type,Strp,buffer); len=buffer[0]; InitFrameBuffer(); SendFrame(type,buffer); pFrm+=1+len; while(1){ len=(u16)RecvFrame(type,buffer); if(len==0)break; if(((type==OBD_TYPE_CAN1) ||(type==OBD_TYPE_CAN2))&&len>13){AddFrameToBuffer(buffer,13);AddFrameToBuffer(buffer+13,13);} else AddFrameToBuffer(buffer,len); if(*pFrm!=(u8)F_MUT){ // if(OSTime-iClick>g_box_out_time)break; } if(((type==OBD_TYPE_CAN1) ||(type==OBD_TYPE_CAN2))&&((buffer[7]==0x10)||(buffer[7+13]==0x10))&&(CANflag)){ can_send( can_snd_id[obd_read_canid_idx], DAT_FRAME, 8, FRAMES);CANflag=0;} else break; } return obd_fbuff.frame_count; }
//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]); }
//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); }
//20 Start void Robot::StartAll(){ SendFrame(20,0,0,0); // //bl=ReceiveFrame(20); emit _StartAll(); // }
//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]); }
//4 0 Cylinder void Robot::BasicCylinderSetToZero(double w){ int val = (int)(w); SendFrame(4,0,w,0); bl=ReceiveFrame(4); emit _BasicCylinderSetToZero(w); }
//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]); }
void CTextEncoder::ProcessTextFrame (CMediaFrame *pFrame) { const char *fptr = (const char *)pFrame->GetData(); EncodeFrame(fptr); SendFrame(pFrame->GetTimestamp()); }
//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(); }
/**************************************************************************** * DecodeBlock: the whole thing **************************************************************************** * This function must be fed with complete frames. ****************************************************************************/ static void *DecodeBlock( decoder_t *p_dec, block_t **pp_block ) { decoder_sys_t *p_sys = p_dec->p_sys; block_t *p_block; void *p_buf; if( !pp_block || !*pp_block ) return NULL; p_block = *pp_block; if( p_block->i_pts <= VLC_TS_INVALID && p_block->i_dts <= VLC_TS_INVALID && !date_Get( &p_sys->pts ) ) { /* We've just started the stream, wait for the first PTS. */ block_Release( p_block ); return NULL; } /* Date management: If there is a pts avaliable, use that. */ if( p_block->i_pts > VLC_TS_INVALID ) { date_Set( &p_sys->pts, p_block->i_pts ); } else if( p_block->i_dts > VLC_TS_INVALID ) { /* NB, davidf doesn't quite agree with this in general, it is ok * for rawvideo since it is in order (ie pts=dts), however, it * may not be ok for an out-of-order codec, so don't copy this * without thinking */ date_Set( &p_sys->pts, p_block->i_dts ); } if( p_block->i_buffer < p_sys->i_raw_size ) { msg_Warn( p_dec, "invalid frame size (%zu < %zu)", p_block->i_buffer, p_sys->i_raw_size ); block_Release( p_block ); return NULL; } if( p_sys->b_packetizer ) { p_buf = SendFrame( p_dec, p_block ); } else { p_buf = DecodeFrame( p_dec, p_block ); } /* Date management: 1 frame per packet */ date_Increment( &p_sys->pts, 1 ); *pp_block = NULL; return p_buf; }
//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); }
ssize_t RawSocketComm::SendPacket(PacketBuffer* buffer, int flags, void* dst_addr) { memcpy(dest_address.sll_addr, dst_addr, ETH_ALEN); ethhdr* eth_header = (ethhdr*)buffer->eth_header; memcpy(eth_header->h_source, mac_addr, ETH_ALEN); memcpy(eth_header->h_dest, dst_addr, ETH_ALEN); eth_header->h_proto = htons(MVCTP_PROTO_TYPE); return SendFrame(buffer->eth_header, buffer->data_len + MVCTP_HLEN + ETH_HLEN); }
//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(); }
//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); }
void EC_ReadOCRResult(void) { // EC_ReadOCRStatus(); // if ( ec_state>EC_OK ) EC_Error(); uint8_t retry = 0; while ( (retry++)<20 ) { SendFrame(EC_FRAME_READ_OCR_RESULT); if ( ec_state==EC_OK) break; delay(10); } }