//------------------------------------------//*//------------------------------------------------
void SendCanMessage(uint32_t id, uint32_t data_l, uint32_t data_h)
{
Message Send_Message;
Send_Message.real_Identifier = id;
Send_Message.data_low_reg = data_l;
Send_Message.data_high_reg = data_h;
Send_Message.canID = 0;
CAN_Write(&Send_Message);
Send_Message.canID = 1;
CAN_Write(&Send_Message);
}
UNS8 canSend_driver(CAN_HANDLE fd0, Message const *m)
{
UNS8 data;
TPCANMsg peakMsg;
peakMsg.ID=m -> cob_id; /* 11/29 bit code */
if(m->rtr == 0)
peakMsg.MSGTYPE = CAN_INIT_TYPE_ST; /* bits of MSGTYPE_*/
else {
peakMsg.MSGTYPE = CAN_INIT_TYPE_ST_RTR; /* bits of MSGTYPE_*/
}
peakMsg.LEN = m->len;
/* count of data bytes (0..8) */
for(data = 0 ; data < m->len; data ++)
peakMsg.DATA[data] = m->data[data]; /* data bytes, up to 8 */
#if defined DEBUG_MSG_CONSOLE_ON
MSG("out : ");
print_message(m);
#endif
if((errno = CAN_Write(fd0, & peakMsg))) {
perror("canSend_driver (Peak_Linux) : error of writing.\n");
return 1;
}
return 0;
}
//-------------------------------------------
bool CANPeakSys2PCI::transmitMsg(CanMsg& CMsg)
{
TPCANMsg TPCMsg;
bool bRet = true;
if (m_bInitialized == false) return false;
// copy CMsg to TPCmsg
TPCMsg.LEN = CMsg.m_iLen;
TPCMsg.ID = CMsg.m_iID;
TPCMsg.MSGTYPE = CMsg.m_iType;
for(int i=0; i<8; i++)
TPCMsg.DATA[i] = CMsg.getAt(i);
// write msg
int iRet;
iRet = CAN_Write(m_handle, &TPCMsg);
iRet = CAN_Status(m_handle);
if(iRet < 0)
{
LOGERROR( "transmitMsg() : errorcode= " << nGetLastError() );
bRet = false;
}
return bRet;
}
//-------------------------------------------
bool CANPeakSysDongle::transmitMsg(CanMsg& CMsg)
{
bool bRet;
int i, iRet;
TPCANMsg TPCMsg;
if(!m_bInitialized) { return false; }
// copy CMsg to TPCmsg
TPCMsg.LEN = CMsg.m_iLen;
TPCMsg.ID = CMsg.m_iID;
TPCMsg.MSGTYPE = CMsg.m_iType;
for(i = 0; i < 8; i++)
{
TPCMsg.DATA[i] = CMsg.getAt(i);
}
// write msg
bRet = true;
// std::cout<<"sending command: "<<std::endl;
CAN_Write(m_handle, &TPCMsg);
iRet = CAN_Status(m_handle);
if(iRet < 0)
{
std::cout<<"transmitMsg() : errorcode= " << nGetLastError()<<std::endl;
bRet = false;
}
return bRet;
}
//-------------------------------------------
bool CanPeakSys::transmitMsg(CanMsg CMsg, bool bBlocking)
{
TPCANMsg TPCMsg;
bool bRet = true;
if (m_bInitialized == false) return false;
// copy CMsg to TPCmsg
TPCMsg.LEN = CMsg.m_iLen;
TPCMsg.ID = CMsg.m_iID;
TPCMsg.MSGTYPE = CMsg.m_iType;
for(int i=0; i<8; i++)
TPCMsg.DATA[i] = CMsg.getAt(i);
// write msg
int iRet;
iRet = CAN_Write(m_handle, &TPCMsg);
iRet = CAN_Status(m_handle);
if(iRet < 0)
{
std::cout << "CanPeakSys::transmitMsg, errorcode= " << nGetLastError() << std::endl;
bRet = false;
}
return bRet;
}
// loop writing to CAN-Bus
int write_loop(__u32 dwMaxTimeInterval, __u32 dwMaxLoop)
{
__u32 l, c=0;
// write out endless loop until Ctrl-C
double scale = (dwMaxTimeInterval * 1000.0) / (RAND_MAX + 1.0);
//while (1)
for (l=0; ; l++) {
std::list<TPCANMsg>::iterator iter;
int i;
if (dwMaxLoop)
if (l >= dwMaxLoop)
break;
for (iter = List->begin(); iter != List->end(); iter++) {
// test for standard frames only
if ((nExtended == CAN_INIT_TYPE_EX) ||
!(iter->MSGTYPE & MSGTYPE_EXTENDED)) {
// send the message
if (CAN_Write(h, &(*iter))) {
perror("transmitest: CAN_Write()");
return errno;
}
// wait some time before the invocation
if (dwMaxTimeInterval)
usleep((__useconds_t)(scale * rand()));
}
}
}
return 0;
}
//----------------------------------------------------------------------------
// real time task
void writing_task_proc(void *arg)
{
#ifdef RTAI
writing_task = rt_task_init_schmod(nam2num("WRTSK"),2, 0, 0,
SCHED_FIFO, 0xF);
rt_make_hard_real_time();
#endif
while (1) {
std::list<TPCANMsg>::iterator iter;
for (iter = List->begin(); iter != List->end(); iter++) {
// test for standard frames only
if ((nExtended == CAN_INIT_TYPE_EX) ||
!(iter->MSGTYPE & MSGTYPE_EXTENDED)) {
// send the message
if ((errno = CAN_Write(h, &(*iter))))
shutdownnow = 1;
}
if (shutdownnow == 1)
break;
}
if (shutdownnow == 1)
break;
}
#ifdef RTAI
rt_make_soft_real_time();
rt_task_delete(writing_task);
#endif
}
void pcan_transmit::transmit(const std_msgs::String msg)
{
/*!
*\brief The transmit() function is based on the write_loop() function of the transmitest program.
*
* In this part the program waits for a string publication on the pcan_transmitted
* topic and then sends a CAN message based on this string.
*
* */
TPCANMsg m = StringToTPCANRdMsg(msg);
//send the message
//CheckTPCANMsg(m) &&
if ( CAN_Write(h, &m))
{
perror("pcan_transmit: CAN_Write()");
}
}
int main(int argc, char *argv[])
{
TPCANMsg message;
TPCANStatus status;
unsigned long n = 0;
status = CAN_Initialize(PCAN_CHANNEL, PCAN_BAUDRATE, 0, 0, 0);
printf("Initialize CAN: 0x%lx\n", status);
if(status != PCAN_ERROR_OK) goto leave;
message.ID = 0x100;
message.LEN = 8;
message.MSGTYPE = PCAN_MESSAGE_STANDARD;
memset(message.DATA, 0x00, message.LEN);
for(;;) {
while((status = CAN_Write(PCAN_CHANNEL, &message)) == PCAN_ERROR_OK) {
message.DATA[0]++;
if((++n % 1000) == 0)
printf(" - T Message %lu\n", n);
}
}
leave:
CAN_Uninitialize(PCAN_NONEBUS);
return 0;
}
static void test(void * arg)
{
int i;
for (i = 0; i < 5; i++) {
CAN_Write(can_handle, &can_msg[i]);
rt_task_sleep(TIME_DIFF);
}
}
void sendSDO(uint8_t CANid, SDOkey sdo, uint8_t value) {
TPCANMsg msg;
msg.ID = CANid + 0x600; // 0x600 = SDO identifier
msg.LEN = 5;
msg.DATA[0] = 0x2F;
msg.DATA[1] = (sdo.index & 0xFF);
msg.DATA[2] = (sdo.index >> 8) & 0xFF; // todo: & 0xFF not needed, I think
msg.DATA[3] = sdo.subindex;
msg.DATA[4] = value & 0xFF;
CAN_Write(h, &msg);
}
void sendSDO(uint8_t CANid, SDOkey sdo) {
// for SDO read commands, e.g. statusword
TPCANMsg msg;
msg.ID = CANid + 0x600; // 0x600 = SDO identifier
msg.MSGTYPE = 0x00; // standard message
msg.LEN = 4;
msg.DATA[0] = 0x40;
msg.DATA[1] = (sdo.index & 0xFF);
msg.DATA[2] = (sdo.index >> 8) & 0xFF;
msg.DATA[3] = sdo.subindex;
CAN_Write(h, &msg);
}
static void odometry_query_position(void)
{
uint8_t buffer[8];
buffer[0] = 'P';
while(CAN_Write(buffer, DRIVER_TX_IDENTIFICATOR))
_delay_ms(50);
CAN_Read(buffer, DRIVER_RX_IDENTIFICATOR);
position.state = buffer[0];
position.x = (buffer[1] << 8) | buffer[2];
position.y = (buffer[3] << 8) | buffer[4];
position.angle = (buffer[5] << 8) | buffer[6];
}
uint8_t odometry_move_straight(int16_t distance, uint8_t speed, uint8_t (*callback)(uint32_t start_time))
{
uint8_t buffer[8];
odometry_set_speed(speed);
buffer[0] = 'D';
buffer[1] = distance >> 8;
buffer[2] = distance & 0xFF;
while(CAN_Write(buffer, DRIVER_TX_IDENTIFICATOR))
_delay_ms(50);
return odometry_wait_until_done(callback);
}
void odometry_set_speed(uint8_t speed)
{
if(speed == current_speed)
return;
uint8_t buffer[8];
buffer[0] = 'V';
buffer[1] = speed;
while(CAN_Write(buffer, DRIVER_TX_IDENTIFICATOR))
_delay_ms(50);
current_speed = speed;
}
void sendSDO(uint8_t CANid, SDOkey sdo, uint32_t value) {
TPCANMsg msg;
msg.ID = CANid + 0x600; // 0x600 = SDO identifier
msg.LEN = 8;
msg.DATA[0] = 0x23;
msg.DATA[1] = (sdo.index & 0xFF);
msg.DATA[2] = (sdo.index >> 8) & 0xFF;
msg.DATA[3] = sdo.subindex;
msg.DATA[4] = value & 0xFF;
msg.DATA[5] = (value >> 8) & 0xFF;
msg.DATA[6] = (value >> 16) & 0xFF;
msg.DATA[7] = (value >> 24) & 0xFF;
CAN_Write(h, &msg);
}
int BVCan::write( void *param){
// IMPORTANT : msg should be a pointer to a string
BVMessageTPCANMsg message;
message.stringToObject(*(string*)param);
int count;
#ifdef USE_CAN
TPCANMsg canmsg;
canmsg.ID=message.getCanID();
canmsg.MSGTYPE=message.getMSGTYPE();
canmsg.LEN=message.getLEN();
canmsg.DATA[0]=message.getDATA(0);
canmsg.DATA[1]=message.getDATA(1);
canmsg.DATA[2]=message.getDATA(2);
canmsg.DATA[3]=message.getDATA(3);
canmsg.DATA[4]=message.getDATA(4);
canmsg.DATA[5]=message.getDATA(5);
canmsg.DATA[6]=message.getDATA(6);
canmsg.DATA[7]=message.getDATA(7);
errno = CAN_Write(h, &canmsg);
if (errno){
perror("BVCan::write CAN_Write()");
BV_WARNING("BVCan::write .....E R R O R ");
}
else{
count = (sizeof( canmsg.ID )) +
sizeof( canmsg.MSGTYPE )+
sizeof( canmsg.LEN )+
canmsg.LEN * sizeof( canmsg.DATA[0] );
//BV_DEBUGINFO4NNL("BVCan::write ID=" << hex << (int)(canmsg.ID) );
//BV_DEBUGINFO4C(" MSGTYPE=[" << (int)(canmsg.MSGTYPE) << "] " );
//BV_DEBUGINFO4C("LEN=[" << (int)(canmsg.LEN) << "] " );
//BV_DEBUGINFO4C("DATA=[" << (int)(canmsg.DATA[0]) << "]" );
//BV_DEBUGINFO4C("[" << (int)(canmsg.DATA[1]) << "]" );
//BV_DEBUGINFO4C("[" << (int)(canmsg.DATA[2]) << "]" );
//BV_DEBUGINFO4C("[" << (int)(canmsg.DATA[3]) << "]" );
//BV_DEBUGINFO4C("[" << (int)(canmsg.DATA[4]) << "]" );
//BV_DEBUGINFO4C("[" << (int)(canmsg.DATA[5]) << "]" );
//BV_DEBUGINFO4C("[" << (int)(canmsg.DATA[6]) << "]" );
//BV_DEBUGINFO4C("[" << (int)(canmsg.DATA[7])<< "]" << dec << endl );
}
#endif
return count;
}
uint8_t odometry_set_angle(int16_t angle, uint8_t speed, uint8_t (*callback)(uint32_t start_time))
{
uint8_t buffer[8];
odometry_set_speed(speed);
angle *= -1;
buffer[0] = 'A';
buffer[1] = angle >> 8;
buffer[2] = angle & 0xFF;
while(CAN_Write(buffer, DRIVER_TX_IDENTIFICATOR))
_delay_ms(50);
return odometry_wait_until_done(callback);
}
void stop(int8_t type)
{
uint8_t buffer[8];
do
{
buffer[0] = type;
while(CAN_Write(buffer, DRIVER_TX_IDENTIFICATOR))
_delay_ms(50);
odometry_query_position();
}while(position.state == MOVING || position.state == ROTATING);
}
uint8_t odometry_move_to_position(struct odometry_position* position, uint8_t speed, uint8_t direction, uint8_t (*callback)(uint32_t start_time))
{
uint8_t buffer[8];
odometry_set_speed(speed);
buffer[0] = 'G';
buffer[1] = position->x >> 8;
buffer[2] = position->x & 0xFF;
buffer[3] = position->y >> 8;
buffer[4] = position->y & 0xFF;
buffer[5] = 0;//Mozda ne treba 0
buffer[6] = direction;
while(CAN_Write(buffer, DRIVER_TX_IDENTIFICATOR))
_delay_ms(50);
return odometry_wait_until_done(callback);
}
void schunkDefaultPDOOutgoing(uint16_t CANid, double positionValue) {
static const uint16_t myControlword =
controlword_enable_operation | controlword_enable_ip_mode;
TPCANMsg msg;
msg.ID = 0x200 + CANid;
msg.MSGTYPE = 0x00;
msg.LEN = 8;
msg.DATA[0] = myControlword & 0xFF;
msg.DATA[1] = (myControlword >> 8) & 0xFF; // todo: & not needed (?)
msg.DATA[2] = 0;
msg.DATA[3] = 0;
int32_t mdegPos = rad2mdeg(positionValue);
msg.DATA[4] = mdegPos & 0xFF;
msg.DATA[5] = (mdegPos >> 8) & 0xFF;
msg.DATA[6] = (mdegPos >> 16) & 0xFF;
msg.DATA[7] = (mdegPos >> 24) & 0xFF;
CAN_Write(h, &msg);
}
bool PcanPci::sendFrame(TPCANMsg &msg)
{
#ifdef WIN32
int errorcode = CAN_Write(pcanHandle, &msg);
#else
int errorcode = LINUX_CAN_Write_Timeout(pcanHandle, &msg, 100000);
#endif
if (errorcode == 0)
{
//std::cerr << "Sent "; printMsg(msg);
return true;
}
else
{
std::cerr << "sendFrame error, code: " << std::hex << errorcode << std::endl;
return false;
}
}
void odometry_set_position(struct odometry_position* new_position)
{
uint8_t buffer[8];
buffer[0] = 'I';
buffer[1] = new_position->x >> 8;
buffer[2] = new_position->x & 0xFF;
buffer[3] = new_position->y >> 8;
buffer[4] = new_position->y & 0xFF;
buffer[5] = new_position->angle << 8;
buffer[6] = new_position->angle & 0xFF;
position.x = new_position->x;
position.y = new_position->y;
position.angle = new_position->angle;
while(CAN_Write(buffer, DRIVER_TX_IDENTIFICATOR))
_delay_ms(50);
}
// loop writing to CAN-Bus
int write_loop()
{
// write out endless loop until Ctrl-C
while (1)
{
std::list<TPCANMsg>::iterator iter;
int i;
for (iter = List->begin(); iter != List->end(); iter++)
{
// test for standard frames only
if ((nExtended == CAN_INIT_TYPE_EX) || !(iter->MSGTYPE & MSGTYPE_EXTENDED))
{
// send the message
if ((errno = CAN_Write(h, &(*iter))))
{
perror("transmitest: CAN_Write()");
return errno;
}
}
}
}
return 0;
}
void main()
{
u16 tmp;
u8 tmp8, tmp8_A;
static tCANMsg RxMsgBuff;
CLK_Init(CLK_HSE);
disableInterrupts();
GPIO_Init();
TIM4_Init();
// TIM2_Init();
TIM1_Init();
ADC_Init();
/* Configure CAN - Interface */
CAN_Init(); // init CAN - interface
enableInterrupts();
CAN_Start();
CAN_TxMsg1.Length = 4;
CAN_TxMsg1.Xtd = false;
CAN_TxMsg1.rtr = false;
CAN_TxMsg1.ID = 0x280;
CAN_TxMsg1.Data[0] =0 ;
CAN_TxMsg1.Data[1] =0;
//PWM_SetFrequency(1);
do {
if ( IsCAN_MSG1_Send()) {
tmp = ADC_GetValue(ADC_REV_CHANEL);
tmp = tmp *24;
if ( tmp < 100 ) {
OIL_PRESS = false;
} else {
OIL_PRESS = true;
}
CAN_TxMsg1.Data[3] =(u8)(tmp >> 8) ;
CAN_TxMsg1.Data[2] =(u8)(tmp & 0x00FF);
CAN_Write(&CAN_TxMsg1);
}
if ( IsSpeedAdjustTime()) {
tmp = ADC_GetValue(ADC_SPEED_CHANEL);
tmp = tmp * 10;
tmp = tmp/ 34;
PWM_SetFrequency(tmp);
// 1023 = 300 Hz
}
/* if( CAN_GetMsg(&RxMsgBuff)== RET_OK){
if (RxMsgBuff.Xtd ){ // EID
tmp8 = (u8)(RxMsgBuff.timeStamp >>3) & 0xE0U;
if (RxMsgBuff.rtr) tmp8 |= 0x10;
tmp8 |= (RxMsgBuff.Length & 0x0F);
tmp8_A = (RxMsgBuff.ID>>24) & 0x001f;
tmp8_A |= (RxMsgBuff.FilterID << 5);
USART_SendBytesMessage (CAN_MSG_EXT_1, tmp8,(u8)((RxMsgBuff.timeStamp) & 0x00FF), tmp8_A ,(RxMsgBuff.ID>>16) & 0x00ffU);
USART_SendBytesMessage (CAN_MSG_EXT_2, (RxMsgBuff.ID>>8) & 0x00ffU,RxMsgBuff.ID & 0x00ffU,RxMsgBuff.Data[0], RxMsgBuff.Data[1] );
if (RxMsgBuff.Length > 2)
USART_SendBytesMessage (CAN_MSG_EXT_3, RxMsgBuff.Data[2], RxMsgBuff.Data[3], RxMsgBuff.Data[4], RxMsgBuff.Data[5]);
if (RxMsgBuff.Length > 6 )
USART_SendBytesMessage (CAN_MSG_EXT_4, RxMsgBuff.Data[6], RxMsgBuff.Data[7],0,0);
}else{ // standard ID
tmp8 = (u8)(RxMsgBuff.timeStamp >>3) & 0xE0U;
if (RxMsgBuff.rtr) tmp8 |= 0x10;
tmp8 |= (RxMsgBuff.Length & 0x0F);
tmp8_A = (RxMsgBuff.ID>>8) & 0x007f;
tmp8_A |= (RxMsgBuff.FilterID << 3);
USART_SendBytesMessage (CAN_MSG_STD_1, tmp8,(u8)((RxMsgBuff.timeStamp) & 0x00FF), tmp8_A ,RxMsgBuff.ID & 0x00ff);
if (RxMsgBuff.Length > 0)
USART_SendBytesMessage (CAN_MSG_STD_2, RxMsgBuff.Data[0], RxMsgBuff.Data[1], RxMsgBuff.Data[2], RxMsgBuff.Data[3]);
if (RxMsgBuff.Length > 4 )
USART_SendBytesMessage (CAN_MSG_STD_3, RxMsgBuff.Data[4], RxMsgBuff.Data[5], RxMsgBuff.Data[6], RxMsgBuff.Data[7]);
}
}*/
/** LED Flashing **/
if (GetLedState()) {
LED_ON;
} else {
LED_OFF;
}
} while (1);
}
int arm_sendmsg(uint16_t joint, uint8_t cmd, uint8_t index, uint8_t *data, int data_length){
TPCANMsg txmsg;
if(cmd == CMDTYPE_WR){
if(data_length > 6){
printf("arm_sendmsg error,the data_length must <= 6!\n");
}
// clear the txmsg
memset(&txmsg,0,sizeof(TPCANMsg));
// Construct the txmsg
// 1 the STID
txmsg.ID= joint;
txmsg.MSGTYPE = 0;
// 2 the DLC/4 is the data_length + 2
txmsg.LEN = data_length + 2;
// 3 the data 1 Byte is the cmd
txmsg.DATA[0] = cmd;
txmsg.DATA[1] = index;
memcpy(&txmsg.DATA[2], data, data_length);
//Send the TX message
if (CAN_Write(h,&txmsg))
{
printf("ERROR: write 1 returned 1\n");
return -1;
}
//TODO : receive the ack message
if(arm_recvack(joint, cmd, index)==0)
return 0;
else{
printf("recvack is error\n");
return -1;
}
}else if(cmd == CMDTYPE_WR_NR){
if(data_length > 6){
printf("arm_sendmsg error,the data_length must <= 6!\n");
}
// clear the txmsg
memset(&txmsg,0,sizeof(TPCANMsg));
// Construct the txmsg
// 1 the STID
txmsg.ID = joint;
txmsg.MSGTYPE = 0;
// 2 the DLC/4 is the data_length + 2
txmsg.LEN = data_length + 2;
// 3 the data 1 Byte is the cmd
txmsg.DATA[0] = cmd;
txmsg.DATA[1] = index;
memcpy(&txmsg.DATA[2], data, data_length);
//Send the TX message
if (CAN_Write(h,&txmsg))
{
printf("ERROR: write returned 1\n");
return -1;
}
return 0;
}else if(cmd == CMDTYPE_RD){
// clear the txmsg
memset(&txmsg,0,sizeof(TPCANMsg));
// Construct the txmsg
// 1 the STID
txmsg.ID = joint;
txmsg.MSGTYPE = 0;
// 2 the DLC/4 is the data_length + 2
// 2 the DLC/4 is 03 when read cmd
txmsg.LEN = 0x03;
// 3 the data 1 Byte is the cmd
txmsg.DATA[0] = cmd;
txmsg.DATA[1] = index;
txmsg.DATA[2] = (uint8_t)data_length;
//Send the TX message
if (CAN_Write(h,&txmsg))
{
printf("ERROR: sendmsg returned 1\n");
return -1;
}
return 0;
}else if(cmd == CMDTYPE_WR_REG){
}else if(cmd == CMDTYPE_SCP){
}else if(cmd == CMDTYPE_RST){
}else{
printf("cmd error,the joint must in 00-05!\n");
return -1;
}
printf("unknown error\n");
return -1;
}
void _publish()
{
ROS_INFO("_publish()");
printf("yaw: %f\n", yaw_);
printf("throttle: %f\n" , throttle_);
yaw_IEEE754 = pack754(yaw_, 32, 8);
throttle_IEEE754 = pack754(throttle_, 32 , 8 );
// std::cout << "yaw ieee754: " << "0x" <<std::hex << yaw_IEEE754 << std::endl;
// std::cout << "throttle ieee754: " << "0x" <<std::hex << throttle_IEEE754 << std::endl;
// std::cout << "zero ieee754: " << "0x" <<std::hex << zero_IEEE754 << std::endl;
// std::cout << "one ieee754: " << "0x" <<std::hex << one_IEEE754 << std::endl;
std::cout << "forward: " << forward << std::endl;
std::cout << "reverse: " << reverse << std::endl;
std::cout << "reverse_on: " << reverse_on << std::endl;
if(forward) {
if(reverse_on) {
ROS_WARN_NAMED("test","writing forward throttle msg");
if(!CAN_Write(h_, &normal_throttle_msg_)) {
ROS_INFO("Success");
reverse_on=false;
usleep(250000);
}
else
ROS_ERROR("ERROR");
}
}
if(reverse) {
if(!reverse_on) {
ROS_ERROR("writing reverse throttle msg...\n");
if(!CAN_Write(h_, &reverse_throttle_msg_)) {
ROS_INFO("Success");
reverse_on=true;
usleep(250000);
}
else
ROS_ERROR("ERROR");
}
}
//steering msg
can_steering_msg_.ID = 2;
can_steering_msg_.MSGTYPE = MSGTYPE_STANDARD;
can_steering_msg_.LEN = sizeof(yaw_IEEE754);
for(int i=0 ; i<can_steering_msg_.LEN; i++ )
{
can_steering_msg_.DATA[i] = (yaw_IEEE754 >> (8*i)) & 0xff;
}
//throttle msg
can_throttle_msg_.ID = 1;
can_throttle_msg_.MSGTYPE = MSGTYPE_STANDARD;
can_throttle_msg_.LEN = sizeof(throttle_IEEE754);
for(int i=0 ; i<can_throttle_msg_.LEN; i++ )
{
can_throttle_msg_.DATA[i] = (throttle_IEEE754 >> (8*i)) & 0xff;
}
//clear CAN status
CAN_Status(h_);
std::cout << "writing steering msg..." << std::endl;
if(!CAN_Write(h_, &can_steering_msg_))
ROS_INFO("Success");
else
ROS_ERROR("ERROR");
std::cout << "writing throttle msg..." << std::endl;
if(!CAN_Write(h_, &can_throttle_msg_))
ROS_INFO("Success");
else
ROS_ERROR("ERROR");
std::cout << "i'm finished" << std::endl;
}
int main(void)
{
wiimote *wm;
wiimote** wiimotes;
int found, connected;
HANDLE canHandle;
TPCANMsg Message;
int wiimote_led_state;
int exit = 0;
canHandle = initCAN();
if(canHandle == NULL)
{
printf("Error opening CAN device!\n");
return -1;
}
wiimotes = wiiuse_init(MAX_WIIMOTES);
found = wiiuse_find(wiimotes, MAX_WIIMOTES, 5);
if (!found)
{
printf ("No wiimotes found.\n");
return 0;
}
connected = wiiuse_connect(wiimotes, MAX_WIIMOTES);
if (connected)
printf("Connected to %i wiimotes (of %i found).\n", connected, found);
else
{
printf("Failed to connect to any wiimote.\n");
return 0;
}
wm = wiimotes[0];
wiiuse_status(wm);
while(wm->event != WIIUSE_STATUS)
{
wiiuse_poll(wiimotes, MAX_WIIMOTES);
}
printf("Battery level: %f%%\n", wm->battery_level*100);
while (1)
{
if(exit)
break;
if (wiiuse_poll(wiimotes, MAX_WIIMOTES))
{
/*
* This happens if something happened on any wiimote.
* So go through each one and check if anything happened.
*/
int i = 0;
for (; i < MAX_WIIMOTES; ++i)
{
switch (wiimotes[i]->event)
{
case WIIUSE_EVENT:
/* a generic event occured */
handle_event(wiimotes[i]);
Message.ID = CAN_INPUT_MSG_ID;
Message.MSGTYPE = MSGTYPE_STANDARD;
Message.LEN = 7;
Message.DATA[0] = carInputs.accel;
Message.DATA[1] = carInputs.brake;
Message.DATA[2] = carInputs.steer;
Message.DATA[3] = carInputs.gear;
Message.DATA[4] = carInputs.clutch;
Message.DATA[5] = carInputs.controls;
Message.DATA[6] = carInputs.cruisedist;
CAN_Write(canHandle,&Message);
// Show the status of ABS/TC/Cruise on the LEDs
wiimote_led_state = 0;
if(carInputs.controls & ABS)
wiimote_led_state |= WIIMOTE_LED_1;
if(carInputs.controls & TC)
wiimote_led_state |= WIIMOTE_LED_2;
if(carInputs.controls & STABILITY)
wiimote_led_state |= WIIMOTE_LED_3;
if(carInputs.controls & CRUISE)
wiimote_led_state |= WIIMOTE_LED_4;
wiiuse_set_leds(wm, wiimote_led_state);
break;
case WIIUSE_DISCONNECT:
case WIIUSE_UNEXPECTED_DISCONNECT:
/* the wiimote disconnected */
handle_disconnect(wiimotes[i]);
exit = 1;
break;
default:
break;
}
}
}
}
wiiuse_cleanup(wiimotes, MAX_WIIMOTES);
return 0;
}
