Sera / Mbed 2 deprecated Nucleo_AIRo-4_1_converter

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@0:22b26272c47e, 2020-03-22 (annotated)

Committer:
kakogawa
Date:
Sun Mar 22 11:32:10 2020 +0000
Revision:
0:22b26272c47e
Child:
1:3e0ad6a257c5
Uploaded by A. Kakogawa, 03.22.2020

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kakogawa 0:22b26272c47e 1 // Nucleo_CAN_master for AIRo-4.1
kakogawa 0:22b26272c47e 2 // Created by Atsushi Kakogawa, 2020.03.20
kakogawa 0:22b26272c47e 3 // Modified by ****** ******, 2020.**.**
kakogawa 0:22b26272c47e 4 // Department of Robotics, Ritsumeikan University, Japan
kakogawa 0:22b26272c47e 5
kakogawa 0:22b26272c47e 6 #include "mbed.h"
kakogawa 0:22b26272c47e 7 #include "CAN.h"
kakogawa 0:22b26272c47e 8
kakogawa 0:22b26272c47e 9 DigitalOut led1(LED1);
kakogawa 0:22b26272c47e 10 DigitalOut led2(D8);
kakogawa 0:22b26272c47e 11 DigitalOut led3(D4);
kakogawa 0:22b26272c47e 12 Serial pc(USBTX, USBRX, 115200); // Serial PC
kakogawa 0:22b26272c47e 13
kakogawa 0:22b26272c47e 14 Ticker rx; // timer for data receiver
kakogawa 0:22b26272c47e 15
kakogawa 0:22b26272c47e 16 //cancom;
kakogawa 0:22b26272c47e 17 CAN can(PA_11, PA_12);
kakogawa 0:22b26272c47e 18
kakogawa 0:22b26272c47e 19 // from a master computer
kakogawa 0:22b26272c47e 20 int i=0;
kakogawa 0:22b26272c47e 21 char buf[255];
kakogawa 0:22b26272c47e 22
kakogawa 0:22b26272c47e 23 // converter
kakogawa 0:22b26272c47e 24 int mode, dir;
kakogawa 0:22b26272c47e 25 char tx_data[8];
kakogawa 0:22b26272c47e 26
kakogawa 0:22b26272c47e 27 // Data-receiver
kakogawa 0:22b26272c47e 28 int id = 0;
kakogawa 0:22b26272c47e 29 int flag = 0;
kakogawa 0:22b26272c47e 30
kakogawa 0:22b26272c47e 31 // CAN communication to slaves
kakogawa 0:22b26272c47e 32 void send(int id, int mode, int dir, char targ3, char targ2, char targ1) {
kakogawa 0:22b26272c47e 33 int target_ang = ((int)targ3-0x30)*100+((int)targ2-0x30)*10+(int)targ1-0x30; // Convert from String to Int
kakogawa 0:22b26272c47e 34 char tx_target_U = (target_ang >> 8) & 0xff; // Split the Int data to upper
kakogawa 0:22b26272c47e 35 char tx_target_L = target_ang & 0xff; // Split the Int data to lower
kakogawa 0:22b26272c47e 36 tx_data[0] = id; // ID
kakogawa 0:22b26272c47e 37 tx_data[1] = mode; // mode (0: control, 1: response)
kakogawa 0:22b26272c47e 38 tx_data[2] = dir; // dir (0: positive, 1: negative)
kakogawa 0:22b26272c47e 39 tx_data[3] = tx_target_U; // target value upper 8bit
kakogawa 0:22b26272c47e 40 tx_data[4] = tx_target_L; // target value lower 8bit
kakogawa 0:22b26272c47e 41 tx_data[5] = 0;
kakogawa 0:22b26272c47e 42 tx_data[6] = 0;
kakogawa 0:22b26272c47e 43 tx_data[7] = 0;
kakogawa 0:22b26272c47e 44 can.write(CANMessage(1337, tx_data, 5));
kakogawa 0:22b26272c47e 45 //pc.printf("Sent(0x): %x %x %x %x %x %x %x %x\r\n", tx_data[0], tx_data[1], tx_data[2], tx_data[3], tx_data[4], tx_data[5], tx_data[6], tx_data[7]);
kakogawa 0:22b26272c47e 46 }
kakogawa 0:22b26272c47e 47
kakogawa 0:22b26272c47e 48 void receive() {
kakogawa 0:22b26272c47e 49 CANMessage msg;
kakogawa 0:22b26272c47e 50 // CAN read from slaves and send to a master computer
kakogawa 0:22b26272c47e 51 if(can.read(msg)) {
kakogawa 0:22b26272c47e 52 int rid = msg.data[0];
kakogawa 0:22b26272c47e 53 int data1 = (msg.data[2] << 8) + msg.data[3]; // current
kakogawa 0:22b26272c47e 54 int data2 = (msg.data[4] << 8) + msg.data[5]; // velocity
kakogawa 0:22b26272c47e 55 int data3 = (msg.data[6] << 8) + msg.data[7]; // angle
kakogawa 0:22b26272c47e 56 //pc.printf("%d,%.2f,%.2f,%.2f\r\n", id, data1, data2, data3);
kakogawa 0:22b26272c47e 57 pc.printf("%d,%d,%d,%d\r\n", rid, data1, data2, data3);
kakogawa 0:22b26272c47e 58 }
kakogawa 0:22b26272c47e 59 }
kakogawa 0:22b26272c47e 60
kakogawa 0:22b26272c47e 61 // Data-receiver timer
kakogawa 0:22b26272c47e 62 void RXdata() {
kakogawa 0:22b26272c47e 63 if (flag == 1) {
kakogawa 0:22b26272c47e 64 for (id=1; id<3; id++) {
kakogawa 0:22b26272c47e 65 send(id,1,0,0,0,0); // Receive command
kakogawa 0:22b26272c47e 66 receive();
kakogawa 0:22b26272c47e 67 }
kakogawa 0:22b26272c47e 68 }
kakogawa 0:22b26272c47e 69 }
kakogawa 0:22b26272c47e 70
kakogawa 0:22b26272c47e 71 int main() {
kakogawa 0:22b26272c47e 72
kakogawa 0:22b26272c47e 73 //pc.printf("0,0\r\n");
kakogawa 0:22b26272c47e 74 can.frequency(1000000);
kakogawa 0:22b26272c47e 75 rx.attach(&RXdata, 0.2); // 200 ms
kakogawa 0:22b26272c47e 76
kakogawa 0:22b26272c47e 77 while(1) {
kakogawa 0:22b26272c47e 78
kakogawa 0:22b26272c47e 79 // Serial read from a master computer
kakogawa 0:22b26272c47e 80 if (pc.getc()) {
kakogawa 0:22b26272c47e 81 for (i=1; i<256; i++) {
kakogawa 0:22b26272c47e 82 buf[i] = pc.getc();
kakogawa 0:22b26272c47e 83 if (buf[i] == '\n') {
kakogawa 0:22b26272c47e 84
kakogawa 0:22b26272c47e 85 // ***** Angle control mode *****
kakogawa 0:22b26272c47e 86 // send(ID, mode(0), dir(0 or 1), 3rd digit, 2rd digit, 1rd digit)
kakogawa 0:22b26272c47e 87 if (buf[i-5] == 'A'){
kakogawa 0:22b26272c47e 88 send(buf[i-4]-0x30,0,0,buf[i-3],buf[i-2],buf[i-1]);
kakogawa 0:22b26272c47e 89
kakogawa 0:22b26272c47e 90 // ***** Receive mode (single data) *****
kakogawa 0:22b26272c47e 91 // send(ID, mode(1), dir(0), NC, NC, NC)
kakogawa 0:22b26272c47e 92 } else if (buf[i-5] == 'B') {
kakogawa 0:22b26272c47e 93 send(buf[i-4]-0x30,1,0,0,0,0);
kakogawa 0:22b26272c47e 94 receive();
kakogawa 0:22b26272c47e 95
kakogawa 0:22b26272c47e 96 // ***** Speed control mode (positive) *****
kakogawa 0:22b26272c47e 97 // send(ID, mode(0), dir(0), 3rd digit, 2rd digit, 1rd digit)
kakogawa 0:22b26272c47e 98 } else if (buf[i-5] == 'C') {
kakogawa 0:22b26272c47e 99 send(buf[i-4]-0x30,0,0,buf[i-3],buf[i-2],buf[i-1]);
kakogawa 0:22b26272c47e 100
kakogawa 0:22b26272c47e 101 // ***** Speed control mode (negative) *****
kakogawa 0:22b26272c47e 102 // send(ID, mode(0), dir(1), 3rd digit, 2rd digit, 1rd digit)
kakogawa 0:22b26272c47e 103 } else if (buf[i-5] == 'D') {
kakogawa 0:22b26272c47e 104 send(buf[i-4]-0x30,0,1,buf[i-3],buf[i-2],buf[i-1]);
kakogawa 0:22b26272c47e 105
kakogawa 0:22b26272c47e 106 // ***** Receive mode start (continuous data) *****
kakogawa 0:22b26272c47e 107 // send(ID, mode(1), dir(0), NC, NC, NC)
kakogawa 0:22b26272c47e 108 } else if (buf[i-5] == 'E') {
kakogawa 0:22b26272c47e 109 id = buf[i-4]-0x30;
kakogawa 0:22b26272c47e 110 flag = 1;
kakogawa 0:22b26272c47e 111
kakogawa 0:22b26272c47e 112 // ***** Receive mode stop (continuous data) *****
kakogawa 0:22b26272c47e 113 // send(ID, mode(1), dir(0), NC, NC, NC)
kakogawa 0:22b26272c47e 114 } else if (buf[i-5] == 'F') {
kakogawa 0:22b26272c47e 115 id = 0;
kakogawa 0:22b26272c47e 116 flag = 0;
kakogawa 0:22b26272c47e 117 }
kakogawa 0:22b26272c47e 118 i = 1;
kakogawa 0:22b26272c47e 119 break;
kakogawa 0:22b26272c47e 120 }
kakogawa 0:22b26272c47e 121 }
kakogawa 0:22b26272c47e 122 led1=!led1;
kakogawa 0:22b26272c47e 123 led2=!led2;
kakogawa 0:22b26272c47e 124
kakogawa 0:22b26272c47e 125 //wait (0.001); // Eriminate this "wait" when linux
kakogawa 0:22b26272c47e 126 }
kakogawa 0:22b26272c47e 127
kakogawa 0:22b26272c47e 128 // CAN error check
kakogawa 0:22b26272c47e 129 if(can.rderror()){
kakogawa 0:22b26272c47e 130 pc.printf("rx error,%d\r\n",can.rderror());
kakogawa 0:22b26272c47e 131 can.reset();
kakogawa 0:22b26272c47e 132 led3=1;
kakogawa 0:22b26272c47e 133 wait (0.1);
kakogawa 0:22b26272c47e 134 } else {
kakogawa 0:22b26272c47e 135 led3=0;
kakogawa 0:22b26272c47e 136 }
kakogawa 0:22b26272c47e 137 if(can.tderror()){
kakogawa 0:22b26272c47e 138 pc.printf("tx error,%d\r\n",can.tderror());
kakogawa 0:22b26272c47e 139 can.reset();
kakogawa 0:22b26272c47e 140 led3=1;
kakogawa 0:22b26272c47e 141 wait (0.1);
kakogawa 0:22b26272c47e 142 } else {
kakogawa 0:22b26272c47e 143 led3=0;
kakogawa 0:22b26272c47e 144 }
kakogawa 0:22b26272c47e 145 }
kakogawa 0:22b26272c47e 146 }