/* 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);
}
}
