Yoshimichi Oka
/
yaw_hurihuri
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.
Revision 0:a17be7326244, committed 2021-04-30
- Comitter:
- Y_OKA
- Date:
- Fri Apr 30 05:19:30 2021 +0000
- Commit message:
- To Kobayashi-kun
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Apr 30 05:19:30 2021 +0000
@@ -0,0 +1,158 @@
+// 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
+// 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)
+
+float duty = 0;
+int id = 6;
+int flag = 0;
+int mode=0;
+//float target_ang1 = 165, ang, e = 0, pang, de = 0;
+//float kp=0.01, kd = 0.1;
+
+float thre = 0.25;
+float pre_x = 7.5;
+float x;
+float pre_theta = 165; //IDに依存
+float theta;
+float dx, dtheta;
+float kp = 1;
+
+char tx_data[8];
+char tx_data1_U, tx_data1_L, tx_data2_U, tx_data2_L, tx_data3_U, tx_data3_L;
+
+// PD-control timer
+void controller() {
+ if(mode==3){
+ //x = (1-potensio1.read())*30;
+ dx = x - pre_x;
+ if(kp*abs(dx)>1){
+ DA_crt=1;
+ }else{
+ DA_crt = kp*abs(dx);
+ }
+ theta = 330*(potensio1.read()- 0.01); //yaw joint anbgle
+ dtheta = theta - pre_theta;
+
+ if (abs(dx) < thre){
+ mdir1 = 0;
+ mdir2 = 0;
+ }else{
+ if(dtheta > 0){
+ mdir1 = 1;
+ mdir2 = 1;
+ wait(0.8);
+ }else {
+ mdir1 = 1;
+ mdir2 = 0;
+ wait(0.8);
+ }
+ }
+ pre_x=x;
+ pre_theta=theta;
+ //wait(0.1);
+ /*
+ //パンタ基板からのデータをそのモジュールのヨー軸へ目標値として送る
+ //modeを戻す動作を忘れずに，waitは通信割込みができるように
+ //第二関節はどうするか，時間差？第一ヨー軸の角度変位量の半分を目標値として与え，次に変位量がマイナスになった瞬間の最大変位量になるように目標値を増やす
+ //現在角度が最大値になったら徐々に初期姿勢に戻す
+ //もしくは最初にdisableで突っ込んで，回転したら回転方向に対してスイッチング動作開始→角度が戻ったらその関節を初期姿勢に戻す
+ //必須：ロール動作の確認，配管内突っ張り動作の確認（車輪半径，アーム長さ・角度）．ロボットを横倒しにして目標アーム角度になるまでPI制御できるボタン追加
+ //
+ ang = (potensio1.read()- 0.01)*330;
+ e = target_ang1 - ang;
+ de = ang - pang;
+ DA_crt = kp*abs(e) - kd*abs(de);
+ if (e < 0) {
+ mdir1 = 1;
+ mdir2 = 0;
+ } else if (e > 0) {
+ mdir1 = 1;
+ mdir2 = 1;
+ } else {
+ mdir1 = 0;
+ mdir2 = 0;
+ }
+ pang = ang;
+ */
+ }
+}
+
+int main() {
+
+ //control.attach(&controller, 0.3); // 1 ms
+
+ CAN can(PA_11, PA_12);
+ can.frequency(50000);
+ CANMessage msg;
+
+ while(1) {
+ controller();
+ 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];
+ } 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 = 330*potensio2.read()*100;//AD_vlc.read()*1000;
+ tx_data2_U = (i_data2 >> 8) & 0xff;
+ tx_data2_L = i_data2 & 0xff;
+ int i_data3 = (1-potensio1.read())*30*100;//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));
+ } else if (msg.data[1] == 3) {
+ x = (msg.data[3] << 8) + msg.data[4];
+ mode=3;
+ int i_data1 = AD_crt.read()*1000;
+ tx_data1_U = (i_data1 >> 8) & 0xff;
+ tx_data1_L = i_data1 & 0xff;
+ int i_data2 = 330*potensio2.read()*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);
+ } else {
+ myled = 0; // LED is OFF
+ }
+ } // while
+}
\ No newline at end of file