Thank you so much Jan,
The program is provided from the company so there is no link.
I will open another topic.
Regards
Nada
I thought an example, here on the site Mbed.
Then OK.
BR, Jan
Thank you Jan.
Regards
Nada
Dear Jan,
I hop you re doing well.
I want to put more than one message in the queue, read it back, write them in the CAN Bus like the example below: (part of my code)
void State_Machine_Thread()
{
CANMessage msg_Local;
bool local_i = false;
bool local_j = false;
message_t *message = mpool.alloc();
message->can_id = 0x00;
message->can_len = 2;
message->can_msg[0]= 0x01;
message->can_msg[1]=0x00;
queue.put(message);
while(true)
{
// Thread::signal_wait(0x1);
/* CAN_read_Mutex.lock();
msg_Local = msg;
CAN_read_Mutex.unlock();*/
Enable_Voltage_Mutex.lock();
local_i = global_i;
Enable_Voltage_Mutex.unlock();
Enable_State_Mutex.lock();
local_j = global_j;
Enable_State_Mutex.unlock();
if(local_i)
{
// printf(" Start Homing\n\r");
//set up the homing method
message_t *message_1 = mpool.alloc();
message_1->can_id = 0x602;
message_1->can_len = 8;
message_1->can_msg[0]= 0x2F;
message_1->can_msg[1]=0x98;
message_1->can_msg[2]=0x60;
message_1->can_msg[3]=0x00;
message_1->can_msg[4]=0x21;
message_1->can_msg[5]=0x00;
message_1->can_msg[6]=0x00;
message_1->can_msg[7]=0x00;
queue.put(message_1);
//Thread::wait(200);
//encoder resolution
message_t *message_2 = mpool.alloc(); // Encoder increments
message_2->can_id = 0x602;
message_2->can_len = 8;
message_2->can_msg[0]= 0x23;
message_2->can_msg[1]=0x8F;
message_2->can_msg[2]=0x60;
message_2->can_msg[3]=0x01;
message_2->can_msg[4]=0xD0;
message_2->can_msg[5]=0x02;
message_2->can_msg[6]=0x00;
message_2->can_msg[7]=0x00;
queue.put(message_2);
//Thread::wait(200);
//Motor revolution
message_t *message_3 = mpool.alloc();
message_3->can_id = 0x602;
message_3->can_len = 8;
message_3->can_msg[0]= 0x23;
message_3->can_msg[1]=0x8F;
message_3->can_msg[2]=0x60;
message_3->can_msg[3]=0x02;
message_3->can_msg[4]=0x01;
message_3->can_msg[5]=0x00;
message_3->can_msg[6]=0x00;
message_3->can_msg[7]=0x00;
queue.put(message_3);
//Thread::wait(200);
//operation mode : homing
message_t *message_4 = mpool.alloc();
message_4->can_id = 0x602;
message_4->can_len = 8;
message_4->can_msg[0]=0x2F;
message_4->can_msg[1]=0x60;
message_4->can_msg[2]=0x60;
message_4->can_msg[3]=0x00;
message_4->can_msg[4]=0x06;
message_4->can_msg[5]=0x00;
message_4->can_msg[6]=0x00;
message_4->can_msg[7]=0x00;
queue.put(message_4);
//Thread::wait(200);
//set the state machine:
message_t *message_5 = mpool.alloc();
message_5->can_id = 0x602;
message_5->can_len = 8;
message_5->can_msg[0]=0x2B;
message_5->can_msg[1]=0x40;
message_5->can_msg[2]=0x60;
message_5->can_msg[3]=0x00;
message_5->can_msg[4]=0x06;
message_5->can_msg[5]=0x00;
message_5->can_msg[6]=0x00;
message_5->can_msg[7]=0x00;
queue.put(message_5);
//Thread::wait(200);
//read back the stausword
message_t *message_6 = mpool.alloc();
message_6->can_id = 0x602;
message_6->can_len = 8;
message_6->can_msg[0]=0x40;
message_6->can_msg[1]=0x41;
message_6->can_msg[2]=0x60;
message_6->can_msg[3]=0x00;
message_6->can_msg[4]=0x00;
message_6->can_msg[5]=0x00;
message_6->can_msg[6]=0x00;
message_6->can_msg[7]=0x00;
queue.put(message_6);
// Thread::wait(200);
if(local_j)
{
// printf("start state");
//switch to switch on state
message_t *message_7 = mpool.alloc();
message_7->can_id = 0x602;
message_7->can_len = 8;
message_7->can_msg[0]=0x2B;
message_7->can_msg[1]=0x40;
message_7->can_msg[2]=0x60;
message_7->can_msg[3]=0x00;
message_7->can_msg[4]=0x07;
message_7->can_msg[5]=0x00;
message_7->can_msg[6]=0x00;
message_7->can_msg[7]=0x00;
queue.put(message_7);
// Thread::wait(200);
//read back 6041:
message_t *message_8 = mpool.alloc();
message_8->can_id = 0x602;
message_8->can_len = 8;
message_8->can_msg[0]=0x40;
message_8->can_msg[1]=0x41;
message_8->can_msg[2]=0x60;
message_8->can_msg[3]=0x00;
message_8->can_msg[4]=0x00;
message_8->can_msg[5]=0x00;
message_8->can_msg[6]=0x00;
message_8->can_msg[7]=0x00;
queue.put(message_8);
message_t *message_9 = mpool.alloc();
message_9->can_id = 0x602;
message_9->can_len = 8;
message_9->can_msg[0]=0x2B;
message_9->can_msg[1]=0x40;
message_9->can_msg[2]=0x60;
message_9->can_msg[3]=0x00;
message_9->can_msg[4]=0x0F;
message_9->can_msg[5]=0x00;
message_9->can_msg[6]=0x00;
message_9->can_msg[7]=0x00;
queue.put(message_9);
message_t *message_10 = mpool.alloc();
message_10->can_id = 0x602;
message_10->can_len = 8;
message_10->can_msg[0]=0x2B;
message_10->can_msg[1]=0x40;
message_10->can_msg[2]=0x60;
message_10->can_msg[3]=0x00;
message_10->can_msg[4]=0x1F;
message_10->can_msg[5]=0x00;
message_10->can_msg[6]=0x00;
message_10->can_msg[7]=0x00;
queue.put(message_10);
/* message_t *message_8 = mpool.alloc();
message_8->can_id = 0x602;
message_8->can_len = 8;
message_8->can_msg[0]=0x40;
message_8->can_msg[1]=0x41;
message_8->can_msg[2]=0x60;
message_8->can_msg[3]=0x00;
message_8->can_msg[4]=0x00;
message_8->can_msg[5]=0x00;
message_8->can_msg[6]=0x00;
message_8->can_msg[7]=0x00;
queue.put(message_8);
// Thread::wait(200);*/
}
}
Thread::wait(200);
}
}
//////////////////////////////////////////////////////////////////////////CAN Read////////////////////////////////////////////////////////////////////////////////////
void receive()
{
CANMessage msg_Local;//create empty CAN message
bool local_i= false;
bool local_j=false;
while(1)
{
//CAN_Mutex.lock();
if(can1.read(msg_Local)) // if message is available, read Into msg
{
//CAN_Mutex.unlock();
CAN_read_Mutex.lock();
msg = msg_Local;
CAN_read_Mutex.unlock();
if((msg_Local.id == 0x702) && (msg_Local.data[0]== 0x05))
{
printf(" Node is operational\n\r");
// State_Machine.signal_set(0x1);
local_i= true;
Enable_Voltage_Mutex.lock();
global_i = local_i;
Enable_Voltage_Mutex.unlock();
}
if((msg_Local.id == 0x582) && (msg_Local.data[1]==0x41) &&(msg_Local.data[2]==0x60) &&(msg_Local.data[3]==0x00) && ((msg_Local.data[4] & 0xEF )== 0x21))
{
printf("voltage is enabled\n\r");
local_j= true;
Enable_State_Mutex.lock();
global_j = local_j;
Enable_State_Mutex.unlock();
//printf("local_j_read= %i\n\r",local_j);
}
if((msg_Local.id == 0x582) && (msg_Local.data[1]==0x41) &&(msg_Local.data[2]==0x60) &&(msg_Local.data[3]==0x00) && ((msg_Local.data[4] & 0xEF )== 0x23))
{
printf("State is enabled\n\r");
local_j= true;
Enable_State_Mutex.lock();
global_j = local_j;
Enable_State_Mutex.unlock();
}
printf("CAN message received\r\n");
printf(" ID = 0x%.3x\r\n", msg_Local.id);
printf(" Type = %d\r\n", msg_Local.type);
printf(" Format = %d\r\n", msg_Local.format);
printf(" Length = %d\r\n", msg_Local.len);
printf(" Data =");
for (int i = 0; i < msg_Local.len; i++)
{
//CAN_msg.float_bytes[i]=msg.data[i];
printf(" HEX= %.2X\n\r", msg_Local.data[i]);
// printf("\r\n");
}
led2 = ! led2; //toggle status LED//printf("send....\r\n");
}
Thread::wait(1);
}
}
///////////////////////////////////////////////////////////////////////////CAN Write/////////////////////////////////////////////////////////////////////////////////////////////
void send()
{
while(1)
{//send value to CAN bus AND monitor return value to check if CAN message was sent successfully. If so display, increment and toggle.
osEvent evt = queue.get();
if (evt.status == osEventMessage)
{
message_t *message = (message_t*)evt.value.p;
CAN_Mutex.lock();
can1.write(CANMessage(message->can_id, message->can_msg, message->can_len));
CAN_Mutex.unlock();
mpool.free(message);
led1=!led1;
}
if(queue.empty())
{
printf("queus is empty\n\r");
Thread::wait(200);
}
//printf(" free queue = %i\n\r",queue.empty());
}
}
it seems that the queue is free before writing all the messages. and if I didn’t check if the queue is free, the nucleo is crashing.
please your help
Regards
Nada
Hello Nada,
from my point of view
void send()
{
while(1)
{//send value to CAN bus AND monitor return value to check if CAN message was sent successfully. If so display, increment and toggle.
if(!queue.empty())
{
osEvent evt = queue.get();
if (evt.status == osEventMessage)
{
message_t *message = (message_t*)evt.value.p;
CAN_Mutex.lock();
can1.write(CANMessage(message->can_id, message->can_msg, message->can_len));
CAN_Mutex.unlock();
mpool.free(message);
led1=!led1;
}
}
else
{
//printf("queus is empty\n\r");
Thread::wait(10);
}
}
}
BR, Jan
Thank you so much Jan for your continuas support.
I will try and let you know.
Regards
Nada
Dear Jan,
I hope you are doing well.
Could you please have a look to my following program:
#include "mbed.h"
#include "Map.hpp"
DigitalOut led1(LED1);// status LED
DigitalOut led2(LED2);// status LED
DigitalOut led3(LED3);// status LED
//CAN can1(p9, p10);
//CAN can2(p30, p29);
CAN can1(PB_8, PB_9,1000000); // CAN interface
Thread CAN_Read(osPriorityNormal, 2000);
Thread CAN_Write(osPriorityNormal, 2000);
Thread M1(osPriorityNormal, 2000);
Thread M2(osPriorityNormal, 2000);
typedef struct
{
int can_id;
char can_msg[8];
int can_len;
} message_t;
MemoryPool<message_t, 32> mpool;
Queue<message_t, 32> queue;
Mutex CAN_Mutex;
Mutex CAN_read_Mutex;
CANMessage msg;
//STATES
/*#define STATE_NMT 0
#define STATE_CHECK_NMT 1
#define STATE_HOMMING_METHOD 2
#define STATE_CHECH_HOMING 3
#define STATE_SET_ENCODER_RESOLUTION 4
#define STATE_CHECK_ENCODER_RESOLUTION 5
#define STATE_SET_MOTOR_REVOLUTION 6
#define STATE_CHECK_MOTOR_REVOLUTION 7
#define STATE_SET_OPERATION_MODE 8
#define STATE_CHECK_OPERATION_MODE 9
#define STATE_ENABLE_VOLTAGE 10
#define STATE_CHECK_STATUSWORLD_VOLTAGE 11
#define STATE_ENABLE_STATE 12
#define STATE_CHECK_STATUSWORLD_STATE 13
#define STATE_ENABLE_OPERATION 14
#define STATE_CHECK_STATUSWORLD_OPERATION 15
#define STATE_START_HOMING 16
#define STATE_CHECK_HOMING_FINISHED 17*/
//profile position M1
#define STATE_PROFILE_POSITION_1 0
#define STATE_CHECK_PROFILE_POSITION_1 1
#define STATE_TARGET_POSITION_1 2
#define STATE_CHECH_TARGET_POSITION_1 3
#define STATE_SET_SPEED_1 4
#define STATE_CHECK_SPEED_1 5
#define STATE_ENABLE_VOLTAGE_P_1 10
#define STATE_CHECK_STATUSWORLD_VOLTAGE_P_1 11
#define STATE_ENABLE_STATE_P_1 12
#define STATE_CHECK_STATUSWORLD_STATE_P_1 13
#define STATE_ENABLE_OPERATION_P_1 14
#define STATE_CHECK_STATUSWORLD_OPERATION_P_1 15
#define STATE_ABSOLUTE_1 16
#define STATE_REACHED_TARGET_1 17
//profile position M2
#define STATE_PROFILE_POSITION_2 18
#define STATE_CHECK_PROFILE_POSITION_2 19
#define STATE_TARGET_POSITION_2 20
#define STATE_CHECH_TARGET_POSITION_2 21
#define STATE_SET_SPEED_2 22
#define STATE_CHECK_SPEED_2 23
#define STATE_ENABLE_VOLTAGE_P_2 24
#define STATE_CHECK_STATUSWORLD_VOLTAGE_P_2 25
#define STATE_ENABLE_STATE_P_2 26
#define STATE_CHECK_STATUSWORLD_STATE_P_2 27
#define STATE_ENABLE_OPERATION_P_2 28
#define STATE_CHECK_STATUSWORLD_OPERATION_P_2 29
#define STATE_ABSOLUTE_2 30
#define STATE_REACHED_TARGET_2 31
//NMT state
#define STATE_NMT_1 32
#define STATE_CHECK_NMT_1 33
#define STATE_NMT_2 34
#define STATE_CHECK_NMT_2 35
Map xMapper(0,1, -3000,3000);
///////////////////////////////////////////////////////////////////////////print CAN message//////////////////////////////////////////////////////////////////////////////
void printMsg(CANMessage& msg)
{
printf(" ID = 0x%.3x\r\n", msg.id);
printf(" Type = %d\r\n", msg.type);
printf(" Format = %d\r\n", msg.format);
printf(" Length = %d\r\n", msg.len);
printf(" Data =");
for(int i = 0; i < msg.len; i++)
printf(" 0x%.2X", msg.data[i]);
printf("\r\n");
}
void receive()
{
CANMessage msg_Local;//create empty CAN message
while(1)
{
CAN_Mutex.lock();
if(can1.read(msg_Local))
{ // if message is available, read Into msg
CAN_Mutex.unlock();
CAN_read_Mutex.lock();
msg = msg_Local;
CAN_read_Mutex.unlock();
printMsg(msg_Local);
led2 = ! led2; //toggle status LED//printf("send....\r\n");
}
else
{
CAN_Mutex.unlock();
}
Thread::wait(1);
}
}
///////////////////////////////////////////////////////////////////////////CAN Write/////////////////////////////////////////////////////////////////////////////////////////////
void send()
{
while (true)
{
osEvent evt = queue.get();
if (evt.status == osEventMessage)
{
message_t *message = (message_t*)evt.value.p;
CAN_Mutex.lock();
can1.write(CANMessage(message->can_id, message->can_msg, message->can_len));
CAN_Mutex.unlock();
mpool.free(message);
}
Thread::wait(20);
}
}
/////////////////////////////////////////////////////////////////////////////M1//////////////////////////////////////////////////////////////////////////////
void M1_State()
{
CANMessage msg_Local;
int state = STATE_NMT_1;
while(true)
{
//printf("M1\n\r");
CAN_read_Mutex.lock();
msg_Local = msg;
CAN_read_Mutex.unlock();
//printf("counter = %d\n\r",counter);
// long int target = xMapper.Calculate(i);
switch(state)
{
case STATE_NMT_1:
{
message_t *message_1 = mpool.alloc();
message_1->can_id = 0x00;
message_1->can_len = 2;
message_1->can_msg[0]= 0x01;
message_1->can_msg[1]=0x01;
queue.put(message_1);
state = STATE_CHECK_NMT_1;
break;
}
case STATE_CHECK_NMT_1:
{
if((msg_Local.id == 0x701) && (msg_Local.data[0]== 0x05))
{
state = STATE_PROFILE_POSITION_1;
}
else
{
state = STATE_NMT_1;
}
break;
}
case STATE_PROFILE_POSITION_1:
{
message_t *message_1 = mpool.alloc();
message_1->can_id = 0x601;
message_1->can_len = 8;
message_1->can_msg[0]= 0x2F;
message_1->can_msg[1]=0x60;
message_1->can_msg[2]=0x60;
message_1->can_msg[3]=0x00;
message_1->can_msg[4]=0x01;
message_1->can_msg[5]=0x00;
message_1->can_msg[6]=0x00;
message_1->can_msg[7]=0x00;
queue.put(message_1);
state = STATE_CHECK_PROFILE_POSITION_1;
break;
}
case STATE_CHECK_PROFILE_POSITION_1:
{
if((msg_Local.id == 0x581) && (msg_Local.data[1]== 0x60)&& (msg_Local.data[2]== 0x60) && (msg_Local.data[4]== 0x00))
{
printf("positions_1 set\n\r");
}
else
{
printf("positions_1 NOT set\n\r");
}
break;
}
}
Thread::wait(20);
}
}
/////////////////////////////////////////////////////////////////////////////M2//////////////////////////////////////////////////////////////////
void M2_State()
{
CANMessage msg_Local;
int state = STATE_NMT_2;
while(true)
{
//printf("M1\n\r");
CAN_read_Mutex.lock();
msg_Local = msg;
CAN_read_Mutex.unlock();
//printf("counter = %d\n\r",counter);
// long int target = xMapper.Calculate(i);
switch(state)
{
case STATE_NMT_2:
{
message_t *message_1 = mpool.alloc();
message_1->can_id = 0x00;
message_1->can_len = 2;
message_1->can_msg[0]= 0x01;
message_1->can_msg[1]=0x02;
queue.put(message_1);
state = STATE_CHECK_NMT_2;
break;
}
case STATE_CHECK_NMT_2:
{
if((msg_Local.id == 0x702) && (msg_Local.data[0]== 0x05))
{
state = STATE_PROFILE_POSITION_2;
}
else
{
state = STATE_NMT_2;
}
break;
}
case STATE_PROFILE_POSITION_2:
{
message_t *message_1 = mpool.alloc();
message_1->can_id = 0x602;
message_1->can_len = 8;
message_1->can_msg[0]= 0x2F;
message_1->can_msg[1]=0x60;
message_1->can_msg[2]=0x60;
message_1->can_msg[3]=0x00;
message_1->can_msg[4]=0x01;
message_1->can_msg[5]=0x00;
message_1->can_msg[6]=0x00;
message_1->can_msg[7]=0x00;
queue.put(message_1);
state = STATE_CHECK_PROFILE_POSITION_2;
break;
}
case STATE_CHECK_PROFILE_POSITION_2:
{
if((msg_Local.id == 0x582) && (msg_Local.data[1]== 0x60)&& (msg_Local.data[2]== 0x60) && (msg_Local.data[4]== 0x00))
{
printf("positions_2 set\n\r");
}
else
{
printf("positions_2 NOT set\n\r");
}
break;
}
}
Thread::wait(20);
}
}
int main() {
// printf("main()\n\r");
CAN_Read.start(receive);
CAN_Write.start(send);
M1.start(M1_State);
M2.start(M2_State);
}
it seems that the sending thread is not working.
I don’t know what the issue acyuall.
Please advise
Nada
Hello Nada,
I am doing well, thank you for asking.
I do not know, the send function seems to be OK, but maybe two threads for one CAN can be the issue.
What behavior did you see?
BR, Jan
Hello Jan
Thank you for your reply and sorry for my late reply.
Actually whenever I add any extra sending command to both threads, CAN stop sending the messages. When I changed the thread sleep for both threads it works for a while and then stop sending.
the timing is depend on how many commands I’m sending in each thread.
I tried hard but nothing work with me. if you can advise how can I solve this issue as I have only one can bus.
Regards
Nada
Hello Jan,
I used two Can bus for each thread, but still the timing issue.
Could you please advise.
The modified code is as the following
#include "mbed.h"
#include "Map.hpp"
DigitalOut led1(LED1);// status LED
DigitalOut led2(LED2);// status LED
DigitalOut led3(LED3);// status LED
//CAN can1(p9, p10);
//CAN can2(p30, p29);
CAN can1(PB_8, PB_9,1000000); // CAN interface
CAN can2(PB_5, PB_6,1000000); // CAN interface
Thread CAN_Read_1(osPriorityNormal, 2000);
Thread CAN_Write_1(osPriorityNormal, 2000);
Thread CAN_Read_2(osPriorityNormal, 2000);
Thread CAN_Write_2(osPriorityNormal, 2000);
Thread M1(osPriorityNormal, 2000);
Thread M2(osPriorityNormal, 2000);
typedef struct
{
int can_id;
char can_msg[8];
int can_len;
} message_t;
typedef struct
{
int can_id;
char can_msg[8];
int can_len;
} message_t_1;
MemoryPool<message_t, 32> mpool;
Queue<message_t, 32> queue;
Mutex CAN_Mutex;
Mutex CAN_read_Mutex;
Mutex CAN_1_Mutex;
Mutex CAN_1_read_Mutex;
CANMessage msg_1;
CANMessage msg_2;
//STATES
/*#define STATE_NMT 0
#define STATE_CHECK_NMT 1
#define STATE_HOMMING_METHOD 2
#define STATE_CHECH_HOMING 3
#define STATE_SET_ENCODER_RESOLUTION 4
#define STATE_CHECK_ENCODER_RESOLUTION 5
#define STATE_SET_MOTOR_REVOLUTION 6
#define STATE_CHECK_MOTOR_REVOLUTION 7
#define STATE_SET_OPERATION_MODE 8
#define STATE_CHECK_OPERATION_MODE 9
#define STATE_ENABLE_VOLTAGE 10
#define STATE_CHECK_STATUSWORLD_VOLTAGE 11
#define STATE_ENABLE_STATE 12
#define STATE_CHECK_STATUSWORLD_STATE 13
#define STATE_ENABLE_OPERATION 14
#define STATE_CHECK_STATUSWORLD_OPERATION 15
#define STATE_START_HOMING 16
#define STATE_CHECK_HOMING_FINISHED 17*/
//profile position M1
#define STATE_PROFILE_POSITION_1 0
#define STATE_CHECK_PROFILE_POSITION_1 1
#define STATE_TARGET_POSITION_1 2
#define STATE_CHECH_TARGET_POSITION_1 3
#define STATE_SET_SPEED_1 4
#define STATE_CHECK_SPEED_1 5
#define STATE_ENABLE_VOLTAGE_P_1 10
#define STATE_CHECK_STATUSWORLD_VOLTAGE_P_1 11
#define STATE_ENABLE_STATE_P_1 12
#define STATE_CHECK_STATUSWORLD_STATE_P_1 13
#define STATE_ENABLE_OPERATION_P_1 14
#define STATE_CHECK_STATUSWORLD_OPERATION_P_1 15
#define STATE_ABSOLUTE_1 16
#define STATE_REACHED_TARGET_1 17
//#define STATE_CHECK_VOLTAGE_P_1 32
//profile position M2
#define STATE_PROFILE_POSITION_2 18
#define STATE_CHECK_PROFILE_POSITION_2 19
#define STATE_TARGET_POSITION_2 20
#define STATE_CHECH_TARGET_POSITION_2 21
#define STATE_SET_SPEED_2 22
#define STATE_CHECK_SPEED_2 23
#define STATE_ENABLE_VOLTAGE_P_2 24
#define STATE_CHECK_STATUSWORLD_VOLTAGE_P_2 25
#define STATE_ENABLE_STATE_P_2 26
#define STATE_CHECK_STATUSWORLD_STATE_P_2 27
#define STATE_ENABLE_OPERATION_P_2 28
#define STATE_CHECK_STATUSWORLD_OPERATION_P_2 29
#define STATE_ABSOLUTE_2 30
#define STATE_REACHED_TARGET_2 31
//#define STATE_CHECK_VOLTAGE_P_2 33
//NMT state
#define STATE_NMT_1 32
#define STATE_CHECK_NMT_1 33
#define STATE_NMT_2 34
#define STATE_CHECK_NMT_2 35
Map xMapper(0,1, -3000,3000);
///////////////////////////////////////////////////////////////////////////print CAN message//////////////////////////////////////////////////////////////////////////////
void printMsg(CANMessage& msg)
{
printf(" ID = 0x%.3x\r\n", msg.id);
printf(" Type = %d\r\n", msg.type);
printf(" Format = %d\r\n", msg.format);
printf(" Length = %d\r\n", msg.len);
printf(" Data =");
for(int i = 0; i < msg.len; i++)
printf(" 0x%.2X", msg.data[i]);
printf("\r\n");
}
void receive_1()
{
CANMessage msg_Local;//create empty CAN message
while(1)
{
CAN_Mutex.lock();
if(can1.read(msg_Local))
{ // if message is available, read Into msg
CAN_Mutex.unlock();
CAN_read_Mutex.lock();
msg_1 = msg_Local;
CAN_read_Mutex.unlock();
// printMsg(msg_Local);
led2 = ! led2; //toggle status LED//printf("send....\r\n");
}
else
{
CAN_Mutex.unlock();
}
Thread::wait(1);
}
}
/////////////////////////////////////////////////////////////////////////////CAN2_read//////////////////////////////////////////////////////////////////////////////////////
void receive_2()
{
CANMessage msg_Local_2;//create empty CAN message
//CANMessage msg_Local;_1
while(1)
{
CAN_1_Mutex.lock();
if(can2.read(msg_Local_2))
{ // if message is available, read Into msg
CAN_1_Mutex.unlock();
CAN_1_read_Mutex.lock();
msg_2 = msg_Local_2;
CAN_1_read_Mutex.unlock();
// printMsg(msg_Local);
// led2 = ! led2; //toggle status LED//printf("send....\r\n");
}
else
{
CAN_1_Mutex.unlock();
}
Thread::wait(1);
}
}
///////////////////////////////////////////////////////////////////////////CAN Write/////////////////////////////////////////////////////////////////////////////////////////////
void send_1()
{
while (true)
{
osEvent evt = queue.get();
if (evt.status == osEventMessage)
{
message_t *message = (message_t*)evt.value.p;
CAN_Mutex.lock();
can1.write(CANMessage(message->can_id, message->can_msg, message->can_len));
CAN_Mutex.unlock();
mpool.free(message);
}
Thread::wait(10);
}
}
////////////////////////////////////////////////////////////////////////CAN2-Send///////////////////////////////////////////////////////////////////////////
void send_2()
{
while (true)
{
osEvent evt = queue.get();
if (evt.status == osEventMessage)
{
message_t *message = (message_t*)evt.value.p;
CAN_1_Mutex.lock();
can2.write(CANMessage(message->can_id, message->can_msg, message->can_len));
CAN_1_Mutex.unlock();
mpool.free(message);
}
Thread::wait(20);
}
}
/////////////////////////////////////////////////////////////////////////////M1//////////////////////////////////////////////////////////////////////////////
void M1_State()
{
CANMessage msg_Local;
int state = STATE_NMT_1;
while(true)
{
//printf("M1\n\r");
CAN_read_Mutex.lock();
msg_Local = msg_1;
CAN_read_Mutex.unlock();
//printf("counter = %d\n\r",counter);
// long int target = xMapper.Calculate(i);
switch(state)
{
case STATE_NMT_1:
{
message_t *message_1 = mpool.alloc();
message_1->can_id = 0x00;
message_1->can_len = 2;
message_1->can_msg[0]= 0x01;
message_1->can_msg[1]=0x01;
queue.put(message_1);
state = STATE_CHECK_NMT_1;
break;
}
case STATE_CHECK_NMT_1:
{
if((msg_Local.id == 0x701) && (msg_Local.data[0]== 0x05))
{
state = STATE_PROFILE_POSITION_1;
}
else
{
state = STATE_NMT_1;
}
break;
}
case STATE_PROFILE_POSITION_1:
{
message_t *message_1 = mpool.alloc();
message_1->can_id = 0x601;
message_1->can_len = 8;
message_1->can_msg[0]= 0x2F;
message_1->can_msg[1]=0x60;
message_1->can_msg[2]=0x60;
message_1->can_msg[3]=0x00;
message_1->can_msg[4]=0x01;
message_1->can_msg[5]=0x00;
message_1->can_msg[6]=0x00;
message_1->can_msg[7]=0x00;
queue.put(message_1);
state = STATE_CHECK_PROFILE_POSITION_1;
break;
}
case STATE_CHECK_PROFILE_POSITION_1:
{
if((msg_Local.id == 0x581) && (msg_Local.data[1]== 0x60)&& (msg_Local.data[2]== 0x60) && (msg_Local.data[4]== 0x00))
{
state = STATE_TARGET_POSITION_1;
}
else
{
state = STATE_PROFILE_POSITION_1;
}
break;
}
case STATE_TARGET_POSITION_1:
{
message_t *message_1 = mpool.alloc();
message_1->can_id = 0x601;
message_1->can_len = 8;
message_1->can_msg[0]= 0x23;
message_1->can_msg[1]=0x7A;
message_1->can_msg[2]=0x60;
message_1->can_msg[3]=0x00;
message_1->can_msg[4]=0xD0;
message_1->can_msg[5]=0x07;
message_1->can_msg[6]=0x00;
message_1->can_msg[7]=0x00;
queue.put(message_1);
state = STATE_CHECH_TARGET_POSITION_1;
break;
}
case STATE_CHECH_TARGET_POSITION_1:
{
message_t *message_1 = mpool.alloc();
message_1->can_id = 0x601;
message_1->can_len = 8;
message_1->can_msg[0]= 0x40;
message_1->can_msg[1]=0x7A;
message_1->can_msg[2]=0x60;
message_1->can_msg[3]=0x00;
message_1->can_msg[4]=0x00;
message_1->can_msg[5]=0x00;
message_1->can_msg[6]=0x00;
message_1->can_msg[7]=0x00;
queue.put(message_1);
if((msg_Local.id == 0x581) && (msg_Local.data[1]== 0x7A)&& (msg_Local.data[2]== 0x60) && (msg_Local.data[4]== 0xD0)&& (msg_Local.data[5]== 0x07))
{
state = STATE_SET_SPEED_1;
}
else
{
state = STATE_TARGET_POSITION_1;
}
break;
}
case STATE_SET_SPEED_1:
{
message_t *message_1 = mpool.alloc();
message_1->can_id = 0x601;
message_1->can_len = 8;
message_1->can_msg[0]= 0x23;
message_1->can_msg[1]=0x81;
message_1->can_msg[2]=0x60;
message_1->can_msg[3]=0x03;
message_1->can_msg[4]=0xC8;
message_1->can_msg[5]=0x00;
message_1->can_msg[6]=0x00;
message_1->can_msg[7]=0x00;
queue.put(message_1);
state = STATE_ENABLE_VOLTAGE_P_1;
break;
}
/* case STATE_CHECK_SPEED_1:
{
message_t *message_1 = mpool.alloc();
message_1->can_id = 0x601;
message_1->can_len = 8;
message_1->can_msg[0]= 0x40;
message_1->can_msg[1]=0x81;
message_1->can_msg[2]=0x60;
message_1->can_msg[3]=0x03;
message_1->can_msg[4]=0x00;
message_1->can_msg[5]=0x00;
message_1->can_msg[6]=0x00;
message_1->can_msg[7]=0x00;
queue.put(message_1);
if((msg_Local.id == 0x581) && (msg_Local.data[1]== 0x81)&& (msg_Local.data[2]== 0x60) && (msg_Local.data[3]== 0x03)&& (msg_Local.data[4]== 0x11))
{
state = STATE_ENABLE_VOLTAGE_P_1;
}
else
{
state = STATE_SET_SPEED_1;
}
break;
}*/
case STATE_ENABLE_VOLTAGE_P_1:
{
message_t *message_1 = mpool.alloc();
message_1->can_id = 0x601;
message_1->can_len = 8;
message_1->can_msg[0]= 0x2B;
message_1->can_msg[1]=0x40;
message_1->can_msg[2]=0x60;
message_1->can_msg[3]=0x00;
message_1->can_msg[4]=0x06;
message_1->can_msg[5]=0x00;
message_1->can_msg[6]=0x00;
message_1->can_msg[7]=0x00;
queue.put(message_1);
state = STATE_CHECK_STATUSWORLD_VOLTAGE_P_1;
break;
}
case STATE_CHECK_STATUSWORLD_VOLTAGE_P_1:
{
message_t *message_1 = mpool.alloc();
message_1->can_id = 0x601;
message_1->can_len = 8;
message_1->can_msg[0]= 0x40;
message_1->can_msg[1]=0x41;
message_1->can_msg[2]=0x60;
message_1->can_msg[3]=0x00;
message_1->can_msg[4]=0x00;
message_1->can_msg[5]=0x00;
message_1->can_msg[6]=0x00;
message_1->can_msg[7]=0x00;
queue.put(message_1);
if((msg_Local.id == 0x581) && (msg_Local.data[1]== 0x41)&& (msg_Local.data[2]== 0x60) &&((msg_Local.data[4] & 0xEF)== 0x21))
{
state = STATE_ENABLE_STATE_P_1;
}
else
{
state = STATE_ENABLE_VOLTAGE_P_1;
}
break;
}
case STATE_ENABLE_STATE_P_1:
{
message_t *message_1 = mpool.alloc();
message_1->can_id = 0x601;
message_1->can_len = 8;
message_1->can_msg[0]= 0x2B;
message_1->can_msg[1]=0x40;
message_1->can_msg[2]=0x60;
message_1->can_msg[3]=0x00;
message_1->can_msg[4]=0x07;
message_1->can_msg[5]=0x00;
message_1->can_msg[6]=0x00;
message_1->can_msg[7]=0x00;
queue.put(message_1);
state = STATE_CHECK_STATUSWORLD_STATE_P_1;
break;
}
case STATE_CHECK_STATUSWORLD_STATE_P_1:
{
message_t *message_1 = mpool.alloc();
message_1->can_id = 0x601;
message_1->can_len = 8;
message_1->can_msg[0]= 0x40;
message_1->can_msg[1]=0x41;
message_1->can_msg[2]=0x60;
message_1->can_msg[3]=0x00;
message_1->can_msg[4]=0x00;
message_1->can_msg[5]=0x00;
message_1->can_msg[6]=0x00;
message_1->can_msg[7]=0x00;
queue.put(message_1);
if((msg_Local.id == 0x581) && (msg_Local.data[1]== 0x41)&& (msg_Local.data[2]== 0x60) &&((msg_Local.data[4] & 0xEF)== 0x23))
{
state = STATE_ENABLE_OPERATION_P_1;
}
else
{
state = STATE_ENABLE_STATE_P_1;
}
break;
}
case STATE_ENABLE_OPERATION_P_1:
{
message_t *message_1 = mpool.alloc();
message_1->can_id = 0x601;
message_1->can_len = 8;
message_1->can_msg[0]= 0x2B;
message_1->can_msg[1]=0x40;
message_1->can_msg[2]=0x60;
message_1->can_msg[3]=0x00;
message_1->can_msg[4]=0x0F;
message_1->can_msg[5]=0x00;
message_1->can_msg[6]=0x00;
message_1->can_msg[7]=0x00;
queue.put(message_1);
state = STATE_CHECK_STATUSWORLD_OPERATION_P_1;
break;
}
case STATE_CHECK_STATUSWORLD_OPERATION_P_1:
{
message_t *message_1 = mpool.alloc();
message_1->can_id = 0x601;
message_1->can_len = 8;
message_1->can_msg[0]= 0x40;
message_1->can_msg[1]=0x41;
message_1->can_msg[2]=0x60;
message_1->can_msg[3]=0x00;
message_1->can_msg[4]=0x00;
message_1->can_msg[5]=0x00;
message_1->can_msg[6]=0x00;
message_1->can_msg[7]=0x00;
queue.put(message_1);
if((msg_Local.id == 0x581) && (msg_Local.data[1]== 0x41)&& (msg_Local.data[2]== 0x60) &&((msg_Local.data[4] & 0xEF)== 0x27))
{
state = STATE_ABSOLUTE_1;
}
else
{
state = STATE_ENABLE_OPERATION_P_1;
}
break;
}
}
Thread::wait(10);
}
}
/////////////////////////////////////////////////////////////////////////////M2//////////////////////////////////////////////////////////////////
void M2_State()
{
CANMessage msg_Local;
int state = STATE_NMT_2;
while(true)
{
//printf("M1\n\r");
CAN_read_Mutex.lock();
msg_Local = msg_2;
CAN_read_Mutex.unlock();
//printf("counter = %d\n\r",counter);
// long int target = xMapper.Calculate(i);
switch(state)
{
case STATE_NMT_2:
{
message_t *message_1 = mpool.alloc();
message_1->can_id = 0x00;
message_1->can_len = 2;
message_1->can_msg[0]= 0x01;
message_1->can_msg[1]=0x02;
queue.put(message_1);
state = STATE_CHECK_NMT_2;
break;
}
case STATE_CHECK_NMT_2:
{
if((msg_Local.id == 0x702) && (msg_Local.data[0]== 0x05))
{
state = STATE_PROFILE_POSITION_2;
}
else
{
state = STATE_NMT_2;
}
break;
}
case STATE_PROFILE_POSITION_2:
{
message_t *message_1 = mpool.alloc();
message_1->can_id = 0x602;
message_1->can_len = 8;
message_1->can_msg[0]= 0x2F;
message_1->can_msg[1]=0x60;
message_1->can_msg[2]=0x60;
message_1->can_msg[3]=0x00;
message_1->can_msg[4]=0x01;
message_1->can_msg[5]=0x00;
message_1->can_msg[6]=0x00;
message_1->can_msg[7]=0x00;
queue.put(message_1);
state = STATE_CHECK_PROFILE_POSITION_2;
break;
}
case STATE_CHECK_PROFILE_POSITION_2:
{
if((msg_Local.id == 0x582) && (msg_Local.data[1]== 0x60)&& (msg_Local.data[2]== 0x60) && (msg_Local.data[4]== 0x00))
{
state = STATE_TARGET_POSITION_2;
}
else
{
state = STATE_PROFILE_POSITION_2 ;
}
break;
}
case STATE_TARGET_POSITION_2:
{
message_t *message_1 = mpool.alloc();
message_1->can_id = 0x602;
message_1->can_len = 8;
message_1->can_msg[0]= 0x23;
message_1->can_msg[1]=0x7A;
message_1->can_msg[2]=0x60;
message_1->can_msg[3]=0x00;
message_1->can_msg[4]=0xD0;
message_1->can_msg[5]=0x07;
message_1->can_msg[6]=0x00;
message_1->can_msg[7]=0x00;
queue.put(message_1);
state = STATE_CHECH_TARGET_POSITION_2;
break;
}
case STATE_CHECH_TARGET_POSITION_2:
{
message_t *message_1 = mpool.alloc();
message_1->can_id = 0x602;
message_1->can_len = 8;
message_1->can_msg[0]= 0x40;
message_1->can_msg[1]=0x7A;
message_1->can_msg[2]=0x60;
message_1->can_msg[3]=0x00;
message_1->can_msg[4]=0x00;
message_1->can_msg[5]=0x00;
message_1->can_msg[6]=0x00;
message_1->can_msg[7]=0x00;
queue.put(message_1);
if((msg_Local.id == 0x582) && (msg_Local.data[1]== 0x7A)&& (msg_Local.data[2]== 0x60) && (msg_Local.data[4]== 0xD0)&& (msg_Local.data[5]== 0x07))
{
state = STATE_SET_SPEED_2;
}
else
{
state = STATE_TARGET_POSITION_2;
}
break;
}
case STATE_SET_SPEED_2:
{
message_t *message_1 = mpool.alloc();
message_1->can_id = 0x602;
message_1->can_len = 8;
message_1->can_msg[0]= 0x23;
message_1->can_msg[1]=0x81;
message_1->can_msg[2]=0x60;
message_1->can_msg[3]=0x03;
message_1->can_msg[4]=0xC8;
message_1->can_msg[5]=0x00;
message_1->can_msg[6]=0x00;
message_1->can_msg[7]=0x00;
queue.put(message_1);
state = STATE_ENABLE_VOLTAGE_P_2;
break;
}
/*case STATE_CHECK_SPEED_2:
{
message_t *message_1 = mpool.alloc();
message_1->can_id = 0x602;
message_1->can_len = 8;
message_1->can_msg[0]= 0x40;
message_1->can_msg[1]=0x81;
message_1->can_msg[2]=0x60;
message_1->can_msg[3]=0x03;
message_1->can_msg[4]=0x00;
message_1->can_msg[5]=0x00;
message_1->can_msg[6]=0x00;
message_1->can_msg[7]=0x00;
queue.put(message_1);
if((msg_Local.id == 0x582) && (msg_Local.data[1]== 0x81)&& (msg_Local.data[2]== 0x60) && (msg_Local.data[3]== 0x03)&& (msg_Local.data[4]== 0x11))
{
state = STATE_ENABLE_VOLTAGE_P_2;
}
else
{
state = STATE_SET_SPEED_2;
}
break;
}*/
case STATE_ENABLE_VOLTAGE_P_2:
{
message_t *message_1 = mpool.alloc();
message_1->can_id = 0x602;
message_1->can_len = 8;
message_1->can_msg[0]= 0x2B;
message_1->can_msg[1]=0x40;
message_1->can_msg[2]=0x60;
message_1->can_msg[3]=0x00;
message_1->can_msg[4]=0x06;
message_1->can_msg[5]=0x00;
message_1->can_msg[6]=0x00;
message_1->can_msg[7]=0x00;
queue.put(message_1);
state = STATE_CHECK_STATUSWORLD_VOLTAGE_P_2;
break;
}
case STATE_CHECK_STATUSWORLD_VOLTAGE_P_2:
{
message_t *message_1 = mpool.alloc();
message_1->can_id = 0x602;
message_1->can_len = 8;
message_1->can_msg[0]= 0x40;
message_1->can_msg[1]=0x41;
message_1->can_msg[2]=0x60;
message_1->can_msg[3]=0x00;
message_1->can_msg[4]=0x00;
message_1->can_msg[5]=0x00;
message_1->can_msg[6]=0x00;
message_1->can_msg[7]=0x00;
queue.put(message_1);
if((msg_Local.id == 0x582) && (msg_Local.data[1]== 0x41)&& (msg_Local.data[2]== 0x60) &&((msg_Local.data[4] & 0xEF)== 0x21))
{
state = STATE_ENABLE_STATE_P_2;
}
else
{
state = STATE_ENABLE_VOLTAGE_P_2;
}
break;
}
case STATE_ENABLE_STATE_P_2:
{
message_t *message_1 = mpool.alloc();
message_1->can_id = 0x602;
message_1->can_len = 8;
message_1->can_msg[0]= 0x2B;
message_1->can_msg[1]=0x40;
message_1->can_msg[2]=0x60;
message_1->can_msg[3]=0x00;
message_1->can_msg[4]=0x07;
message_1->can_msg[5]=0x00;
message_1->can_msg[6]=0x00;
message_1->can_msg[7]=0x00;
queue.put(message_1);
state = STATE_CHECK_STATUSWORLD_STATE_P_2;
break;
}
case STATE_CHECK_STATUSWORLD_STATE_P_2:
{
message_t *message_1 = mpool.alloc();
message_1->can_id = 0x602;
message_1->can_len = 8;
message_1->can_msg[0]= 0x40;
message_1->can_msg[1]=0x41;
message_1->can_msg[2]=0x60;
message_1->can_msg[3]=0x00;
message_1->can_msg[4]=0x00;
message_1->can_msg[5]=0x00;
message_1->can_msg[6]=0x00;
message_1->can_msg[7]=0x00;
queue.put(message_1);
if((msg_Local.id == 0x582) && (msg_Local.data[1]== 0x41)&& (msg_Local.data[2]== 0x60) &&((msg_Local.data[4] & 0xEF)== 0x23))
{
state = STATE_ENABLE_OPERATION_P_2;
}
else
{
state = STATE_ENABLE_STATE_P_2;
}
break;
}
case STATE_ENABLE_OPERATION_P_2:
{
message_t *message_1 = mpool.alloc();
message_1->can_id = 0x602;
message_1->can_len = 8;
message_1->can_msg[0]= 0x2B;
message_1->can_msg[1]=0x40;
message_1->can_msg[2]=0x60;
message_1->can_msg[3]=0x00;
message_1->can_msg[4]=0x0F;
message_1->can_msg[5]=0x00;
message_1->can_msg[6]=0x00;
message_1->can_msg[7]=0x00;
queue.put(message_1);
state = STATE_CHECK_STATUSWORLD_OPERATION_P_2;
break;
}
case STATE_CHECK_STATUSWORLD_OPERATION_P_2:
{
message_t *message_1 = mpool.alloc();
message_1->can_id = 0x602;
message_1->can_len = 8;
message_1->can_msg[0]= 0x40;
message_1->can_msg[1]=0x41;
message_1->can_msg[2]=0x60;
message_1->can_msg[3]=0x00;
message_1->can_msg[4]=0x00;
message_1->can_msg[5]=0x00;
message_1->can_msg[6]=0x00;
message_1->can_msg[7]=0x00;
queue.put(message_1);
if((msg_Local.id == 0x582) && (msg_Local.data[1]== 0x41)&& (msg_Local.data[2]== 0x60) &&((msg_Local.data[4] & 0xEF)== 0x27))
{
state = STATE_ENABLE_OPERATION_P_2;
}
else
{
state = STATE_ENABLE_STATE_P_2;
}
break;
}
}
Thread::wait(5);
}
}
int main() {
// printf("main()\n\r");
CAN_Read_1.start(receive_1);
CAN_Write_1.start(send_1);
CAN_Read_2.start(receive_2);
CAN_Write_2.start(send_2);
M1.start(M1_State);
M2.start(M2_State);
}
Regards
Nada
Hello Nada,
I think the Mutexes around write and read are not necessary because CAN Api is already protected by Mutex in its implementation - CAN.cpp
The Thread::wait(10) of your Send() function is maybe too long, you may control the time in a function where you place the message tothe queue but Send() must run imeadetly. So try to reducing it to 1ms.
I recommended to check if(!queue.empty()) which you did not use. I do not know how it exactly works in background but try check Mbed OS Reference | Queue< T, queue_sz > Class Template Reference
- When the timeout is osWaitForever (default), the function waits infinite time until the message is retrieved.
You can also check if the CAN::write was sucesful or check Bus errors with TD/RDerror .
BR, Jan
Thank you so much Jan for your support and help.
I will try what you told me to do and keep you updated.
Again thank you.
Nada
Hello Jan,
I want to ask if I have to include the CAN library?
I tried to do what you told me , but it didn’t work.
When I tried this simple program which was working before
#include "mbed.h"
DigitalOut led1(LED1);
DigitalOut led2(LED2);
CAN can2(PB_5, PB_6,1000000); // CAN interface
char counter = 0;
void send(void const *args) {
while (true) {
wait_ms(20);
printf("send()\n");
if(can2.write(CANMessage(1337, &counter, 1))) {
printf("wloop()\n");
printf("Message sent: %d\n", counter);
counter++;
}
led1 = !led1;
}
}
int main() {
Thread thread(send);
printf("main()\n");
CANMessage msg;
while(1) {
printf("loop()\n");
if(can2.read(msg)) {
printf("Message received: %d\n", msg.data[0]);
led2 = !led2;
}
wait_ms(1);
}
}
now CAN read not working. but CAN write working good
I tried everything I can but I don’t know if the nucleo has some issue with the CAN bus.
Again please your help and advise.
Regards
Nada
I do not understand, the CAN Api is included via mbed.h
You tried it with two boards or with two CAN interfaces on one board, before. However it is good practice, when you have some doubts move back to basics.
But probably is not so good practice to use any printf with 1ms loop.
BR, Jan
Hello Jan,
I’m trying it with two CAN interfaces on one board.
I will keep trying and I will let you know. I will try with the LPC1768 in case I have an issue with the nucleo board.
Thank you so much Jan.
Nada
I used event queue for both and it’s working. I will keep you updated in case I faced any issue later..