Diff: main.cpp

Revision:

2:aeae129daa37

Parent:

1:b74b31a7df07

Child:

3:ab266b418d90

--- a/main.cpp	Wed Apr 14 12:19:14 2021 +0000
+++ b/main.cpp	Tue Jun 22 08:35:12 2021 +0000
@@ -2,89 +2,108 @@
 #include "AMT21.h"
 #include "CalPID.h"
 #include "MotorController.h"
+#include <stdlib.h>
+#include <math.h>
 
 #define DELTA_T 0.01    //制御周期
 #define DUTY_MAX 0.8     //duty比の最大値
 #define OMEGA_MAX 15    //速度制御を利用した角度制御での角速度の最大値
 #define NUM_DATA 150    //記録データ数
+#define L_1 0.416
+#define L_2 0.820
 
 #ifndef M_PI
 #define M_PI 3.14159265359f
 #endif
 
-Serial pc(USBTX, USBRX);   //printfも通シリアル通信なのでこれを書いて、pc.printfとしないとと最悪通信と干渉します。
-DigitalIn toggle(p6,PullUp);
-DigitalOut led1(LED1);
-
 Timer timer_loop;  //制御周期用
 
 
 /////////////////               宣言部分                  //////////////////////
 CalPID speed_pid(0.2,0,0.00455,DELTA_T,DUTY_MAX);   //速度制御のPD計算
 CalPID angle_duty_pid(0.5,0,0.00056,DELTA_T,DUTY_MAX);   //角度制御（duty式）のPD計算
-CalPID angle_omega_pid(25.0,0,0.00224,DELTA_T,OMEGA_MAX);//角度制御（速度式）のPD計算
-Amt21 ec(p9,p10,p8); //エンコーダー
+CalPID angle_omega_pid_1(1.0,0,0,DELTA_T,OMEGA_MAX);//角度制御（速度式）のPD計算 原点
+CalPID angle_omega_pid_2(1.0,0,0,DELTA_T,OMEGA_MAX);//角度制御（速度式）のPD計算　肘関節
+Amt21 ecA(p27,p28,p8); //原点
+Amt21 ecB(p13,p14,p15);//肘関節
 //　上で宣言したインスタンスをMotorControllerに与える　//
-MotorController motor(p22,p21,DELTA_T,ec,speed_pid,angle_duty_pid,angle_omega_pid);
+MotorController motorA(p26,p25,DELTA_T,ecA,speed_pid,angle_duty_pid,angle_omega_pid_1);//原点
+MotorController motorB(p24,p23,DELTA_T,ecB,speed_pid,angle_duty_pid,angle_omega_pid_2);//肘関節
 //////////////////////////////////////////////////////////////////////////
-////データ記録まわり/////
-float angle_saved[NUM_DATA]= {};
-int angle_count=0;
-void saveAngle()
-{
-    if(angle_count<NUM_DATA) {
-        angle_saved[angle_count]=ec.getRad();
-        angle_count++;
-    }
-}
-float omega_saved[NUM_DATA]= {};
-int omega_count=0;
-void saveOmega()
-{
-    if(omega_count<NUM_DATA) {
-        omega_saved[omega_count]=ec.getOmega();
-        omega_count++;
-    }
-}
-void displayData()
-{
-    for(int i=0; i<omega_count; i++) {
-        pc.printf("%f\t%f\t%f\r\n",i*DELTA_T,angle_saved[i],omega_saved[i]);
-        wait(0.001);
-    }
-    omega_count=0;
-    angle_count=0;
-}
-////////////////////////////////////////////////////////////////////////////
 
 float target_rad=0;  //目標角速度（速度制御）
 float target_speed=0;//目標角度（角度制御）
 
 void speedControll()
 {
-    motor.Sc(target_speed);       //速度制御関数
+    motorA.Sc(target_speed);       //速度制御関数]
+    motorB.Sc(target_speed);
+}
 
-    saveAngle();//データ記録
-    saveOmega();//データ記録
-}
 void angleControll()
 {
-//    motor.AcDuty(target_rad);    //duty式角度制御
-    motor.AcOmega(target_rad);   //速度式角度制御
+    //motorA.AcOmega_1(target_rad);    //duty式角度制御
+    //motorB.AcOmega_2(target_rad);   //速度式角度制御
+}
+
+double function(double x)
+{
+    double y;
+    y = 0.5;
+    return y;
+}
 
-    saveAngle();//データ記録
-    saveOmega();//データ記録
+double theta1(double x, double y, double angle_1)//原点角度計算
+{          //(x:目標座標 y:目標座標 angle_1:現在角度)
+    double theta_1_a, theta_1_b;
+    double angle_1_PI = angle_1 * M_PI / 180;
+    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);
+    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);
+    if (fabs(angle_1_PI - theta_1_a) > fabs(angle_1_PI - theta_1_b))
+        return theta_1_b;
+    else
+    {
+        return theta_1_a;
+    }
+}
+
+double theta2(double x, double y, double theta_1)//肘関節角度計算
+{
+    double theta_2;
+    theta_2 = atan((y - L_1 * sin(theta_1)) / (x - L_1 * cos(theta_1))) - theta_1;
+    return theta_2;
 }
 
 //////////////////////////////////////////////////////////////////////////////
 
 int main ()     //調整時を想定しているのでデータを取っている。
 {
-    motor.setEquation(0.022639,0.022651,-0.022297,0.026744);//速度制御のC値D値
+    motorA.setEquation(0.082047,0.020033,-0.08206,0.020087);//原点、速度制御のC値D値
+    motorB.setEquation(0.096001,0.067528,-0.08753,0.043631);//肘関節、速度制御のC値D値
 //////////////////////////////////////////////////////////////////////////////
-    int state=0;
-
+    //int state=0;
+    int x;
+    double DegA;
+    double DegB;
+    double angleA;
+    double angleB;
     while(1) {
+        x = 0.5;
+        ecA.rewriteCount();
+        ecB.rewriteCount();
+        DegA = ecA.getDeg();
+        DegB = ecB.getDeg();
+        angleA = DegA/3;
+        angleB = DegB*36/60;
+        //double target_thetaA = theta1(x,function(x),angleA);
+        //double target_thetaB = theta2(x,function(x),target_thetaA);
+        //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);
+        
+        
+        
+        
+        
+        /*
         if(state==0) {
             if(toggle) {//スタート用のトグルスイッチ。要らない場合はこのif部分の処理をwhileの直前に
                 state++;
@@ -108,7 +127,7 @@
                 state++;
             }
         }
-        wait_us(10);
+        wait_us(10);*/
     }
 }