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@0:db35d0ca1d41, 2021-03-17 (annotated)

Committer:
serararai
Date:
Wed Mar 17 17:55:31 2021 +0000
Revision:
0:db35d0ca1d41
Child:
1:a9f2d00d7a56
test

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