Sera
/
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
- Committer:
- yukikobayashi
- Date:
- 2021-06-10
- Revision:
- 1:a9f2d00d7a56
- Parent:
- 0:db35d0ca1d41
File content as of revision 1:a9f2d00d7a56:
// Nucleo_CAN_slave (AIRo-4.1)
// Angle control
// Created by Atsushi Kakogawa, 2019.09.19
// Edited by Yoshimichi Oka, 2020.2.5
// Modified by Atsushi Kakogawa, 2020.03.22
// Modified by Yoshimichi Oka, 2021.06.10
// Department of Robotics, Ritsumeikan University, Japan
#include "mbed.h"
#include "CAN.h"
Ticker control; // timer for control
DigitalOut myled(PF_1); // LED for communication check
DigitalOut mdir1(PA_9); // D1 Motor ON/OFF (HIGH = ON, Changeable on ESCON Studio)
DigitalOut mdir2(PA_10); // D0 Rotational direction (Direction is changeable on ESCON Studio)
AnalogOut DA_vlc(PA_6); // A5 for velocity analog input to ESCON ANI1+ (0 to 1.0)
AnalogOut DA_crt(PA_5); // A3 for current anaglog input to ESCON ANI2+ (0 to 1.0)
AnalogIn potensio1(PA_0);
AnalogIn potensio2(PA_7); // A6 Potentiometer 2 (5V potentiometer)
AnalogIn AD_crt(PA_1); // Analog Signal from ESCON ANO1 (motor velocity, can be changed by ESCON) (0 to 1.0)
AnalogIn AD_vlc(PA_3); // Analog Signal from ESCON ANO2 (motor current, can be changed by ESCON) (0 to 1.0)
CAN can(PA_11, PA_12);
CANMessage msg;
float duty = 0;
int id = 13; //4,6,8,9,11,13, 8,9はAnalog2pinにALPSのPOT
int mode = 1;
int flag = 0;
int cal = 321; //for ID8, ID9
float target_ang1 = 169;//ID4は100, ID6は165, ID8は180, ID9は140, ID11・13は169
float ang, e = 0, pe=0, pang, de = 0;
float dt=0.01;
float kp = 0.01, kd = 0;
//float kp=0.01, kd = 0.1;
int i=0, j=0;
char tx_data[8];
char tx_data1_U, tx_data1_L, tx_data2_U, tx_data2_L, tx_data3_U, tx_data3_L;
void can_reader(){
if(can.read(msg)) {
if (msg.data[0] == id) { // ID indentify
if (msg.data[1] == 0) { // mode indentify (0: control)
target_ang1 = (msg.data[3] << 8) + msg.data[4];
mode = 1;
} else if (msg.data[1] == 2) {
target_ang1 = (msg.data[3] << 8) + msg.data[4];
mode = 2;
} else if (msg.data[1] == 3) {
target_ang1 = (msg.data[3] << 8) + msg.data[4];
mode = 3;
} else if (msg.data[1] == 1) { // mode indentify (1: response)
int i_data1 = AD_crt.read()*1000;
tx_data1_U = (i_data1 >> 8) & 0xff;
tx_data1_L = i_data1 & 0xff;
int i_data2 = (-1*potensio2.read()*330+cal)*100;;//AD_vlc.read()*1000;
tx_data2_U = (i_data2 >> 8) & 0xff;
tx_data2_L = i_data2 & 0xff;
int i_data3 = potensio1.read()*330*100;
tx_data3_U = (i_data3 >> 8) & 0xff;
tx_data3_L = i_data3 & 0xff;
tx_data[0] = id; // ID
tx_data[1] = 1; // mode (1: response)
tx_data[2] = tx_data1_U; // response value1 upper 8bit
tx_data[3] = tx_data1_L; // response value1 lower 8bit
tx_data[4] = tx_data2_U; // response value2 upper 8bit
tx_data[5] = tx_data2_L; // response value2 lower 8bit
tx_data[6] = tx_data3_U; // response value3 upper 8bit
tx_data[7] = tx_data3_L; // response value3 lower 8bit
can.write(CANMessage(1300 + id, tx_data, 8));
}
}
myled = 1; // LED is ON
wait (0.01);
//break;
} else {
myled = 0; // LED is OFF
}
}
// PD-control timer
void controller() {
if (mode == 1){
if (e < 0) {
mdir1 = 1;
mdir2 = 0;
} else if (e > 0) {
mdir1 = 1;
mdir2 = 1;
} else {
mdir1 = 0;
mdir2 = 0;
}
ang = (potensio1.read())*330;
e = target_ang1 - ang;
de = (e - pe)/dt;
//de = ang - pang;
DA_crt = kp*abs(e) + kd*abs(de);
if (DA_crt > 1){
DA_crt = 1;
} else if (DA_crt <= 0){
DA_crt = 0;
}
pe = e;
//pang = ang;
} else if (mode==2){ //脱力制御
/*
for (i=1; i<200; i++) {
can_reader();
if (e < 0) {
mdir1 = 1;
mdir2 = 0;
} else if (e > 0) {
mdir1 = 1;
mdir2 = 1;
} else {
mdir1 = 0;
mdir2 = 0;
}
ang = (potensio1.read())*330;
e = target_ang1 - ang;
de = (e - pe)/dt;
//de = ang - pang;
DA_crt = kp*abs(e) + kd*abs(de);
if (DA_crt > 1){
DA_crt = 1;
} else if (DA_crt <= 0){
DA_crt = 0;
}
pe = e;
wait(0.01);
if (mode == 1){
break;
}
}
*/
//for (j=1; j<120; j++) {
//can_reader();
mdir1 = 0;
//wait(0.01);
/* if (mode == 1){
break;
}
}
*/
//can_reader();
} else if (mode==3){ //角度制御+脱力制御（T字管用）
//if(id == 13){
if (e < 0) {
mdir1 = 1;
mdir2 = 0;
} else if (e > 0) {
mdir1 = 1;
mdir2 = 1;
} else {
mdir1 = 0;
mdir2 = 0;
}
ang = (potensio1.read())*330;
e = target_ang1 - ang;
de = (e - pe)/dt;
DA_crt = kp*abs(e) + kd*abs(de);
if (DA_crt > 1){
DA_crt = 1;
} else if (DA_crt <= 0){
DA_crt = 0;
}
pe = e;
/*
}else {
//mdir1 = 0;
if (e < 0) {
mdir1 = 1;
mdir2 = 0;
} else if (e > 0) {
mdir1 = 1;
mdir2 = 1;
} else {
mdir1 = 0;
mdir2 = 0;
}
ang = (potensio1.read())*330;
e = target_ang1 - ang;
de = (e - pe)/dt;
//de = ang - pang;
DA_crt = kp*abs(e) + kd*abs(de);
if (DA_crt > 1){
DA_crt = 1;
} else if (DA_crt <= 0){
DA_crt = 0;
}
pe = e;
}
*/
}
}
int main() {
control.attach(&controller, dt); // 1 ms
can.frequency(50000);
while(1) {
can_reader();
//control.attach(&controller, dt);
/*
if(can.read(msg)) {
if (msg.data[0] == id) { // ID indentify
if (msg.data[1] == 0) { // mode indentify (0: control)
target_ang1 = (msg.data[3] << 8) + msg.data[4];
mode = 1;
if (mode==1){
control.attach(&controller, dt);
}
} else if (msg.data[1] == 2) {
target_ang1 = (msg.data[3] << 8) + msg.data[4];
mode = 2;
controller();
wait(2);
stop_motion();
} else if (msg.data[1] == 1) { // mode indentify (1: response)
int i_data1 = AD_crt.read()*1000;
tx_data1_U = (i_data1 >> 8) & 0xff;
tx_data1_L = i_data1 & 0xff;
int i_data2 = AD_vlc.read()*1000;
tx_data2_U = (i_data2 >> 8) & 0xff;
tx_data2_L = i_data2 & 0xff;
int i_data3 = potensio1.read()*330*100;
tx_data3_U = (i_data3 >> 8) & 0xff;
tx_data3_L = i_data3 & 0xff;
tx_data[0] = id; // ID
tx_data[1] = 1; // mode (1: response)
tx_data[2] = tx_data1_U; // response value1 upper 8bit
tx_data[3] = tx_data1_L; // response value1 lower 8bit
tx_data[4] = tx_data2_U; // response value2 upper 8bit
tx_data[5] = tx_data2_L; // response value2 lower 8bit
tx_data[6] = tx_data3_U; // response value3 upper 8bit
tx_data[7] = tx_data3_L; // response value3 lower 8bit
can.write(CANMessage(1300 + id, tx_data, 8));
}
}
myled = 1; // LED is ON
wait (0.01);
//break;
} else {
myled = 0; // LED is OFF
}
*/
} // while
}