/* Monitor Sensor Two Task
* This task handles the monitoring of sensor two, the start sensor
* and sends a message to the inpiut robot when the conveyor is
* occupied or free to deposit.
*/
static void appTaskMonitorSens2(void *pdata) {
/*
* Now execute the main task loop for this task
*/
while (true) {
if (conveyorItemPresent(CONVEYOR_SENSOR_2)){
canSend(CONVEYOR_OCCUPIED);
}
if (!conveyorItemPresent(CONVEYOR_SENSOR_2)){
canSend(CONVEYOR_FREE);
}
if (conveyorItemPresent(CONVEYOR_SENSOR_2) && checkForInputBlock) {
OSTimeDlyHMSM(0,0,2,0);
if (conveyorItemPresent(CONVEYOR_SENSOR_2)){
checkForInputBlock = false;
stopConveyorForLoad = false;
noBlocks += 1;
if (!conveyorItemPresent(CONVEYOR_SENSOR_1)){
conveyorSetState(CONVEYOR_REVERSE);
}
}
}
if (stopConveyorForLoad){
conveyorSetState(CONVEYOR_OFF);
}
OSTimeDly(20);
}
}
/*
* appTaskMoveBlock - Task for moving the block from the conveyor to the pad.
* The task move block is used for the process of moving the block
* from the conveyor to the pad. The task works by waiting for a block to be on
* the conveyor end sensor and when it is true it runs.
*
* The task picks up the block and moves it using the moveJoint function to the required
* position. The paused functionality works by pausing the system after the move is complete
* as indicated in the system specification.
*
* The pickUpAttempts function checks for a block failing to be picked up. If this
* fails twice then the full system will emergency stop.
*/
static void appTaskMoveBlock(void *pdata) {
while (true) {
while(moveBlock) {
if (pickupAttempts == 2) {
canSend(EMERGENCY_STOP);
emergencyStop=true;
}
canSend(QUERY_PAD2_STATUS); //Send a message asking if the PAD is clear.
while(!padClear)
{
OSTimeDlyHMSM(0,0,0,10);
}
padClear = false;
//Waist to Neutral
moveJoint(ROBOT_WAIST, 67250);
//Elbow to Neutral
moveJoint(ROBOT_ELBOW, 71750);
//Wrist to Neutral
moveJoint(ROBOT_WRIST, 89000);
//HAND to Neutral
moveJoint(ROBOT_HAND, 45000);
//Waist to Convey
moveJoint(ROBOT_WAIST, 47500);
pickupAttempts++;
//Wrist to Convey
moveJoint(ROBOT_WRIST, 64500);
//Elbow to Convey
moveJoint(ROBOT_ELBOW, 94500);
//HAND to Convey
moveJoint(ROBOT_HAND, 80000);
//Elbow to Neutral
moveJoint(ROBOT_ELBOW, 72500);
//Waist to Pad
moveJoint(ROBOT_WAIST, 84250);
//Wrist to Pad
moveJoint(ROBOT_WRIST, 95000);
//Elbow to Pad
moveJoint(ROBOT_ELBOW, 81250);
//HAND to Pad
moveJoint(ROBOT_HAND, 45000);
//Elbow to Neutral
moveJoint(ROBOT_ELBOW, 71750);
//Wrist to Neutral
moveJoint(ROBOT_WRIST, 89000);
//HAND to Neutral
moveJoint(ROBOT_HAND, 45000);
//Waist to Neutral
moveJoint(ROBOT_WAIST, 67250);
moveBlock = false;
canSend(OUTPUT_ROBOT_FINISHED);
}
while(paused)
{
OSTimeDlyHMSM(0,0,0,500);
}
OSTimeDly(20);
}
}
/*!
**
** @param d
** @param cob_id
**
** @return
**/
UNS8 sendEMCY(CO_Data* d, UNS16 errCode, UNS8 errRegister, const void *Specific, UNS8 SpecificLength)
{
Message m;
MSG_WAR(0x3051, "sendEMCY", 0);
m.cob_id = (UNS16)(*(UNS32*)d->error_cobid);
m.rtr = NOT_A_REQUEST;
m.Data[0] = errCode & 0xFF; /* LSB */
m.Data[1] = (errCode >> 8) & 0xFF; /* MSB */
m.Data[2] = errRegister;
if(Specific==NULL)
{
m.Data[3] = 0; /* Manufacturer specific Error Field omitted */
m.Data[4] = 0;
m.Data[5] = 0;
m.Data[6] = 0;
m.Data[7] = 0;
SpecificLength = 5;
}
else
{
if(SpecificLength>5) SpecificLength = 5;
memcpy(&m.Data[3],Specific,SpecificLength);
}
m.len = SpecificLength + 3;
return canSend(d->canHandle,&m);
}
void MsgEdit::textChanged()
{
#ifdef USE_SPELL
btnSpell->setEnabled(canSpell());
#endif
btnSend->setEnabled(canSend());
}
/******************************************************************************
* version: 1.0
* author: link
* date: 2016.02.26
* brief: 设置电阻硬件参数
******************************************************************************/
void widget_Res::setHardware()
{
int i;
can_frame frame;
for (i=0; i<(setting.size()-OTHER)/RES_NUM; i++){
if (setting[OTHER+i*RES_NUM+0].toInt() == 1){
frame.can_id = 0x02;
frame.can_dlc = 0x07;
frame.data[0] = 0x04;
frame.data[1] = i;
frame.data[2] = setting[OTHER+i*RES_NUM+2].toInt();
frame.data[3] = setting[OTHER+i*RES_NUM+2].toInt();
if (setting[OTHER+i*RES_NUM+4].toInt()<2)
frame.data[4] = 1;
else if (setting[OTHER+i*RES_NUM+4].toInt()<20)
frame.data[4] = 2;
else if (setting[OTHER+i*RES_NUM+4].toInt()<200)
frame.data[4] = 3;
else if (setting[OTHER+i*RES_NUM+4].toInt()<2000)
frame.data[4] = 4;
else
frame.data[4] = 5;
frame.data[5] = 0x0f;
frame.data[6] = 0x5a;
canSend(frame);
}
}
}
//-----------------------------------------------
bool CanESD::transmitMsg(CanMsg &CMsg)
{
CMSG NTCANMsg;
NTCANMsg.id = CMsg.m_iID;
NTCANMsg.len = CMsg.m_iLen;
for(int i=0; i<8; i++)
NTCANMsg.data[i] = CMsg.getAt(i);
NTCAN_RESULT ret;
int32_t len;
bool bRet = true;
len = 1;
//ret = canWrite(m_Handle, canmsg, &len, NULL);
ret = canSend(m_Handle, &NTCANMsg, &len);
if( ret != NTCAN_SUCCESS)
{
LOGERROR("canSend() failed: " << getErrorMessage(ret));
bRet = false;
}
m_LastID = (int)NTCANMsg.data[0];
//ret auswerten
return bRet;
}
void RFM69::send(uint8_t toAddress, const void* buffer, uint8_t bufferSize, bool requestACK)
{
writeReg(REG_PACKETCONFIG2, (readReg(REG_PACKETCONFIG2) & 0xFB) | RF_PACKET2_RXRESTART); // avoid RX deadlocks
uint32_t now = millis();
while (!canSend() && millis() - now < RF69_CSMA_LIMIT_MS) receiveDone();
sendFrame(toAddress, buffer, bufferSize, requestACK, false);
}
/*!
**
**
** @param d
** @param cob_id
**
** @return
**/
UNS8 sendSlaveLSSMessage(CO_Data* d, UNS8 command,void *dat1,void *dat2)
{
Message m;
UNS8 i;
if (!d->CurrentCommunicationState.csLSS)
{
MSG_WAR(0x2D17, "unable to send the LSS message, not in the proper state =>", d->nodeState);
return 0xFF;
}
for (i = 1; i < 8; i++)
{
m.data[i] = 0;
}
m.len = 8;
m.rtr = NOT_A_REQUEST;
m.data[0]=command;
m.cob_id=UNS16_LE(SLSS_ADRESS);
/* Tha data sent with the msg depends on the command */
switch(command)
{
case LSS_INQ_NODE_ID: /* Inquire Node-ID */
{
m.data[1]=*(UNS8 *)dat1;
} break;
case LSS_CONF_NODE_ID: /* Configure Node-ID */
case LSS_CONF_BIT_TIMING: /* Configure Bit Timing Parameters */
case LSS_CONF_STORE: /* Store Configured Parameters */
{
m.data[1]=*(UNS8 *)dat1;
m.data[2]=*(UNS8 *)dat2;
} break;
case LSS_INQ_VENDOR_ID: /* Inquire Identity Vendor-ID */
case LSS_INQ_PRODUCT_CODE: /* Inquire Identity Product-Code */
case LSS_INQ_REV_NUMBER: /* Inquire Identity Revision-Number */
case LSS_INQ_SERIAL_NUMBER: /* Inquire Identity Serial-Number */
{
m.data[1]=(UNS8)(*(UNS32*)dat1 & 0xFF);
m.data[2]=(UNS8)(*(UNS32*)dat1>>8 & 0xFF);
m.data[3]=(UNS8)(*(UNS32*)dat1>>16 & 0xFF);
m.data[4]=(UNS8)(*(UNS32*)dat1>>24 & 0xFF);
} break;
case LSS_SM_SELECTIVE_RESP: /* Switch Mode Selective response*/
case LSS_IDENT_SLAVE: /* LSS Identify Slave */
case LSS_IDENT_NON_CONF_SLAVE: /* LSS identify non-configured remote slave */
{
} break;
default:
{
MSG_ERR(0x1D18, "send Slave LSS command not implemented", command);
return 0xFF;
}
}
return canSend(d->canHandle,&m);
}
void Mailbox::__addThing(int32_t index, Thing* thing)
{
if(Item* item = thing->getItem())
{
if(canSend(item))
sendItem(item);
}
}
// should be called immediately after reception in case sender wants ACK
void RFM69::sendACK(const void* buffer, uint8_t bufferSize) {
uint8_t sender = SENDERID;
int16_t _RSSI = RSSI; // save payload received RSSI value
writeReg(REG_PACKETCONFIG2, (readReg(REG_PACKETCONFIG2) & 0xFB) | RF_PACKET2_RXRESTART); // avoid RX deadlocks
uint32_t now = millis();
while (!canSend() && millis() - now < RF69_CSMA_LIMIT_MS) receiveDone();
sendFrame(sender, buffer, bufferSize, false, true);
RSSI = _RSSI; // restore payload RSSI
}
void Mailbox::__addThing(Creature* actor, int32_t, Thing* thing)
{
Item* item = thing->getItem();
if(!item)
return;
if(canSend(item))
sendItem(actor, item);
}
ReturnValue Mailbox::__queryAdd(int32_t index, const Thing* thing, uint32_t count,
uint32_t flags) const
{
if(const Item* item = thing->getItem())
{
if(canSend(item))
return RET_NOERROR;
}
return RET_NOTPOSSIBLE;
}
// should be called immediately after reception in case sender wants ACK
void RFM69::sendACK(const void* buffer, uint8_t bufferSize) {
ACK_REQUESTED = 0; // TWS added to make sure we don't end up in a timing race and infinite loop sending Acks
uint8_t sender = SENDERID;
int16_t _RSSI = RSSI; // save payload received RSSI value
writeReg(REG_PACKETCONFIG2, (readReg(REG_PACKETCONFIG2) & 0xFB) | RF_PACKET2_RXRESTART); // avoid RX deadlocks
uint32_t now = millis();
while (!canSend() && millis() - now < RF69_CSMA_LIMIT_MS) { delayMicroseconds(MICROSLEEP_LENGTH); /* printf(".");*/ receiveDone();}
sendFrame(sender, buffer, bufferSize, false, true);
RSSI = _RSSI; // restore payload RSSI
}
/* Monitor Sensor One Task
* This task handles the monitoring of sensor one, the end sensor
* and sends a message to the output robot when a block is ready
* to be picked up.
*/
static void appTaskMonitorSens1(void *pdata) {
while (true) {
if (conveyorItemPresent(CONVEYOR_SENSOR_1)) {
conveyorSetState(CONVEYOR_OFF);
canSend(READY_TO_PICKUP_CONVEYOR);
while(conveyorItemPresent(CONVEYOR_SENSOR_1)){
canSend(READY_TO_PICKUP_CONVEYOR);
OSTimeDly(20);
}
canSend(PICKED_UP_CONVEYOR);
noBlocks -= 1;
}
if (!conveyorItemPresent(CONVEYOR_SENSOR_1) && (noBlocks > 0) && !stopConveyorForLoad){
conveyorSetState(CONVEYOR_REVERSE);
}
OSTimeDly(20);
}
}
static inline void canSendGetAddr(void) {
uint8_t d[8];
*((uint32_t *)&d[0]) = canData.uuid;
d[4] = CAN_TYPE_ESC;
d[5] = escId;
canSend(CAN_LCC_NORMAL | CAN_TT_NODE | CAN_FID_REQ_ADDR, 0, canGetSeqId(), 6, d);
}
const unsigned int UdpSock::sendTo(const Address & targetAddress, const void * sourceBuffer, const unsigned int length) const
{
unsigned int bytesSent = 0;
if(canSend())
{
socklen_t toLen = sizeof(struct sockaddr_in);
bytesSent = ::sendto(handle, sourceBuffer, static_cast<const int>(length), 0, reinterpret_cast<const struct sockaddr *>(&(targetAddress.getSockAddr4())), toLen); //lint !e732 // MS wants an int, should be unsigned IMO
}
return bytesSent;
}
ReturnValue Mailbox::__queryAdd(int32_t index, const Thing* thing, uint32_t count,
uint32_t flags, Creature* actor/* = nullptr*/) const
{
if (const Item* item = thing->getItem()) {
if (canSend(item)) {
return RET_NOERROR;
}
}
return RET_NOTPOSSIBLE;
}
void UART::txrxInterruptHandler() {
if (rxIntEnabled && canRecv()) {
assert_param(recvListener);
recvListener(recvListenerObj);
}
if (txIntEnabled && canSend()) {
assert_param(sendListener);
sendListener(sendListenerObj);
}
}
//=============================================================================
// sendAck() - updated to call new sendFrame with additional parameters
//=============================================================================
// should be called immediately after reception in case sender wants ACK
void RFM69_ATC::sendACK(const void* buffer, uint8_t bufferSize) {
ACK_REQUESTED = 0; // TomWS1 added to make sure we don't end up in a timing race and infinite loop sending Acks
uint8_t sender = SENDERID;
int16_t _RSSI = RSSI; // save payload received RSSI value
bool sendRSSI = ACK_RSSI_REQUESTED;
writeReg(REG_PACKETCONFIG2, (readReg(REG_PACKETCONFIG2) & 0xFB) | RF_PACKET2_RXRESTART); // avoid RX deadlocks
uint32_t now = millis();
while (!canSend() && millis() - now < RF69_CSMA_LIMIT_MS) receiveDone();
SENDERID = sender; // TomWS1: Restore SenderID after it gets wiped out by receiveDone()
sendFrame(sender, buffer, bufferSize, false, true, sendRSSI, _RSSI); // TomWS1: Special override on sendFrame with extra params
RSSI = _RSSI; // restore payload RSSI
}
/*!
**
**
** @param d
** @param cob_id
**
** @return
**/
UNS8 sendSYNCMessage(CO_Data* d)
{
Message m;
MSG_WAR(0x3001, "sendSYNC ", 0);
m.cob_id = (UNS16)UNS16_LE(*d->COB_ID_Sync);
m.rtr = NOT_A_REQUEST;
m.len = 0;
return canSend(d->canHandle,&m);
}
void MsgEdit::messageReceived(ICQMessage *m)
{
if (m->getUin() != Uin()) return;
setupNext();
if (msg && msg->Received && (static_cast<UserBox*>(topLevelWidget())->currentUser() == Uin)) return;
if (canSend()) return;
ICQMessage *msg_copy = history()->getMessage(m->Id);
setMessage(msg_copy, false, true);
if (qApp->activeWindow() != topLevelWidget()) return;
if (static_cast<UserBox*>(topLevelWidget())->currentUser() != Uin) return;
pClient->markAsRead(m);
setupNext();
}
bool LiveJournalClient::send(Message *msg, void *_data)
{
if (!canSend(msg->type(), _data))
return false;
LiveJournalUserData *data = (LiveJournalUserData*)_data;
const char *journal = NULL;
if (data->User.ptr && strcmp(data->User.ptr, this->data.owner.User.ptr))
journal = data->User.ptr;
m_requests.push_back(new MessageRequest(this, static_cast<JournalMessage*>(msg), journal));
msg->setClient(dataName(_data).c_str());
send();
return true;
}
/*!
**
**
** @param d
** @param nodeId
**
** @return
**/
UNS8 masterSendNMTnodeguard(CO_Data* d, UNS8 nodeId)
{
Message m;
/* message configuration */
UNS16 tmp = nodeId | (NODE_GUARD << 7);
m.cob_id = UNS16_LE(tmp);
m.rtr = REQUEST;
m.len = 0;
MSG_WAR(0x3503, "Send_NODE_GUARD to node : ", nodeId);
return canSend(d->canHandle,&m);
}
/*!
**
**
** @param d
** @param Node_ID
** @param cs
**
** @return
**/
UNS8 masterSendNMTstateChange(CO_Data* d, UNS8 Node_ID, UNS8 cs)
{
Message m;
MSG_WAR(0x3501, "Send_NMT cs : ", cs);
MSG_WAR(0x3502, " to node : ", Node_ID);
/* message configuration */
m.cob_id = 0x0000; /*(NMT) << 7*/
m.rtr = NOT_A_REQUEST;
m.len = 2;
m.data[0] = cs;
m.data[1] = Node_ID;
return canSend(d->canHandle,&m);
}
int main(int argc, char *argv[])
{
QApplication a(argc, argv);
QTextCodec::setCodecForTr(QTextCodec::codecForName("UTF-8"));
MainWindow* masterwindow=new MainWindow;
masterwindow->show();
masterwindow->setWindowTitle("Server");
//add
Server server;
//开始监听
server.listen((quint16)8989);
QObject::connect(masterwindow->scene,SIGNAL(canSend(QList<records>)),&server,SLOT(sendClientPoint(QList<records>)));
//add end
return a.exec();
}
size_t AsyncClient::write(const char* data, size_t size) {
if(!_pcb || size == 0 || data == NULL)
return 0;
if(!canSend())
return 0;
size_t room = tcp_sndbuf(_pcb);
size_t will_send = (room < size) ? room : size;
int8_t err = tcp_write(_pcb, data, will_send, 0);
if(err != ERR_OK)
return 0;
tcp_output(_pcb);
_pcb_sent_at = millis();
_pcb_busy = true;
if(will_send < size){
size_t left = size - will_send;
return will_send + write(data+will_send, left);
}
return size;
}
/******************************************************************************
* version: 1.0
* author: link
* date: 2016.02.26
* brief: 启动电阻测试
******************************************************************************/
void widget_Res::startTest()
{
int i;
int temp=0;
can_frame frame;
for (i=0; i<(setting.size()-OTHER)/RES_NUM; i++)
if (setting[OTHER+i*RES_NUM+0].toInt() == 1)
temp += 1<<i;
frame.can_id = 0x02;
frame.can_dlc = 0x05;
frame.data[0] = 0x01;
frame.data[1] = 0x3f;
frame.data[2] = temp/256;
frame.data[3] = temp%256;
frame.data[4] = 0x00;
canSend(frame);
}
/**
* Transmit a message via the CAN bus.
* Additionally, an error flag is set internally if the transmission does not succeed.
* @param CMsg Structure containing the CAN message.
* @return true on success, false on failure.
*/
bool CanESD::transmitMsg(CanMsg CMsg, bool bBlocking)
{
CMSG NTCANMsg;
NTCANMsg.id = CMsg.m_iID;
NTCANMsg.len = CMsg.m_iLen;
for(int i=0; i<8; i++)
{
NTCANMsg.data[i] = CMsg.getAt(i);
//std::cout << "Msg: "std::cout << CMsg.getAt(i) << std::endl;
}
int ret;
//long len;
int32_t len;
bool bRet = true;
len = 1;
//m_Mutex.lock();
if (bBlocking)
ret = canWrite(m_Handle, &NTCANMsg, &len,+ NULL);
else
ret = canSend(m_Handle, &NTCANMsg, &len);
//m_Mutex.unlock();
if( ret != NTCAN_SUCCESS)
{
std::cout << "error in CANESD::transmitMsg: " << GetErrorStr(ret) << std::endl;
bRet = false;
}
m_LastID = (int)NTCANMsg.data[0];
//readEvent();
//ret auswerten
m_bIsTXError = !bRet;
return bRet;
}
/// Should be called immediately after reception in case sender wants ACK
void RFM69::sendACK(const void* buffer, byte bufferSize) {
byte sender = SENDERID;
while (!canSend()) receiveDone();
sendFrame(sender, buffer, bufferSize, false, true);
}
/******************************************************************************
* version: 1.0
* author: link
* date: 2016.01.28
* brief: 更新数据
******************************************************************************/
void w_Conf::updateData()
{
int i;
for (i=0; i<projExist.size(); i++){
switch(projExist[i].toInt()){
case W_ALL:
if (wAll == NULL){
wAll = new widget_All(this);
tabWidget->addTab(wAll,tr("首页"));
connect(wAll,SIGNAL(switchWindow(int)),this,SLOT(winTransmit(int)));
connect(wAll,SIGNAL(switchFile(int)),this,SLOT(fileTransmit(int)));
}
wAll->fileExist = fileExist;
wAll->projExist = projExist;
wAll->updateShow(CONF);
wAll->setHardware();
break;
case W_RES:
if (wRes == NULL){
wRes = new widget_Res(this);
tabWidget->addTab(wRes,tr("电阻"));
connect(wRes,SIGNAL(canSend(can_frame)),this,SLOT(canTransmit(can_frame)));
}
wRes->updateShow(RES);
wRes->setHardware();
break;
case W_OPP:
if (wOpp == NULL){
wOpp = new widget_Opp(this);
tabWidget->addTab(wOpp,tr("反嵌"));
connect(wOpp,SIGNAL(canSend(can_frame)),this,SLOT(canTransmit(can_frame)));
}
wOpp->updateShow(OPP);
wOpp->setHardware();
break;
case W_INS:
if (wIns == NULL){
wIns = new widget_Ins(this);
tabWidget->addTab(wIns,tr("绝缘"));
}
wIns->updateShow(INS);
wIns->setHardware();
break;
case W_ACV:
if (wAcv == NULL){
wAcv = new widget_Acv(this);
tabWidget->addTab(wAcv,tr("交耐"));
}
wAcv->updateShow(ACV);
wAcv->setHardware();
break;
case W_DCV:
if (wDcv == NULL){
wDcv = new widget_Dcv(this);
tabWidget->addTab(wDcv,tr("直耐"));
}
wDcv->updateShow(DCV);
wDcv->setHardware();
break;
case W_ITT:
if (wItt == NULL){
wItt = new widget_Itt(this);
tabWidget->addTab(wItt,tr("匝间"));
connect(wItt,SIGNAL(canSend(can_frame)),this,SLOT(canTransmit(can_frame)));
}
wItt->updateShow(ITT);
wItt->setHardware();
break;
default:
break;
}
}
