Sera / Mbed 2 deprecated oka_master2

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

Dependents:   Kaiduka_20210318

main.cpp@1:1f21cff06302, 2021-03-17 (annotated)

Committer:
serararai
Date:
Wed Mar 17 17:54:54 2021 +0000
Revision:
1:1f21cff06302
Parent:
0:4143c439ad0e
Child:
2:f10d06f31141
test

Who changed what in which revision?

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