A test for vescs

main.cpp

Committer:
DunkelAmber
Date:
2019-06-14
Revision:
1:9ecda2b969c2
Parent:
0:0da0fd430e20
Child:
2:12431dd6475f

File content as of revision 1:9ecda2b969c2:

 #include "mbed.h" //5.4.7 (144)

const PinName can1rxPins[] = {PA_11};
const PinName can1txPins[] = {PA_12};
const PinName can2rxPins[] = {PB_5};
const PinName can2txPins[] = {PB_6};

uint64_t counter=0;

//CAN1
//0,0=OK 0,1=OK 0,2=OK 1,0=OK 1,1=OK 1,2=OK 2,0=OK 2,1=OK 2,2=OK
//CAN2
//0,0=RX_OK 0,1=OK 1,0=RX_OK 1,1=OK

CAN can1(can1rxPins[0], can1txPins[0]);
CAN can(can2rxPins[0], can2txPins[0]);

  //UART1, Tx, Rx (Debug)
DigitalOut led1(LED1);


Thread sendThread(osPriorityAboveNormal, 2048);
Thread canrx;
CANMessage messageIn;
CANMessage messageOut;

uint64_t cantoi(CANMessage message) {
    int data=0;
    for(int i=message.len-1, k=0; i>=0; i--, k++) {
        data += message.data[i] << (8*k); 
    }
    return data;
    //pose = message.data[0] + (message.data[1] << 8) + (message.data[2] << 16) + (message.data[3] << 24);
}

void itocan(uint64_t data, CANMessage& message) {
    
    for(int i=message.len-1; i>=0; i--) {
        //printf("dataconv:\t% 12llu\n\r", data);
        message.data[i] = data & 0x00000000000000ff;
        data = data>>8; 
    }
}

void canRxIsr()
{
    while(1) {
        if(can.read(messageIn)) {
            led1 = !led1;
            printf("received from %lu\t%llu\n\r", messageIn.id, cantoi(messageIn));
        }
    }
}

//pose = messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24);

void sendMessage()
{
    int status = can.write(messageOut);
    printf("Send status: %d\r\n", status);
}

void sendMessageLoop()
{
    while (true) {
        counter += 1;
        
        messageOut.id = 1000;
        messageOut.format = CANStandard;
        messageOut.data[0]=25;
        messageOut.len = 8;
    
        itocan(counter, messageOut);
        printf("sending couter: %llu\t| CAN data: %llu\n\r", counter, cantoi(messageOut));
        sendMessage();
        osDelay(1000);
    }
}

int main()
{
    
    printf("CAN receive / transmit test.\r\n");
    can.frequency(500000);

    messageOut.id = 1000;
    messageOut.format = CANStandard;
    messageOut.data[0]=24;
    messageOut.len = 8;

    canrx.start(canRxIsr);
      printf("CAN receive / .\r\n");
    sendThread.start(&sendMessageLoop);
      printf("CAN \r\n");

    while (true)
    {
        
        led1 = 0;
        osDelay(1000);
    }
}