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@1:3e0ad6a257c5, 2021-03-05 (annotated)

Committer:
serararai
Date:
Fri Mar 05 08:38:30 2021 +0000
Revision:
1:3e0ad6a257c5
Parent:
0:22b26272c47e 
test

Who changed what in which revision?

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