Sera / Mbed 2 deprecated yaw_joint_control_ver2

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:a9f2d00d7a56, 2021-06-10 (annotated)

Committer:
yukikobayashi
Date:
Thu Jun 10 06:14:49 2021 +0000
Revision:
1:a9f2d00d7a56
Parent:
0:db35d0ca1d41 
20210610

Who changed what in which revision?

UserRevisionLine numberNew contents of line
serararai 0:db35d0ca1d41 1 // Nucleo_CAN_slave (AIRo-4.1)
serararai 0:db35d0ca1d41 2 // Angle control
serararai 0:db35d0ca1d41 3 // Created by Atsushi Kakogawa, 2019.09.19
serararai 0:db35d0ca1d41 4 // Edited by Yoshimichi Oka, 2020.2.5
serararai 0:db35d0ca1d41 5 // Modified by Atsushi Kakogawa, 2020.03.22
yukikobayashi 1:a9f2d00d7a56 6 // Modified by Yoshimichi Oka, 2021.06.10
serararai 0:db35d0ca1d41 7 // Department of Robotics, Ritsumeikan University, Japan
serararai 0:db35d0ca1d41 8 #include "mbed.h"
serararai 0:db35d0ca1d41 9 #include "CAN.h"
serararai 0:db35d0ca1d41 10
serararai 0:db35d0ca1d41 11 Ticker control; // timer for control
serararai 0:db35d0ca1d41 12 DigitalOut myled(PF_1); // LED for communication check
serararai 0:db35d0ca1d41 13 DigitalOut mdir1(PA_9); // D1 Motor ON/OFF (HIGH = ON, Changeable on ESCON Studio)
serararai 0:db35d0ca1d41 14 DigitalOut mdir2(PA_10); // D0 Rotational direction (Direction is changeable on ESCON Studio)
serararai 0:db35d0ca1d41 15 AnalogOut DA_vlc(PA_6); // A5 for velocity analog input to ESCON ANI1+ (0 to 1.0)
serararai 0:db35d0ca1d41 16 AnalogOut DA_crt(PA_5); // A3 for current anaglog input to ESCON ANI2+ (0 to 1.0)
serararai 0:db35d0ca1d41 17 AnalogIn potensio1(PA_0);
serararai 0:db35d0ca1d41 18 AnalogIn potensio2(PA_7); // A6 Potentiometer 2 (5V potentiometer)
serararai 0:db35d0ca1d41 19 AnalogIn AD_crt(PA_1); // Analog Signal from ESCON ANO1 (motor velocity, can be changed by ESCON) (0 to 1.0)
serararai 0:db35d0ca1d41 20 AnalogIn AD_vlc(PA_3); // Analog Signal from ESCON ANO2 (motor current, can be changed by ESCON) (0 to 1.0)
serararai 0:db35d0ca1d41 21
serararai 0:db35d0ca1d41 22 CAN can(PA_11, PA_12);
serararai 0:db35d0ca1d41 23 CANMessage msg;
serararai 0:db35d0ca1d41 24
serararai 0:db35d0ca1d41 25
serararai 0:db35d0ca1d41 26 float duty = 0;
serararai 0:db35d0ca1d41 27 int id = 13; //4,6,8,9,11,13, 8,9はAnalog2pinにALPSのPOT
serararai 0:db35d0ca1d41 28 int mode = 1;
serararai 0:db35d0ca1d41 29 int flag = 0;
serararai 0:db35d0ca1d41 30 int cal = 321; //for ID8, ID9
serararai 0:db35d0ca1d41 31 float target_ang1 = 169;//ID4は100, ID6は165, ID8は180, ID9は140, ID11・13は169
serararai 0:db35d0ca1d41 32 float ang, e = 0, pe=0, pang, de = 0;
serararai 0:db35d0ca1d41 33 float dt=0.01;
serararai 0:db35d0ca1d41 34 float kp = 0.01, kd = 0;
serararai 0:db35d0ca1d41 35 //float kp=0.01, kd = 0.1;
serararai 0:db35d0ca1d41 36
serararai 0:db35d0ca1d41 37 int i=0, j=0;
serararai 0:db35d0ca1d41 38
serararai 0:db35d0ca1d41 39 char tx_data[8];
serararai 0:db35d0ca1d41 40 char tx_data1_U, tx_data1_L, tx_data2_U, tx_data2_L, tx_data3_U, tx_data3_L;
serararai 0:db35d0ca1d41 41
serararai 0:db35d0ca1d41 42 void can_reader(){
serararai 0:db35d0ca1d41 43 if(can.read(msg)) {
serararai 0:db35d0ca1d41 44 if (msg.data[0] == id) { // ID indentify
serararai 0:db35d0ca1d41 45 if (msg.data[1] == 0) { // mode indentify (0: control)
serararai 0:db35d0ca1d41 46 target_ang1 = (msg.data[3] << 8) + msg.data[4];
serararai 0:db35d0ca1d41 47 mode = 1;
serararai 0:db35d0ca1d41 48 } else if (msg.data[1] == 2) {
serararai 0:db35d0ca1d41 49 target_ang1 = (msg.data[3] << 8) + msg.data[4];
serararai 0:db35d0ca1d41 50 mode = 2;
yukikobayashi 1:a9f2d00d7a56 51 } else if (msg.data[1] == 3) {
yukikobayashi 1:a9f2d00d7a56 52 target_ang1 = (msg.data[3] << 8) + msg.data[4];
yukikobayashi 1:a9f2d00d7a56 53 mode = 3;
serararai 0:db35d0ca1d41 54 } else if (msg.data[1] == 1) { // mode indentify (1: response)
serararai 0:db35d0ca1d41 55 int i_data1 = AD_crt.read()*1000;
serararai 0:db35d0ca1d41 56 tx_data1_U = (i_data1 >> 8) & 0xff;
serararai 0:db35d0ca1d41 57 tx_data1_L = i_data1 & 0xff;
serararai 0:db35d0ca1d41 58 int i_data2 = (-1*potensio2.read()*330+cal)*100;;//AD_vlc.read()*1000;
serararai 0:db35d0ca1d41 59 tx_data2_U = (i_data2 >> 8) & 0xff;
serararai 0:db35d0ca1d41 60 tx_data2_L = i_data2 & 0xff;
serararai 0:db35d0ca1d41 61 int i_data3 = potensio1.read()*330*100;
serararai 0:db35d0ca1d41 62 tx_data3_U = (i_data3 >> 8) & 0xff;
serararai 0:db35d0ca1d41 63 tx_data3_L = i_data3 & 0xff;
serararai 0:db35d0ca1d41 64 tx_data[0] = id; // ID
serararai 0:db35d0ca1d41 65 tx_data[1] = 1; // mode (1: response)
serararai 0:db35d0ca1d41 66 tx_data[2] = tx_data1_U; // response value1 upper 8bit
serararai 0:db35d0ca1d41 67 tx_data[3] = tx_data1_L; // response value1 lower 8bit
serararai 0:db35d0ca1d41 68 tx_data[4] = tx_data2_U; // response value2 upper 8bit
serararai 0:db35d0ca1d41 69 tx_data[5] = tx_data2_L; // response value2 lower 8bit
serararai 0:db35d0ca1d41 70 tx_data[6] = tx_data3_U; // response value3 upper 8bit
serararai 0:db35d0ca1d41 71 tx_data[7] = tx_data3_L; // response value3 lower 8bit
serararai 0:db35d0ca1d41 72 can.write(CANMessage(1300 + id, tx_data, 8));
serararai 0:db35d0ca1d41 73 }
serararai 0:db35d0ca1d41 74 }
serararai 0:db35d0ca1d41 75 myled = 1; // LED is ON
serararai 0:db35d0ca1d41 76 wait (0.01);
serararai 0:db35d0ca1d41 77 //break;
serararai 0:db35d0ca1d41 78 } else {
serararai 0:db35d0ca1d41 79 myled = 0; // LED is OFF
serararai 0:db35d0ca1d41 80 }
serararai 0:db35d0ca1d41 81 }
serararai 0:db35d0ca1d41 82
serararai 0:db35d0ca1d41 83 // PD-control timer
serararai 0:db35d0ca1d41 84 void controller() {
serararai 0:db35d0ca1d41 85 if (mode == 1){
serararai 0:db35d0ca1d41 86 if (e < 0) {
serararai 0:db35d0ca1d41 87 mdir1 = 1;
serararai 0:db35d0ca1d41 88 mdir2 = 0;
serararai 0:db35d0ca1d41 89 } else if (e > 0) {
serararai 0:db35d0ca1d41 90 mdir1 = 1;
serararai 0:db35d0ca1d41 91 mdir2 = 1;
serararai 0:db35d0ca1d41 92 } else {
serararai 0:db35d0ca1d41 93 mdir1 = 0;
serararai 0:db35d0ca1d41 94 mdir2 = 0;
serararai 0:db35d0ca1d41 95 }
serararai 0:db35d0ca1d41 96
serararai 0:db35d0ca1d41 97 ang = (potensio1.read())*330;
serararai 0:db35d0ca1d41 98 e = target_ang1 - ang;
serararai 0:db35d0ca1d41 99 de = (e - pe)/dt;
serararai 0:db35d0ca1d41 100 //de = ang - pang;
serararai 0:db35d0ca1d41 101 DA_crt = kp*abs(e) + kd*abs(de);
serararai 0:db35d0ca1d41 102 if (DA_crt > 1){
serararai 0:db35d0ca1d41 103 DA_crt = 1;
serararai 0:db35d0ca1d41 104 } else if (DA_crt <= 0){
serararai 0:db35d0ca1d41 105 DA_crt = 0;
serararai 0:db35d0ca1d41 106 }
serararai 0:db35d0ca1d41 107
serararai 0:db35d0ca1d41 108 pe = e;
serararai 0:db35d0ca1d41 109 //pang = ang;
serararai 0:db35d0ca1d41 110
yukikobayashi 1:a9f2d00d7a56 111 } else if (mode==2){ //脱力制御
serararai 0:db35d0ca1d41 112 /*
serararai 0:db35d0ca1d41 113 for (i=1; i<200; i++) {
serararai 0:db35d0ca1d41 114 can_reader();
serararai 0:db35d0ca1d41 115 if (e < 0) {
serararai 0:db35d0ca1d41 116 mdir1 = 1;
serararai 0:db35d0ca1d41 117 mdir2 = 0;
serararai 0:db35d0ca1d41 118 } else if (e > 0) {
serararai 0:db35d0ca1d41 119 mdir1 = 1;
serararai 0:db35d0ca1d41 120 mdir2 = 1;
serararai 0:db35d0ca1d41 121 } else {
serararai 0:db35d0ca1d41 122 mdir1 = 0;
serararai 0:db35d0ca1d41 123 mdir2 = 0;
serararai 0:db35d0ca1d41 124 }
serararai 0:db35d0ca1d41 125
serararai 0:db35d0ca1d41 126 ang = (potensio1.read())*330;
serararai 0:db35d0ca1d41 127 e = target_ang1 - ang;
serararai 0:db35d0ca1d41 128 de = (e - pe)/dt;
serararai 0:db35d0ca1d41 129 //de = ang - pang;
serararai 0:db35d0ca1d41 130 DA_crt = kp*abs(e) + kd*abs(de);
serararai 0:db35d0ca1d41 131 if (DA_crt > 1){
serararai 0:db35d0ca1d41 132 DA_crt = 1;
serararai 0:db35d0ca1d41 133 } else if (DA_crt <= 0){
serararai 0:db35d0ca1d41 134 DA_crt = 0;
serararai 0:db35d0ca1d41 135 }
serararai 0:db35d0ca1d41 136
serararai 0:db35d0ca1d41 137 pe = e;
serararai 0:db35d0ca1d41 138 wait(0.01);
serararai 0:db35d0ca1d41 139 if (mode == 1){
serararai 0:db35d0ca1d41 140 break;
serararai 0:db35d0ca1d41 141 }
serararai 0:db35d0ca1d41 142 }
serararai 0:db35d0ca1d41 143 */
serararai 0:db35d0ca1d41 144 //for (j=1; j<120; j++) {
serararai 0:db35d0ca1d41 145 //can_reader();
serararai 0:db35d0ca1d41 146 mdir1 = 0;
serararai 0:db35d0ca1d41 147 //wait(0.01);
serararai 0:db35d0ca1d41 148 /* if (mode == 1){
serararai 0:db35d0ca1d41 149 break;
serararai 0:db35d0ca1d41 150 }
serararai 0:db35d0ca1d41 151 }
serararai 0:db35d0ca1d41 152 */
serararai 0:db35d0ca1d41 153 //can_reader();
yukikobayashi 1:a9f2d00d7a56 154 } else if (mode==3){ //角度制御+脱力制御（T字管用）
yukikobayashi 1:a9f2d00d7a56 155 //if(id == 13){
yukikobayashi 1:a9f2d00d7a56 156 if (e < 0) {
yukikobayashi 1:a9f2d00d7a56 157 mdir1 = 1;
yukikobayashi 1:a9f2d00d7a56 158 mdir2 = 0;
yukikobayashi 1:a9f2d00d7a56 159 } else if (e > 0) {
yukikobayashi 1:a9f2d00d7a56 160 mdir1 = 1;
yukikobayashi 1:a9f2d00d7a56 161 mdir2 = 1;
yukikobayashi 1:a9f2d00d7a56 162 } else {
yukikobayashi 1:a9f2d00d7a56 163 mdir1 = 0;
yukikobayashi 1:a9f2d00d7a56 164 mdir2 = 0;
yukikobayashi 1:a9f2d00d7a56 165 }
yukikobayashi 1:a9f2d00d7a56 166
yukikobayashi 1:a9f2d00d7a56 167 ang = (potensio1.read())*330;
yukikobayashi 1:a9f2d00d7a56 168 e = target_ang1 - ang;
yukikobayashi 1:a9f2d00d7a56 169 de = (e - pe)/dt;
yukikobayashi 1:a9f2d00d7a56 170
yukikobayashi 1:a9f2d00d7a56 171 DA_crt = kp*abs(e) + kd*abs(de);
yukikobayashi 1:a9f2d00d7a56 172 if (DA_crt > 1){
yukikobayashi 1:a9f2d00d7a56 173 DA_crt = 1;
yukikobayashi 1:a9f2d00d7a56 174 } else if (DA_crt <= 0){
yukikobayashi 1:a9f2d00d7a56 175 DA_crt = 0;
yukikobayashi 1:a9f2d00d7a56 176 }
yukikobayashi 1:a9f2d00d7a56 177 pe = e;
yukikobayashi 1:a9f2d00d7a56 178 /*
yukikobayashi 1:a9f2d00d7a56 179 }else {
yukikobayashi 1:a9f2d00d7a56 180 //mdir1 = 0;
yukikobayashi 1:a9f2d00d7a56 181 if (e < 0) {
yukikobayashi 1:a9f2d00d7a56 182 mdir1 = 1;
yukikobayashi 1:a9f2d00d7a56 183 mdir2 = 0;
yukikobayashi 1:a9f2d00d7a56 184 } else if (e > 0) {
yukikobayashi 1:a9f2d00d7a56 185 mdir1 = 1;
yukikobayashi 1:a9f2d00d7a56 186 mdir2 = 1;
yukikobayashi 1:a9f2d00d7a56 187 } else {
yukikobayashi 1:a9f2d00d7a56 188 mdir1 = 0;
yukikobayashi 1:a9f2d00d7a56 189 mdir2 = 0;
yukikobayashi 1:a9f2d00d7a56 190 }
yukikobayashi 1:a9f2d00d7a56 191
yukikobayashi 1:a9f2d00d7a56 192 ang = (potensio1.read())*330;
yukikobayashi 1:a9f2d00d7a56 193 e = target_ang1 - ang;
yukikobayashi 1:a9f2d00d7a56 194 de = (e - pe)/dt;
yukikobayashi 1:a9f2d00d7a56 195 //de = ang - pang;
yukikobayashi 1:a9f2d00d7a56 196 DA_crt = kp*abs(e) + kd*abs(de);
yukikobayashi 1:a9f2d00d7a56 197 if (DA_crt > 1){
yukikobayashi 1:a9f2d00d7a56 198 DA_crt = 1;
yukikobayashi 1:a9f2d00d7a56 199 } else if (DA_crt <= 0){
yukikobayashi 1:a9f2d00d7a56 200 DA_crt = 0;
yukikobayashi 1:a9f2d00d7a56 201 }
yukikobayashi 1:a9f2d00d7a56 202
yukikobayashi 1:a9f2d00d7a56 203 pe = e;
yukikobayashi 1:a9f2d00d7a56 204
yukikobayashi 1:a9f2d00d7a56 205
yukikobayashi 1:a9f2d00d7a56 206 }
yukikobayashi 1:a9f2d00d7a56 207 */
yukikobayashi 1:a9f2d00d7a56 208 }
serararai 0:db35d0ca1d41 209 }
serararai 0:db35d0ca1d41 210
serararai 0:db35d0ca1d41 211 int main() {
serararai 0:db35d0ca1d41 212
serararai 0:db35d0ca1d41 213 control.attach(&controller, dt); // 1 ms
serararai 0:db35d0ca1d41 214
serararai 0:db35d0ca1d41 215 can.frequency(50000);
serararai 0:db35d0ca1d41 216
serararai 0:db35d0ca1d41 217 while(1) {
serararai 0:db35d0ca1d41 218 can_reader();
serararai 0:db35d0ca1d41 219 //control.attach(&controller, dt);
serararai 0:db35d0ca1d41 220 /*
serararai 0:db35d0ca1d41 221 if(can.read(msg)) {
serararai 0:db35d0ca1d41 222 if (msg.data[0] == id) { // ID indentify
serararai 0:db35d0ca1d41 223 if (msg.data[1] == 0) { // mode indentify (0: control)
serararai 0:db35d0ca1d41 224 target_ang1 = (msg.data[3] << 8) + msg.data[4];
serararai 0:db35d0ca1d41 225 mode = 1;
serararai 0:db35d0ca1d41 226 if (mode==1){
serararai 0:db35d0ca1d41 227 control.attach(&controller, dt);
serararai 0:db35d0ca1d41 228 }
serararai 0:db35d0ca1d41 229 } else if (msg.data[1] == 2) {
serararai 0:db35d0ca1d41 230 target_ang1 = (msg.data[3] << 8) + msg.data[4];
serararai 0:db35d0ca1d41 231 mode = 2;
serararai 0:db35d0ca1d41 232 controller();
serararai 0:db35d0ca1d41 233 wait(2);
serararai 0:db35d0ca1d41 234 stop_motion();
serararai 0:db35d0ca1d41 235 } else if (msg.data[1] == 1) { // mode indentify (1: response)
serararai 0:db35d0ca1d41 236 int i_data1 = AD_crt.read()*1000;
serararai 0:db35d0ca1d41 237 tx_data1_U = (i_data1 >> 8) & 0xff;
serararai 0:db35d0ca1d41 238 tx_data1_L = i_data1 & 0xff;
serararai 0:db35d0ca1d41 239 int i_data2 = AD_vlc.read()*1000;
serararai 0:db35d0ca1d41 240 tx_data2_U = (i_data2 >> 8) & 0xff;
serararai 0:db35d0ca1d41 241 tx_data2_L = i_data2 & 0xff;
serararai 0:db35d0ca1d41 242 int i_data3 = potensio1.read()*330*100;
serararai 0:db35d0ca1d41 243 tx_data3_U = (i_data3 >> 8) & 0xff;
serararai 0:db35d0ca1d41 244 tx_data3_L = i_data3 & 0xff;
serararai 0:db35d0ca1d41 245 tx_data[0] = id; // ID
serararai 0:db35d0ca1d41 246 tx_data[1] = 1; // mode (1: response)
serararai 0:db35d0ca1d41 247 tx_data[2] = tx_data1_U; // response value1 upper 8bit
serararai 0:db35d0ca1d41 248 tx_data[3] = tx_data1_L; // response value1 lower 8bit
serararai 0:db35d0ca1d41 249 tx_data[4] = tx_data2_U; // response value2 upper 8bit
serararai 0:db35d0ca1d41 250 tx_data[5] = tx_data2_L; // response value2 lower 8bit
serararai 0:db35d0ca1d41 251 tx_data[6] = tx_data3_U; // response value3 upper 8bit
serararai 0:db35d0ca1d41 252 tx_data[7] = tx_data3_L; // response value3 lower 8bit
serararai 0:db35d0ca1d41 253 can.write(CANMessage(1300 + id, tx_data, 8));
serararai 0:db35d0ca1d41 254 }
serararai 0:db35d0ca1d41 255 }
serararai 0:db35d0ca1d41 256 myled = 1; // LED is ON
serararai 0:db35d0ca1d41 257 wait (0.01);
serararai 0:db35d0ca1d41 258 //break;
serararai 0:db35d0ca1d41 259 } else {
serararai 0:db35d0ca1d41 260 myled = 0; // LED is OFF
serararai 0:db35d0ca1d41 261 }
serararai 0:db35d0ca1d41 262 */
serararai 0:db35d0ca1d41 263 } // while
serararai 0:db35d0ca1d41 264 }