7th_DENSOU / Mbed 2 deprecated Right_Arm

9/19

Dependencies:   mbed KondoServoLibrary CalPID AMT21_right_arm ros_lib_melodic MotorController_AbsEC_right_arm

main.cpp@2:aeae129daa37, 2021-06-22 (annotated)

Committer:
oshin1030
Date:
Tue Jun 22 08:35:12 2021 +0000
Revision:
2:aeae129daa37
Parent:
1:b74b31a7df07
Child:
3:ab266b418d90
6/22

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yuki0108 0:cb9f2ec9d902 1 #include "mbed.h"
yuki0108 0:cb9f2ec9d902 2 #include "AMT21.h"
yuki0108 0:cb9f2ec9d902 3 #include "CalPID.h"
yuki0108 0:cb9f2ec9d902 4 #include "MotorController.h"
oshin1030 2:aeae129daa37 5 #include <stdlib.h>
oshin1030 2:aeae129daa37 6 #include <math.h>
yuki0108 0:cb9f2ec9d902 7
yuki0108 0:cb9f2ec9d902 8 #define DELTA_T 0.01 //制御周期
yuki0108 0:cb9f2ec9d902 9 #define DUTY_MAX 0.8 //duty比の最大値
yuki0108 1:b74b31a7df07 10 #define OMEGA_MAX 15 //速度制御を利用した角度制御での角速度の最大値
yuki0108 1:b74b31a7df07 11 #define NUM_DATA 150 //記録データ数
oshin1030 2:aeae129daa37 12 #define L_1 0.416
oshin1030 2:aeae129daa37 13 #define L_2 0.820
yuki0108 0:cb9f2ec9d902 14
yuki0108 0:cb9f2ec9d902 15 #ifndef M_PI
yuki0108 0:cb9f2ec9d902 16 #define M_PI 3.14159265359f
yuki0108 0:cb9f2ec9d902 17 #endif
yuki0108 0:cb9f2ec9d902 18
yuki0108 1:b74b31a7df07 19 Timer timer_loop; //制御周期用
yuki0108 1:b74b31a7df07 20
yuki0108 0:cb9f2ec9d902 21
yuki0108 0:cb9f2ec9d902 22 ///////////////// 宣言部分 //////////////////////
yuki0108 1:b74b31a7df07 23 CalPID speed_pid(0.2,0,0.00455,DELTA_T,DUTY_MAX); //速度制御のPD計算
yuki0108 0:cb9f2ec9d902 24 CalPID angle_duty_pid(0.5,0,0.00056,DELTA_T,DUTY_MAX); //角度制御（duty式）のPD計算
oshin1030 2:aeae129daa37 25 CalPID angle_omega_pid_1(1.0,0,0,DELTA_T,OMEGA_MAX);//角度制御（速度式）のPD計算 原点
oshin1030 2:aeae129daa37 26 CalPID angle_omega_pid_2(1.0,0,0,DELTA_T,OMEGA_MAX);//角度制御（速度式）のPD計算　肘関節
oshin1030 2:aeae129daa37 27 Amt21 ecA(p27,p28,p8); //原点
oshin1030 2:aeae129daa37 28 Amt21 ecB(p13,p14,p15);//肘関節
yuki0108 0:cb9f2ec9d902 29 //　上で宣言したインスタンスをMotorControllerに与える　//
oshin1030 2:aeae129daa37 30 MotorController motorA(p26,p25,DELTA_T,ecA,speed_pid,angle_duty_pid,angle_omega_pid_1);//原点
oshin1030 2:aeae129daa37 31 MotorController motorB(p24,p23,DELTA_T,ecB,speed_pid,angle_duty_pid,angle_omega_pid_2);//肘関節
yuki0108 0:cb9f2ec9d902 32 //////////////////////////////////////////////////////////////////////////
yuki0108 0:cb9f2ec9d902 33
yuki0108 0:cb9f2ec9d902 34 float target_rad=0; //目標角速度（速度制御）
yuki0108 0:cb9f2ec9d902 35 float target_speed=0;//目標角度（角度制御）
yuki0108 0:cb9f2ec9d902 36
yuki0108 0:cb9f2ec9d902 37 void speedControll()
yuki0108 0:cb9f2ec9d902 38 {
oshin1030 2:aeae129daa37 39 motorA.Sc(target_speed); //速度制御関数]
oshin1030 2:aeae129daa37 40 motorB.Sc(target_speed);
oshin1030 2:aeae129daa37 41 }
yuki0108 1:b74b31a7df07 42
yuki0108 0:cb9f2ec9d902 43 void angleControll()
yuki0108 0:cb9f2ec9d902 44 {
oshin1030 2:aeae129daa37 45 //motorA.AcOmega_1(target_rad); //duty式角度制御
oshin1030 2:aeae129daa37 46 //motorB.AcOmega_2(target_rad); //速度式角度制御
oshin1030 2:aeae129daa37 47 }
oshin1030 2:aeae129daa37 48
oshin1030 2:aeae129daa37 49 double function(double x)
oshin1030 2:aeae129daa37 50 {
oshin1030 2:aeae129daa37 51 double y;
oshin1030 2:aeae129daa37 52 y = 0.5;
oshin1030 2:aeae129daa37 53 return y;
oshin1030 2:aeae129daa37 54 }
yuki0108 1:b74b31a7df07 55
oshin1030 2:aeae129daa37 56 double theta1(double x, double y, double angle_1)//原点角度計算
oshin1030 2:aeae129daa37 57 { //(x:目標座標 y:目標座標 angle_1:現在角度)
oshin1030 2:aeae129daa37 58 double theta_1_a, theta_1_b;
oshin1030 2:aeae129daa37 59 double angle_1_PI = angle_1 * M_PI / 180;
oshin1030 2:aeae129daa37 60 theta_1_a = -acos((pow(x, 2) + pow(y, 2) + pow(L_1, 2) - pow(L_2, 2)) / (2 * L_1 * hypot(x, y))) + atan(y / x);
oshin1030 2:aeae129daa37 61 theta_1_b = acos((pow(x, 2) + pow(y, 2) + pow(L_1, 2) - pow(L_2, 2)) / (2 * L_1 * hypot(x, y))) + atan(y / x);
oshin1030 2:aeae129daa37 62 if (fabs(angle_1_PI - theta_1_a) > fabs(angle_1_PI - theta_1_b))
oshin1030 2:aeae129daa37 63 return theta_1_b;
oshin1030 2:aeae129daa37 64 else
oshin1030 2:aeae129daa37 65 {
oshin1030 2:aeae129daa37 66 return theta_1_a;
oshin1030 2:aeae129daa37 67 }
oshin1030 2:aeae129daa37 68 }
oshin1030 2:aeae129daa37 69
oshin1030 2:aeae129daa37 70 double theta2(double x, double y, double theta_1)//肘関節角度計算
oshin1030 2:aeae129daa37 71 {
oshin1030 2:aeae129daa37 72 double theta_2;
oshin1030 2:aeae129daa37 73 theta_2 = atan((y - L_1 * sin(theta_1)) / (x - L_1 * cos(theta_1))) - theta_1;
oshin1030 2:aeae129daa37 74 return theta_2;
yuki0108 0:cb9f2ec9d902 75 }
yuki0108 0:cb9f2ec9d902 76
yuki0108 1:b74b31a7df07 77 //////////////////////////////////////////////////////////////////////////////
yuki0108 0:cb9f2ec9d902 78
yuki0108 1:b74b31a7df07 79 int main () //調整時を想定しているのでデータを取っている。
yuki0108 1:b74b31a7df07 80 {
oshin1030 2:aeae129daa37 81 motorA.setEquation(0.082047,0.020033,-0.08206,0.020087);//原点、速度制御のC値D値
oshin1030 2:aeae129daa37 82 motorB.setEquation(0.096001,0.067528,-0.08753,0.043631);//肘関節、速度制御のC値D値
yuki0108 0:cb9f2ec9d902 83 //////////////////////////////////////////////////////////////////////////////
oshin1030 2:aeae129daa37 84 //int state=0;
oshin1030 2:aeae129daa37 85 int x;
oshin1030 2:aeae129daa37 86 double DegA;
oshin1030 2:aeae129daa37 87 double DegB;
oshin1030 2:aeae129daa37 88 double angleA;
oshin1030 2:aeae129daa37 89 double angleB;
yuki0108 0:cb9f2ec9d902 90 while(1) {
oshin1030 2:aeae129daa37 91 x = 0.5;
oshin1030 2:aeae129daa37 92 ecA.rewriteCount();
oshin1030 2:aeae129daa37 93 ecB.rewriteCount();
oshin1030 2:aeae129daa37 94 DegA = ecA.getDeg();
oshin1030 2:aeae129daa37 95 DegB = ecB.getDeg();
oshin1030 2:aeae129daa37 96 angleA = DegA/3;
oshin1030 2:aeae129daa37 97 angleB = DegB*36/60;
oshin1030 2:aeae129daa37 98 //double target_thetaA = theta1(x,function(x),angleA);
oshin1030 2:aeae129daa37 99 //double target_thetaB = theta2(x,function(x),target_thetaA);
oshin1030 2:aeae129daa37 100 //printf("%f\t%f\t%f\t%f\t%f\r\n",ecA.read(),ecA.getCount(),ecA.getDeg(),ecA.getOmega(),angleA,target_thetaA,target_thetaB);
oshin1030 2:aeae129daa37 101
oshin1030 2:aeae129daa37 102
oshin1030 2:aeae129daa37 103
oshin1030 2:aeae129daa37 104
oshin1030 2:aeae129daa37 105
oshin1030 2:aeae129daa37 106 /*
yuki0108 1:b74b31a7df07 107 if(state==0) {
yuki0108 1:b74b31a7df07 108 if(toggle) {//スタート用のトグルスイッチ。要らない場合はこのif部分の処理をwhileの直前に
yuki0108 1:b74b31a7df07 109 state++;
yuki0108 1:b74b31a7df07 110 target_speed=5; //目標角速度を5rad/sに
yuki0108 1:b74b31a7df07 111 target_rad=(M_PI/6); //目標角度をπ/6（rad）に（単位に注意）
yuki0108 1:b74b31a7df07 112
yuki0108 1:b74b31a7df07 113 timer_loop.reset();
yuki0108 1:b74b31a7df07 114 timer_loop.start();
yuki0108 1:b74b31a7df07 115 }
yuki0108 1:b74b31a7df07 116 } else if(state==1) {
yuki0108 1:b74b31a7df07 117 if(timer_loop.read()>DELTA_T) {//Tickerだと通信と割り込みが干渉してしまうのでタイマーで
yuki0108 1:b74b31a7df07 118 // speedControll();//速度制御
yuki0108 1:b74b31a7df07 119 angleControll();//角度制御
yuki0108 1:b74b31a7df07 120
yuki0108 1:b74b31a7df07 121 timer_loop.reset();
yuki0108 1:b74b31a7df07 122 }
yuki0108 1:b74b31a7df07 123 if(omega_count==NUM_DATA) {//データが集まったら表示
yuki0108 1:b74b31a7df07 124 motor.stop();
yuki0108 1:b74b31a7df07 125 displayData();
yuki0108 1:b74b31a7df07 126 timer_loop.stop();
yuki0108 1:b74b31a7df07 127 state++;
yuki0108 1:b74b31a7df07 128 }
yuki0108 1:b74b31a7df07 129 }
oshin1030 2:aeae129daa37 130 wait_us(10);*/
yuki0108 0:cb9f2ec9d902 131 }
yuki0108 0:cb9f2ec9d902 132 }
yuki0108 0:cb9f2ec9d902 133