CAN BUS Data problem

Hello everyone,

I'm new in programming arduino and I'm trying to get some information from a motor with integrated electronic that uses CANBus for sharing informations.

The message that I'm sending is the following one and I'm already getting some answer from the electronic.

WHAT I NEED TO RECEIVE AND OBTAIN AT THE END
Send: 606 8 40 C2 20 00 00 00 00 00
Receive: 586 8 40 C2 20 00 05 7A 09 00
DATA RECEIVED: 05 7A 09 00
This value represent the data received from the motor electronic, it has to manipulated in order to get the following values:
-00 09 7A 05
Than it has to be converted from HEX value to Int Value (I know that are the same thing). I need to convert it from char to int value.

WHAT I RECEIVED
Send: 606 8 40 C2 20 00 00 00 00 00
Receive: 586 8 40 C2 20 00 05 7A 09

The last byte seems to be disappered, I don't know if I'm doing something wrong, but I got the answer to the rquest.

Can you please help me to find the solution and get the int value at the end?

Here below you can see the code for sending and receiving.
Thank you very much for your help,

SENDER + RECEIVER

#include <CAN.h>

//CAN
char CANBuffer[8];
char CANReadBuffer[8];
char StepsBuffer[4];
long idIn=0;
long steps;
char prova;


int StrToHex(char str[])
{
  return (int) strtol(str, 0, 16);
}

void CANsend(long id, char *message, char msgLength) {
  CAN.beginPacket(id);
  for (char i = 0; i < msgLength; i ++) {
    CAN.write(message[i]);
  }
  CAN.endPacket();
}

void setBuffer(char *inBuffer, char d0, char d1, char d2, char d3, char d4, char d5, char d6, char d7) {
  inBuffer[0] = d0;
  inBuffer[1] = d1;
  inBuffer[2] = d2;
  inBuffer[3] = d3;
  inBuffer[4] = d4;
  inBuffer[5] = d5;
  inBuffer[6] = d6;
  inBuffer[7] = d7;
}


void setup() {
  Serial.begin(9600);
  while (!Serial);

  Serial.println("CAN Sender");

  // start the CAN bus at 500 kbps
  if (!CAN.begin(125000)) {
    Serial.println("Starting CAN failed!");
    while (1);
  }
}

void loop() {
  // send packet: id is 11 bits, packet can contain up to 8 bytes of data
  Serial.print("Sending packet ... ");

  setBuffer(CANBuffer, 0x40, 0xC2, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00);
  CANsend(0x606, CANBuffer, 8);

  Serial.println("done");

  delay(1000);

  // try to parse packet
  int packetSize = CAN.parsePacket();

  if (packetSize) {
    // received a packet
    Serial.print("Received ");

//    if (CAN.packetRtr()) {
//      // Remote transmission request, packet contains no data
//      Serial.print("RTR ");
//    }
//
//    Serial.print("packet with id 0x");
//    Serial.print(CAN.packetId(), HEX);
//
//    if (CAN.packetRtr()) {
//      Serial.print(" and requested length ");
//      Serial.println(CAN.packetDlc());
//    } else {
//      Serial.print(" and length ");
//      Serial.println(packetSize);
//
//      // only print packet data for non-RTR packets
      while (CAN.available()) {
        //*CANReadBuffer=(char)CAN.read();
        //CANReadBuffer++;
        //Serial.print((char)CAN.read(),HEX);
      //prova=(char)CAN.read();
      for (char i = 0; i < 10; i ++) {
      CANReadBuffer[i] = CAN.read();
      }
      
      Serial.print(CANReadBuffer[0],HEX);
      Serial.print(CANReadBuffer[1],HEX);
      Serial.print(CANReadBuffer[2],HEX);
      Serial.print(CANReadBuffer[3],HEX);
      Serial.print(CANReadBuffer[4],HEX);
      Serial.print(CANReadBuffer[5],HEX);
      Serial.print(CANReadBuffer[6],HEX);
      Serial.println(CANReadBuffer[7],HEX);

      StepsBuffer[0] = CANReadBuffer[7];
      StepsBuffer[1] = CANReadBuffer[6];
      StepsBuffer[2] = CANReadBuffer[5];
      StepsBuffer[3] = CANReadBuffer[4];

      //StepsBuffer[4] = CANReadBuffer[3];
      //StepsBuffer[5] = CANReadBuffer[2];
      //StepsBuffer[6] = CANReadBuffer[1];
      //StepsBuffer[7] = CANReadBuffer[0];

      steps = StrToHex(StepsBuffer);
      Serial.print(steps);
      }
  }

}

I'm confused...

you are receiving bytes that I assume are RAW binary value and NOT ASCII values, so WHY are you using strol???

if these are the bytes that need to be combined into an int then one way to do this would be some thing like this:

//CANReadBuffer[4] CANReadBuffer[5] CANReadBuffer[6] CANReadBuffer[7]
//    (MSB)                                               (LSB)
//     00                09              7A                05

uint32_t val = 0;

for(int i =4; i<8; ++i){
  val<<=8; //left shift by 8 bits
  val|=CANReadBuffer[i]; //Logical OR operation
}

Serial.println(val,HEX); //would give you 97A05 (the leading zeros are NOT printed)
Serial.println(val,DEC); //would give you 621061

hope that helps...

Hello,

Thank you very much, it works..

I have anther problem, I'm sending some values uint16_t on the serial communication:

Serial.write(timeStamp & 0x00ff);
    Serial.write((timeStamp & 0xff00) >> 8);
    Serial.write('i');
    Serial.write(sense & 0x00ff);
    Serial.write((sense & 0xff00) >> 8);
    Serial.write('v');
    Serial.write(voltage & 0x00ff);
    Serial.write((voltage & 0xff00) >> 8);
    Serial.write('s');
    Serial.write(steps & 0x00ff);
    Serial.write((steps & 0xff00) >> 8);

How can I send a uint_32?

Thank you very much!!

I hope you ACTUALLY understand what ur doing as it should be 'obvious' from the snippet you posted.

from your snippet, for a uint16_t variable:

    Serial.write(voltage & 0x00ff); //<---- first sends LSByte
    Serial.write((voltage & 0xff00) >> 8); //<---- then sends MSByte

for a uint32_t, probably less clunky to use a 'for loop' as in:

for(int i =0; i<4; ++i){ //LSByte sent written out first
  Serial.write(val & 0xff); 
  val>>=8; //right shift by 8 bits
}

please note that the value of 'val' at the end of the 'for loop' execution will be ZERO.

hope that helps....

Hello,

Thank you very much, it works and I got the point!

Now I put everything into a function like this:

void readsteps() {
  for(int k=0; k<10;k++){
  //Serial.println("Sending Message 24..");
  // readstep command
  //flushBuffer(CANBuffer);
  setBuffer(CANBuffer, 0x40, 0xC2, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00);
  CANsend(0x606, CANBuffer, 8);

  delay(1000);

  // try to parse packet
  int packetSize = CAN.parsePacket();
  
  if (packetSize) {

      while (CAN.available()) {
      for (char i = 0; i < 10; i ++) {
      CANReadBuffer[i] = CAN.read();
      }
      
      StepsBuffer[0] = CANReadBuffer[7];
      StepsBuffer[1] = CANReadBuffer[6];
      StepsBuffer[2] = CANReadBuffer[5];
      StepsBuffer[3] = CANReadBuffer[4];

      uint32_t steps= 0;
      
      for(int j =0; j<4; ++j){
        steps<<=8; //left shift by 8 bits
         steps|=StepsBuffer[j]; //Logical OR operation
      }
      
      //Serial.println(val,HEX); //would give you 97A05 (the leading zeros are NOT printed)
      //Serial.println(val,DEC); //would give you 621061
    }
  }
}
}

After that I send the steps value with :

    for(int i =0; i<4; ++i){ //LSByte sent written out first
    Serial.write(steps & 0xff); 
    steps>>=8; //right shift by 8 bits
    }

It doesn't work. I don't know why, from my side nothing change.

Thank you very much for you support and help.

Have a nice day

I'm not familiar with the CAN library ur using but this is my take on what I 'think' ur trying to do:
(compiles, NOT tested!)

//Send: 606 8 40 C2 20 00 00 00 00 00
//Receive: 586 8 40 C2 20 00 05 7A 09 00

#include <mcp_can.h>
#include <SPI.h>

long unsigned int rxId;
unsigned char len = 0;
unsigned char rxBuf[8];
byte txBuf[8] = {0x40, 0xC2, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00};

#define CAN0_INT 2                              //pin 2 connected to MCP2515 INT pin
#define CAN0_CS 10                              //pin 10 connected to MCP2515 CS pin
MCP_CAN CAN0(CAN0_CS);                         //instate MCP_CAN object

void setup()
{
  Serial.begin(115200);

  // Initialize MCP2515 running at 16MHz with a baudrate of 125kb/s and the masks and filters disabled.
  if (CAN0.begin(MCP_ANY, CAN_125KBPS, MCP_16MHZ) == CAN_OK)
    Serial.println("MCP2515 Initialized Successfully!");
  else
    Serial.println("Error Initializing MCP2515...");

  CAN0.setMode(MCP_NORMAL);                     // Set operation mode to normal so the MCP2515 sends acks to received data.

  pinMode(CAN0_INT, INPUT);                            // Configuring pin for /INT input

  Serial.println("Sending first request...!");

  // send data:  ID = 0x606, Standard CAN Frame, Data length = 8 bytes, 'txBuf' = array of data bytes to send
  CAN0.sendMsgBuf(0x606, 0, 8, txBuf);
}

void loop() {
  if (!digitalRead(CAN0_INT))                        // If CAN0_INT pin is low, read receive buffer
  {
    CAN0.readMsgBuf(&rxId, &len, rxBuf);      // Read data: len = data length, rxBuf = data byte(s)

    if (rxId == 0x586) {
      Serial.println("reply received!");
      //print out received frame to serial monitor
      Serial.print("ID: ");
      Serial.print(rxId, HEX);
      Serial.print(", DLC: ");
      Serial.print(len, HEX);
      Serial.print(", data: ");
      for (byte i = 0; i < len; i++) {
        Serial.print(rxBuf[i], HEX);
        Serial.print(" ");
      }
      Serial.println();

      //compute and print out received value to serial monitor
      uint32_t val = 0;
      for (byte i = 4; i < 8; ++i) {
        val <<= 8; //left shift by 8 bits
        val |= rxBuf[i]; //Logical OR operation
      }

      Serial.print("value received: ");
      Serial.println(val, HEX);
      Serial.println(val, DEC);

      /*
        uint32_t temp = val;
        for (int i = 0; i < 4; ++i) { //LSByte sent written out first
        Serial.write(temp & 0xff);
        temp >>= 8; //right shift by 8 bits
        }
      */

      //resend request:
      delay(1000); //arbitrary delay
      CAN0.sendMsgBuf(0x606, 0, 8, txBuf);
    }
  }

}

using this CAN library in this code. also assuming ur using a CAN shield like this one.

hope that helps....

Hello,

The suggestios that you gave me before were perfetc, into a simple code everything works fine, I can send the can message and receive the answer. After that I'm able to convert the char into Int value.
Here below the simple code for tests:

#include <CAN.h>

//CAN
char CANBuffer[8];
char CANReadBuffer[8];
char StepsBuffer[4];
long idIn=0;
long steps;
char prova;

int StrToHex(char str[])
{
  return (int) strtol(str, 0, 16);
}

void CANsend(long id, char *message, char msgLength) {
  CAN.beginPacket(id);
  for (char i = 0; i < msgLength; i ++) {
    CAN.write(message[i]);
  }
  CAN.endPacket();
}

void CANreceive(long *id, char *message) {
  long packetSize = 0;
  //xTaskCreate(CANTimeoutTask, "CAN Timeout Task", 64, NULL, tskIDLE_PRIORITY, &CANTimeoutTaskHandler);
  while (!packetSize) {
    packetSize = CAN.parsePacket();
    delay(1);
  }
  //vTaskDelete(CANTimeoutTaskHandler);
  *id = CAN.packetId();
  while (CAN.available()) {
    *message = CAN.read();
    //Serial.print(*message, HEX);
    //Serial.print(" ");
    message ++;
  }

}

void setBuffer(char *inBuffer, char d0, char d1, char d2, char d3, char d4, char d5, char d6, char d7) {
  inBuffer[0] = d0;
  inBuffer[1] = d1;
  inBuffer[2] = d2;
  inBuffer[3] = d3;
  inBuffer[4] = d4;
  inBuffer[5] = d5;
  inBuffer[6] = d6;
  inBuffer[7] = d7;
}

void setup() {
  Serial.begin(9600);
  while (!Serial);

  Serial.println("CAN Sender");

  // start the CAN bus at 500 kbps
  if (!CAN.begin(125000)) {
    Serial.println("Starting CAN failed!");
    while (1);
  }
}

void loop() {
  // send packet: id is 11 bits, packet can contain up to 8 bytes of data
  Serial.print("Sending packet ... ");

  setBuffer(CANBuffer, 0x40, 0xC2, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00);
  CANsend(0x606, CANBuffer, 8);

  Serial.println("done");

  delay(1000);

  // try to parse packet
  int packetSize = CAN.parsePacket();

  if (packetSize) {
    // received a packet
    Serial.print("Received ");

      while (CAN.available()) {

      for (char i = 0; i < 10; i ++) {
      CANReadBuffer[i] = CAN.read();
      }
      
      Serial.print(CANReadBuffer[0],HEX);
      Serial.print(CANReadBuffer[1],HEX);
      Serial.print(CANReadBuffer[2],HEX);
      Serial.print(CANReadBuffer[3],HEX);
      Serial.print(CANReadBuffer[4],HEX);
      Serial.print(CANReadBuffer[5],HEX);
      Serial.print(CANReadBuffer[6],HEX);
      Serial.println(CANReadBuffer[7],HEX);

      StepsBuffer[0] = CANReadBuffer[7];
      StepsBuffer[1] = CANReadBuffer[6];
      StepsBuffer[2] = CANReadBuffer[5];
      StepsBuffer[3] = CANReadBuffer[4];
      
      uint32_t val = 0;

      for(int i =0; i<4; ++i){
        val<<=8; //left shift by 8 bits
         val|=StepsBuffer[i]; //Logical OR operation
      }
      Serial.println(val,HEX); //would give you 97A05 (the leading zeros are NOT printed)
      Serial.println(val,DEC); //would give you 621061
      }
  }
}

After that I wanted to put this simple code into my main program, and I don't know where is the problem, just for you information the code below works, from my phyton I receive a fixed value like 635876, so I suppose that I have problem in receiveing the message now.

    for(int i =0; i<4; ++i){ //LSByte sent written out first
    Serial.write(steps & 0xff); 
    steps>>=8; //right shift by 8 bits
    }

This below is my main program:

*/
#include <FreeRTOS_SAMD21.h>
#include <CAN.h>

//CAN
#define CAN_TIMEOUT_MS  1000
#define MAX_SPEED       5000


//CAN
char CANBuffer[8];
char CANReadBuffer[8];
char StepsBuffer[4];
bool notReceived = true;
long idIn = 0;
char byte1, byte2;

#define HOMING_TIME     15000
#define SAMPLING_TIME   100
#define BUFFER_L        HOMING_TIME / SAMPLING_TIME

//Acquisitions
uint16_t voltageBuffer[BUFFER_L], senseBuffer[BUFFER_L], bufferPos;
uint16_t sensorValueMedio=0;
uint32_t steps;

static void CANTimeoutTask(void *param) {
  int CANTimeOut = 0;
  while (1) {
    //Serial.print("CAN Timeout Task ");
    //Serial.print(CANTimeOut);
    //Serial.println();
    CANTimeOut ++;
    delay(1);
    if (CANTimeOut > CAN_TIMEOUT_MS) {
      L(RED, 1);
      L(GREEN, 0);
      L(BLUE, 0);
      while (1) {
        //Serial.println("DEBUG: CAN TIMEOUT");
        delay(1000);
      }
    }
  }
}

void CANsend(long id, char *message, char msgLength) {
  CAN.beginPacket(id);
  for (char i = 0; i < msgLength; i ++) {
    CAN.write(message[i]);
  }
  CAN.endPacket();
}

void flushBuffer(char *inBuffer) {
  for (char i = 0; i < 8; i ++) {
    inBuffer[i] = 0;
  }
}

void setBuffer(char *inBuffer, char d0, char d1, char d2, char d3, char d4, char d5, char d6, char d7) {
  inBuffer[0] = d0;
  inBuffer[1] = d1;
  inBuffer[2] = d2;
  inBuffer[3] = d3;
  inBuffer[4] = d4;
  inBuffer[5] = d5;
  inBuffer[6] = d6;
  inBuffer[7] = d7;
}

void readsteps() {
  for(int k=0; k<10;k++){
  //Serial.println("Sending Message 24..");
  // readstep command
  //flushBuffer(CANBuffer);
  setBuffer(CANBuffer, 0x40, 0xC2, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00);
  CANsend(0x606, CANBuffer, 8);

  delay(1000);

  // try to parse packet
  int packetSize = CAN.parsePacket();
  
  if (packetSize) {

      while (CAN.available()) {
      for (char i = 0; i < 10; i ++) {
      CANReadBuffer[i] = CAN.read();
      }
      
      StepsBuffer[0] = CANReadBuffer[7];
      StepsBuffer[1] = CANReadBuffer[6];
      StepsBuffer[2] = CANReadBuffer[5];
      StepsBuffer[3] = CANReadBuffer[4];

      uint32_t val = 0;
      
      for(int j =0; j<4; ++j){
        val<<=8; //left shift by 8 bits
         val|=StepsBuffer[j]; //Logical OR operation
      }

      steps=val;
      
      //Serial.println(val,HEX); //would give you 97A05 (the leading zeros are NOT printed)
      //Serial.println(val,DEC); //would give you 621061
    }
  }
}
}

void setup() {
 
  Serial.begin(9600);
  //while (!Serial);
  //Serial.println("DEBUG: ");
  //Serial.println("DEBUG: ******************************");
  //Serial.println("DEBUG:        Program start         ");
  //Serial.println("DEBUG: ******************************");
  Serial.flush();

  //Serial.println("DEBUG: Initializing CAN");
  CAN.setPins(3, 7);
  // start the CAN bus at 125 kbps
  if (!CAN.begin(125000)) {
    //Serial.println("DEBUG: Starting CAN failed!");
    while (1);
  }
  vTaskStartScheduler();
}

void loop() {

  readsteps();
 
  Serial.println("DEBUG: Homing Completed");
  for (uint16_t i = 0; i < BUFFER_L; i ++) {
    uint16_t timeStamp = i * SAMPLING_TIME;
    uint16_t voltage = voltageBuffer[i];
    uint16_t sense = senseBuffer[i];
    Serial.write('t');
    Serial.write(timeStamp & 0x00ff);
    Serial.write((timeStamp & 0xff00) >> 8);
    Serial.write('i');
    Serial.write(sense & 0x00ff);
    Serial.write((sense & 0xff00) >> 8);
    Serial.write('v');
    Serial.write(voltage & 0x00ff);
    Serial.write((voltage & 0xff00) >> 8);
    Serial.write('s');
    //Serial.write(steps & 0x00ff);
    //Serial.write((steps & 0xffff) >> 16);
    for(int i =0; i<4; ++i){ //LSByte sent written out first
    Serial.write(steps & 0xff); 
    steps>>=8; //right shift by 8 bits
    }
    
    Serial.write('\r');
    Serial.write('\n');
    delay(100);
  }
}

Thank you again for your support!

so if I understand correctly the only issue you have is sending the value back to your PC.

this part of your code seems unnecessarily complicated:

    for (uint16_t i = 0; i < BUFFER_L; i ++) {
    uint16_t timeStamp = i * SAMPLING_TIME;
    uint16_t voltage = voltageBuffer[i];
    uint16_t sense = senseBuffer[i];
    Serial.write('t');
    Serial.write(timeStamp & 0x00ff);
    Serial.write((timeStamp & 0xff00) >> 8);
    Serial.write('i');
    Serial.write(sense & 0x00ff);
    Serial.write((sense & 0xff00) >> 8);
    Serial.write('v');
    Serial.write(voltage & 0x00ff);
    Serial.write((voltage & 0xff00) >> 8);
    Serial.write('s');
    //Serial.write(steps & 0x00ff);
    //Serial.write((steps & 0xffff) >> 16);
    for (int i = 0; i < 4; ++i) { //LSByte sent written out first
      Serial.write(steps & 0xff);
      steps >>= 8; //right shift by 8 bits
    }

    Serial.write('\r');
    Serial.write('\n');

IMHO you could simply have transmitted back all that data with something like this:

  for (uint16_t i = 0; i < BUFFER_L; i ++) {
    char str_buf[36];
    sprintf(str_buf, "t%di%dv%ds%ld\r\n", i * SAMPLING_TIME, senseBuffer[i], voltageBuffer[i], steps);
    Serial.print(str_buf); //write string to serial
  }

you will receive an ASCII string and you dont not even need to worry about splitting your uint16_t and uint32_t then! :wink:

hope that helps

Hello,

No that part works fine, the part that doesn't work is the one of the CAN comunication, it works in the first simple code(I have it in order to test the functionality) and not in the main code (the second one that I put in the previous message) where I need to use the previous code. II don't underatand where is the problem, Can you help me looking to my main code? Wha does it change with respect to the simple code for testing?

Thank you

I don't have your hardware so it is impossible for me to check. However I did notice some differences when you combined your 'simple code' into your 'main code'.

This is my take on it based on this FreeRTOS_SAMD21 example. Please give it a go if you wish.
(did not check if code compiles or test it!)

#include <FreeRTOS_SAMD21.h>
#include <CAN.h>

//**************************************************************************
// Type Defines and Constants
//**************************************************************************

#define  ERROR_LED_PIN  13 //Led Pin: Typical Arduino Board
//#define  ERROR_LED_PIN  2 //Led Pin: samd21 xplained board

#define ERROR_LED_LIGHTUP_STATE  HIGH // the state that makes the led light up on your board, either low or high

// Select the serial port the project should use and communicate over
// Some boards use SerialUSB, some use Serial
//#define SERIAL          SerialUSB //Sparkfun Samd21 Boards
#define SERIAL          Serial //Adafruit, other Samd21 Boards

//CAN
#define CAN_TIMEOUT_MS  1000
#define MAX_SPEED       5000

#define HOMING_TIME     15000
#define SAMPLING_TIME   100
#define SAMPLES        HOMING_TIME / SAMPLING_TIME

//**************************************************************************
// global variables
//**************************************************************************
TaskHandle_t Handle_aTask;
TaskHandle_t Handle_bTask;
TaskHandle_t Handle_monitorTask;

//Acquisitions
uint16_t get_voltage, get_sensor;
uint32_t steps, id;
uint8_t sampling = 0, voltage_ready = 0, sensor_ready = 0, steps_ready = 0;
uint8_t CANSendBuffer[8],  CANReadBuffer[8];

void CANsend(long id, char *message, char msgLength) {
  CAN.beginPacket(id);
  for (char i = 0; i < msgLength; i ++) {
    CAN.write(message[i]);
  }
  CAN.endPacket();
}

void CANreceive(long *id, char *message) {
  long packetSize = 0;
  //xTaskCreate(CANTimeoutTask, "CAN Timeout Task", 64, NULL, tskIDLE_PRIORITY, &CANTimeoutTaskHandler);
  while (!packetSize) {
    packetSize = CAN.parsePacket();
    delay(1);
  }
  //vTaskDelete(CANTimeoutTaskHandler);
  *id = CAN.packetId();
  uint8_t i = 0
  while (CAN.available()) {
    *message = CAN.read();
    //Serial.print(*message, HEX);
    //Serial.print(" ");
    message++;
    if (i++ > 8) break;
  }

}

void setBuffer(char *inBuffer, char d0, char d1, char d2, char d3, char d4, char d5, char d6, char d7) {
  inBuffer[0] = d0;
  inBuffer[1] = d1;
  inBuffer[2] = d2;
  inBuffer[3] = d3;
  inBuffer[4] = d4;
  inBuffer[5] = d5;
  inBuffer[6] = d6;
  inBuffer[7] = d7;
}

//**************************************************************************
// Can use these function for RTOS delays
// Takes into account processor speed
// Use these instead of delay(...) in rtos tasks
//**************************************************************************
void myDelayUs(int us)
{
  vTaskDelay( us / portTICK_PERIOD_US );
}

void myDelayMs(int ms)
{
  vTaskDelay( (ms * 1000) / portTICK_PERIOD_US );
}

void myDelayMsUntil(TickType_t *previousWakeTime, int ms)
{
  vTaskDelayUntil( previousWakeTime, (ms * 1000) / portTICK_PERIOD_US );
}


//*****************************************************************
// Create a thread that reads the sensor every sample period
// this task will run forever
//*****************************************************************
static void threadVoltage( void *pvParameters )
{
  SERIAL.println("thread Voltage: Started");

  while (1)
  {
    if (voltage_ready == 0) {
      get_voltage = analogRead(A2); //change to whatever methold you are reading the voltage
      voltage_ready = 1;
    }
    myDelayMs(SAMPLING_TIME);
  }

}

//*****************************************************************
// Create a thread that reads the voltage every sample period
// this task will run forever
//*****************************************************************
static void threadSensor( void *pvParameters )
{
  SERIAL.println("thread Sensor: Started");

  while (1)
  {
    if (sensor_ready == 0) {
      get_sensor = analogRead(A1); //change to whatever methold you are reading the sensor
      sensor_ready = 1;
    }
    myDelayMs(SAMPLING_TIME);
  }

}

//*****************************************************************
// Create a thread that reads the voltage every sample period
// this task will run forever
//*****************************************************************
static void threadCAN( void *pvParameters )
{
  SERIAL.println("thread CAN: Started");

  while (1)
  {
    if (steps_ready == 0) {
      setBuffer(CANSendBuffer, 0x40, 0xC2, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00);
      CANsend(0x606, CANBuffer, 8);

      CANreceive(&id, CANReadBuffer) //wait to receive reply
      if (id == 0x586) steps_ready = 1;
    }
    myDelayMs(SAMPLING_TIME);
  }

}

void setup()
{

  SERIAL.begin(115200);

  delay(1000); // prevents usb driver crash on startup, do not omit this
  while (!SERIAL) ;  // Wait for serial terminal to open port before starting program

  // start the CAN bus at 125 kbps
  if (!CAN.begin(125000)) {
    //SERIAL.println("DEBUG: Starting CAN failed!");
    while (1);
  }

  SERIAL.println("");
  SERIAL.println("******************************");
  SERIAL.println("        Program start         ");
  SERIAL.println("******************************");
  SERIAL.flush();

  // Set the led the rtos will blink when we have a fatal rtos error
  // RTOS also Needs to know if high/low is the state that turns on the led.
  // Error Blink Codes:
  //    3 blinks - Fatal Rtos Error, something bad happened. Think really hard about what you just changed.
  //    2 blinks - Malloc Failed, Happens when you couldn't create a rtos object.
  //               Probably ran out of heap.
  //    1 blink  - Stack overflow, Task needs more bytes defined for its stack!
  //               Use the taskMonitor thread to help gauge how much more you need
  vSetErrorLed(ERROR_LED_PIN, ERROR_LED_LIGHTUP_STATE);

  // sets the serial port to print errors to when the rtos crashes
  // if this is not set, serial information is not printed by default
  vSetErrorSerial(&SERIAL);

  // Create the threads that will be managed by the rtos
  // Sets the stack size and priority of each task
  // Also initializes a handler pointer to each task, which are important to communicate with and retrieve info from tasks
  xTaskCreate(threadSensor,     "Task Sensor",       256, NULL, tskIDLE_PRIORITY + 3, &Handle_SensorTask);
  xTaskCreate(threadVoltage,     "Task Voltage",       256, NULL, tskIDLE_PRIORITY + 2, &Handle_VoltageTask);
  xTaskCreate(threadSteps, "Task CAN", 256, NULL, tskIDLE_PRIORITY + 1, &Handle_CANTask);

  // Start the RTOS, this function will never return and will schedule the tasks.
  vTaskStartScheduler();

  // error scheduler failed to start
  // should never get here
  while (1)
  {
    SERIAL.println("Scheduler Failed! \n");
    SERIAL.flush();
    delay(1000);
  }

}

//*****************************************************************
// This is now the rtos idle loop
// No rtos blocking functions allowed!
//*****************************************************************
void loop()
{
  if (SAMPLES > sampling++) {
    if (((voltage_ready & sensor_ready)&steps_ready)) {
      steps = 0;
      for (int i = 7; i > 3; --i) {
        steps <<= 8; //left shift by 8 bits
        steps |= CANReadBuffer[i]; //Logical OR operation
      }

      uint16_t timeStamp = sampling * SAMPLING_TIME;
      SERIAL.write('t');
      SERIAL.write(timeStamp & 0x00ff);
      SERIAL.write((timeStamp & 0xff00) >> 8);
      SERIAL.write('i');
      SERIAL.write(get_sense & 0x00ff);
      SERIAL.write((get_sense & 0xff00) >> 8);
      SERIAL.write('v');
      SERIAL.write(get_voltage & 0x00ff);
      SERIAL.write((get_voltage & 0xff00) >> 8);
      SERIAL.write('s');
      for (int i = 0; i < 4; ++i) { //LSByte sent written out first
        SERIAL.write(steps & 0xff);
        steps >>= 8; //right shift by 8 bits
      }
      SERIAL.write('\r');
      SERIAL.write('\n');

      voltage_ready = 0;
      sensor_ready = 0;
      steps_ready = 0
    }
  }
  delay(100);
}


//*****************************************************************

hope that helps (no idea if it even will work! :sweat_smile:)....