Revision 1:5a65fc20f8c2, committed 2022-01-26

Comitter:

e5119053f6

Date:

Wed Jan 26 11:21:36 2022 +0000

Parent:

0:5e4f1e288e2a

Child:

2:f206311600ee

Commit message:

added the "DDSScontrol"

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DDSScontrol.cpp	Wed Jan 26 11:21:36 2022 +0000
@@ -0,0 +1,66 @@
+#include "mbed.h"
+#include "define.h"
+#include "DDSScontrol.h"
+#include "math.h"
+
+DDSS::DDSS(PinName _pwm_a,PinName _pwm_b,PinName _dir_a,PinName _dir_b) : pwm_a(_pwm_a),pwm_b(_pwm_b),dir_a(_dir_a),dir_b(_dir_b)//コンストラクタ ピン番号を引数にする
+{
+    pwm_a.write(0.0);
+    pwm_b.write(0.0);
+    dir_a.write(0);
+    dir_b.write(0);
+}
+
+void DDSS::setposi(double _Wp_x,double _Wp_y)//操舵点の座標を渡す
+{
+    Wp_x = _Wp_x;
+    Wp_y = _Wp_y;
+}
+
+void DDSS::control(double _refVx,double _refVy,double _refVz,double _stear_angle)//実際に制御を行う
+{
+    refVx = _refVx;
+    refVy = _refVy;
+    refVz = _refVz;
+    stear_angle = _stear_angle;
+
+    V_x = refVx - Wp_y * refVz;
+    V_y = refVy + Wp_x * refVz;
+
+    omega_wheel = (V_x * cos(stear_angle) + V_y * sin(stear_angle)) / WHEEL_R;//操舵角速度
+    omega_stear = (-V_x * sin(stear_angle) + V_y * cos(stear_angle)) / OFFSET;//走行角速度
+
+    omega_a = GEAR_RATIO_TRANS * omega_wheel - GEAR_RATIO_TRANS * omega_stear;//モーターAの角速度
+    omega_b = GEAR_RATIO_TRANS * omega_wheel + GEAR_RATIO_TRANS * omega_stear;//モーターBの角速度
+
+    rpm_a = omega_a * 9.554;//角速度から回転数(rpm)に換算するところ
+    rpm_b = omega_b * 9.554;
+
+    MD_val_a = fabs(rpm_a) * 0.8 / MAX_RPM+0.1;//ESCONの指令値に変換する処理
+    MD_val_b = fabs(rpm_b) * 0.8 / MAX_RPM+0.1;
+    if(MD_val_a>0.9) {//ESCONのPWM指令は10%から90%の間で有効なのでその範囲に収める処理
+        MD_val_a = 0.9;
+    } else if(MD_val_a<0.1) {
+        MD_val_a=0.1;
+    }
+    if(MD_val_b>0.9) {
+        MD_val_b = 0.9;
+    } else if(MD_val_b<0.1) {
+        MD_val_b=0.1;
+    }
+    
+    if(rpm_a>0) {
+        dir_a.write(1);
+    } else {
+        dir_a.write(0);
+    }
+    if(rpm_b>0) {
+        dir_b.write(0);
+    } else {
+        dir_b.write(1);
+    }
+    
+    pwm_a.write(MD_val_a);
+    pwm_b.write(MD_val_b);
+
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DDSScontrol.h	Wed Jan 26 11:21:36 2022 +0000
@@ -0,0 +1,41 @@
+#include "mbed.h"
+/*
+アクティブキャスタ制御クラス
+2022/01/26 A.Kobayashi
+*/
+#ifndef DDSScontrol_h
+#define DDSScontrol_h
+
+class DDSS
+{
+public:
+    DDSS(PinName _pwm_a,PinName _pwm_b,PinName _dir_a,PinName _dir_b);
+
+    void setposi(double _Wp_x,double _Wp_y);
+    double Wp_x;
+    double Wp_y;
+
+    void control(double _refVx,double _refVy,double _refVz,double _stear_angle);
+    double V_x;
+    double V_y;
+    double omega_wheel;
+    double omega_stear;
+    double omega_a;
+    double omega_b;
+    double rpm_a;
+    double rpm_b;
+    double refVx;
+    double refVy;
+    double refVz;
+    double stear_angle;
+    double MD_val_a;
+    double MD_val_b;
+
+private:
+    PwmOut pwm_a;
+    PwmOut pwm_b;
+    DigitalOut dir_a;
+    DigitalOut dir_b;
+};
+
+#endif
\ No newline at end of file

--- a/Platform.cpp	Mon Jan 24 03:12:54 2022 +0000
+++ b/Platform.cpp	Wed Jan 26 11:21:36 2022 +0000
@@ -5,27 +5,34 @@
 // 作成者：上野祐樹
 
 //アクティブキャスタの処理を追加
-//追加日:2021年12月22日
+//追加日:2022年01月26日
 //編集者:小林亮紘
 
 #include "mbed.h"
 #include "Platform.h"
-//#include "AMT22.h"
+#include "AMT22.h"
+#include "DDSScontrol.h"
 //#include "RoboClaw.h"
 //#include "AMT203VPeach.h"
 
 //RoboClaw MD(&SERIAL_ROBOCLAW,1);
 
 #if DRIVE_UNIT == PLATFORM_DUALWHEEL
-    AMT203V amt203(&SPI, PIN_CSB);
+AMT203V amt203(&SPI, PIN_CSB);
 #elif DRIVE_UNIT == PLATFORM_ACTIVECASTER3WHEEL
-	//SPI spi(P10_14, P10_15, P10_12); // mosi, miso, sclk
-	//AMT203V absenc_1(&spi, PIN_CSB_1);
-	//AMT203V absenc_2(&spi, PIN_CSB_2);
-	//AMT203V absenc_3(&spi, PIN_CSB_3);
+	SPI spi(P10_14, P10_15, P10_12); // mosi, miso, sclk
+	AMT203V absenc_1(&spi, PIN_CSB_1);
+	AMT203V absenc_2(&spi, PIN_CSB_2);
+	AMT203V absenc_3(&spi, PIN_CSB_3);
+	
+	DDSS unit1(PIN_PWM_1A,PIN_PWM_1B,PIN_DIR_1A,PIN_DIR_1B);//ピン宣言
+	DDSS unit2(PIN_PWM_2A,PIN_PWM_2B,PIN_DIR_2A,PIN_DIR_2B);//ピン宣言
+	DDSS unit3(PIN_PWM_3A,PIN_PWM_3B,PIN_DIR_3A,PIN_DIR_3B);//ピン宣言
+
 #endif
 
-Platform::Platform(int dir1, int dir2, int dir3, int dir4){
+Platform::Platform(int dir1, int dir2, int dir3, int dir4)
+{
     rotateDir[0] = (dir1 > 0) - (dir1 < 0); // 1より大きい数値だった場合に，1か-1にするための処理
     rotateDir[1] = (dir2 > 0) - (dir2 < 0);
     rotateDir[2] = (dir3 > 0) - (dir3 < 0);
@@ -34,7 +41,8 @@
     init_done = false;
 }
 
-Platform::Platform(int dir1, int dir2, int dir3){
+Platform::Platform(int dir1, int dir2, int dir3)
+{
     rotateDir[0] = (dir1 > 0) - (dir1 < 0); // 1より大きい数値だった場合に，1か-1にするための処理
     rotateDir[1] = (dir2 > 0) - (dir2 < 0);
     rotateDir[2] = (dir3 > 0) - (dir3 < 0);
@@ -42,7 +50,8 @@
     init_done = false;
 }
 
-Platform::Platform(){
+Platform::Platform()
+{
     rotateDir[0] = 1; // 指示がないときはすべて1
     rotateDir[1] = 1;
     rotateDir[2] = 1;
@@ -52,13 +61,17 @@
 }
 
 // 自己位置推定の初期化
-void Platform::platformInit(coords initPosi){
+void Platform::platformInit(coords initPosi)
+{
 #if DRIVE_UNIT == PLATFORM_DUALWHEEL
     SPI.begin(); // ここでSPIをbeginしてあげないとちゃんと動かなかった
     SPI.setClockDivider(SPI_CLOCK_DIV16); //SPI通信のクロックを1MHzに設定 beginの後に置かないと，処理が止まる
     stateamt203 = amt203.init();
 #elif DRIVE_UNIT == PLATFORM_ACTIVECASTER3WHEEL
-	//printf("AMT222 initialization %d %d %d\r\n", absenc_1.init(), absenc_2.init(), absenc_3.init());
+    printf("AMT222 initialization %d %d %d\r\n", absenc_1.init(), absenc_2.init(), absenc_3.init());
+    unit1.setposi(W1_x,W1_y);
+    unit2.setposi(W2_x,W2_y);
+    unit3.setposi(W3_x,W3_y);
 #endif
 
     //MD.begin(115200);
@@ -72,13 +85,15 @@
 }
 
 // 自己位置推定値(Posi)を外部からセット
-void Platform::setPosi(coords tempPosi){
+void Platform::setPosi(coords tempPosi)
+{
     Posi = tempPosi;
 }
 
 // エンコーダのカウント値と，ジャイロセンサから取得した角度をもとに自己位置を計算する
-coords Platform::getPosi(int encX, int encY, double angle_rad){
-    if(init_done){
+coords Platform::getPosi(int encX, int encY, double angle_rad)
+{
+    if(init_done) {
         // ローカル座標系での変化量を計算(zは角度)
         coords diff;
 
@@ -86,7 +101,7 @@
         double angX, angY;
         angX = (double)( encX - preEncX ) * _2PI_RES4;
         angY = (double)( encY - preEncY ) * _2PI_RES4;
-        
+
         double angle_diff;
         angle_diff = angle_rad - pre_angle_rad; // 角度の変化量を計算
         diff.z = angle_diff;
@@ -97,7 +112,7 @@
         Posi.z += diff.z;
         Posi.x += diff.x * cos( Posi.z ) - diff.y * sin( Posi.z );
         Posi.y += diff.x * sin( Posi.z ) + diff.y * cos( Posi.z );
-        
+
         // 1サンプル前のデータとして今回取得したデータを格納
         preEncX = encX;
         preEncY = encY;
@@ -106,93 +121,104 @@
     return Posi;
 }
 
-void Platform::VelocityControl(coords refV){
-    if(init_done){
-        #if DRIVE_UNIT == PLATFORM_OMNI3WHEEL
-            double refOmegaA, refOmegaB, refOmegaC;
-            // 車輪反時計方向が正
-            refOmegaA = (-refV.y - refV.z * DIST2WHEEL) / WHEEL_R * GEARRATIO;
-            refOmegaB = ( refV.x*COS_PI_6 + refV.y*SIN_PI_6 - refV.z * DIST2WHEEL) / WHEEL_R * GEARRATIO;
-            refOmegaC = (-refV.x*COS_PI_6 + refV.y*SIN_PI_6 - refV.z * DIST2WHEEL) / WHEEL_R * GEARRATIO;
+void Platform::VelocityControl(coords refV)
+{
+    if(init_done) {
+#if DRIVE_UNIT == PLATFORM_OMNI3WHEEL
+        double refOmegaA, refOmegaB, refOmegaC;
+        // 車輪反時計方向が正
+        refOmegaA = (-refV.y - refV.z * DIST2WHEEL) / WHEEL_R * GEARRATIO;
+        refOmegaB = ( refV.x*COS_PI_6 + refV.y*SIN_PI_6 - refV.z * DIST2WHEEL) / WHEEL_R * GEARRATIO;
+        refOmegaC = (-refV.x*COS_PI_6 + refV.y*SIN_PI_6 - refV.z * DIST2WHEEL) / WHEEL_R * GEARRATIO;
 
-            // RoboClawの指令値に変換
-            double mdCmdA, mdCmdB, mdCmdC;
-            mdCmdA = refOmegaA * _2RES_PI;
-            mdCmdB = refOmegaB * _2RES_PI;
-            mdCmdC = refOmegaC * _2RES_PI;
+        // RoboClawの指令値に変換
+        double mdCmdA, mdCmdB, mdCmdC;
+        mdCmdA = refOmegaA * _2RES_PI;
+        mdCmdB = refOmegaB * _2RES_PI;
+        mdCmdC = refOmegaC * _2RES_PI;
 
-            // モータにcmdを送り，回す
-            //MD.SpeedM1(ADR_MD1, (int)mdCmdA * rotateDir[0]);// 右前
-            //MD.SpeedM2(ADR_MD1, (int)mdCmdB * rotateDir[1]);// 左前
-            //MD.SpeedM2(ADR_MD2, (int)mdCmdC * rotateDir[2]);// 右後
-         #elif DRIVE_UNIT == PLATFORM_OMNI4WHEEL
-            double refOmegaA, refOmegaB, refOmegaC, refOmegaD;
-            // 車輪反時計方向が正
-            refOmegaA = ( refV.x * COS_PI_4 - refV.y * SIN_PI_4 - refV.z * DIST2WHEEL) / WHEEL_R * GEARRATIO; // [rad/s]
-            refOmegaB = ( refV.x * COS_PI_4 + refV.y * SIN_PI_4 - refV.z * DIST2WHEEL) / WHEEL_R * GEARRATIO;
-            refOmegaC = (-refV.x * COS_PI_4 + refV.y * SIN_PI_4 - refV.z * DIST2WHEEL) / WHEEL_R * GEARRATIO;
-            refOmegaD = (-refV.x * COS_PI_4 - refV.y * SIN_PI_4 - refV.z * DIST2WHEEL) / WHEEL_R * GEARRATIO;
+        // モータにcmdを送り，回す
+        //MD.SpeedM1(ADR_MD1, (int)mdCmdA * rotateDir[0]);// 右前
+        //MD.SpeedM2(ADR_MD1, (int)mdCmdB * rotateDir[1]);// 左前
+        //MD.SpeedM2(ADR_MD2, (int)mdCmdC * rotateDir[2]);// 右後
+#elif DRIVE_UNIT == PLATFORM_OMNI4WHEEL
+        double refOmegaA, refOmegaB, refOmegaC, refOmegaD;
+        // 車輪反時計方向が正
+        refOmegaA = ( refV.x * COS_PI_4 - refV.y * SIN_PI_4 - refV.z * DIST2WHEEL) / WHEEL_R * GEARRATIO; // [rad/s]
+        refOmegaB = ( refV.x * COS_PI_4 + refV.y * SIN_PI_4 - refV.z * DIST2WHEEL) / WHEEL_R * GEARRATIO;
+        refOmegaC = (-refV.x * COS_PI_4 + refV.y * SIN_PI_4 - refV.z * DIST2WHEEL) / WHEEL_R * GEARRATIO;
+        refOmegaD = (-refV.x * COS_PI_4 - refV.y * SIN_PI_4 - refV.z * DIST2WHEEL) / WHEEL_R * GEARRATIO;
 
-            // RoboClawの指令値に変換
-            double mdCmdA, mdCmdB, mdCmdC, mdCmdD;
-            mdCmdA = refOmegaA * _2RES_PI;
-            mdCmdB = refOmegaB * _2RES_PI;
-            mdCmdC = refOmegaC * _2RES_PI;
-            mdCmdD = refOmegaD * _2RES_PI;
+        // RoboClawの指令値に変換
+        double mdCmdA, mdCmdB, mdCmdC, mdCmdD;
+        mdCmdA = refOmegaA * _2RES_PI;
+        mdCmdB = refOmegaB * _2RES_PI;
+        mdCmdC = refOmegaC * _2RES_PI;
+        mdCmdD = refOmegaD * _2RES_PI;
+
+        //MD.SpeedM1(ADR_MD1, (int)mdCmdA * rotateDir[0]); // 左前 ①
+        //MD.SpeedM2(ADR_MD1, (int)mdCmdC * rotateDir[1]); // 左後 ②
+        //MD.SpeedM1(ADR_MD2, (int)mdCmdD * rotateDir[2]); // 右後 ③
+        //MD.SpeedM2(ADR_MD2, (int)mdCmdB * rotateDir[3]); // 右前 ④
+#elif DRIVE_UNIT == PLATFORM_DUALWHEEL
+        // ターンテーブルの角度取得
+        double thetaDuEnc, thetaDu;
+        static double  preThetaDuEnc = thetaDuEnc;
+        thetaDuEnc = amt203.getEncount();
+        if( thetaDuEnc == -1 ) {
+            thetaDuEnc = preThetaDuEnc; // -1はエラーなので，前の値を格納しておく
+        }
+        preThetaDuEnc = thetaDuEnc;
+        thetaDu = (double)thetaDuEnc*2*PI / TT_RES4;	// 角度に変換
 
-            //MD.SpeedM1(ADR_MD1, (int)mdCmdA * rotateDir[0]); // 左前 ①
-            //MD.SpeedM2(ADR_MD1, (int)mdCmdC * rotateDir[1]); // 左後 ②
-            //MD.SpeedM1(ADR_MD2, (int)mdCmdD * rotateDir[2]); // 右後 ③
-            //MD.SpeedM2(ADR_MD2, (int)mdCmdB * rotateDir[3]); // 右前 ④
-        #elif DRIVE_UNIT == PLATFORM_DUALWHEEL
-            // ターンテーブルの角度取得
-            double thetaDuEnc, thetaDu;
-            static double  preThetaDuEnc = thetaDuEnc;
-            thetaDuEnc = amt203.getEncount(); 
-            if( thetaDuEnc == -1 ){
-            thetaDuEnc = preThetaDuEnc; // -1はエラーなので，前の値を格納しておく
-            }
-            preThetaDuEnc = thetaDuEnc;
-            thetaDu = (double)thetaDuEnc*2*PI / TT_RES4;	// 角度に変換
-            
-            // 車輪やターンテーブルの指令速度を計算
-            double cosDu, sinDu, refOmegaR, refOmegaL, refOmegaT;
-            cosDu = cos(thetaDu);
-            sinDu = sin(thetaDu);
-            refOmegaR = ( ( cosDu - sinDu ) * refV.x + ( sinDu + cosDu ) * refV.y ) / RADIUS_R;// right
-            refOmegaL = ( ( cosDu + sinDu ) * refV.x + ( sinDu - cosDu ) * refV.y ) / RADIUS_L;// left
-            refOmegaT = ( - ( 2 * sinDu / W ) * refV.x + ( 2 * cosDu / W ) * refV.y - refV.z ) * GEARRATIO;// turntable
+        // 車輪やターンテーブルの指令速度を計算
+        double cosDu, sinDu, refOmegaR, refOmegaL, refOmegaT;
+        cosDu = cos(thetaDu);
+        sinDu = sin(thetaDu);
+        refOmegaR = ( ( cosDu - sinDu ) * refV.x + ( sinDu + cosDu ) * refV.y ) / RADIUS_R;// right
+        refOmegaL = ( ( cosDu + sinDu ) * refV.x + ( sinDu - cosDu ) * refV.y ) / RADIUS_L;// left
+        refOmegaT = ( - ( 2 * sinDu / W ) * refV.x + ( 2 * cosDu / W ) * refV.y - refV.z ) * GEARRATIO;// turntable
+
+        // RoboClawの指令値に変換
+        double mdCmdR, mdCmdL, mdCmdT;
+        mdCmdR = refOmegaR * _2RES_PI;
+        mdCmdL = refOmegaL * _2RES_PI;
+        mdCmdT = refOmegaT * _2RES_PI_T;
 
-            // RoboClawの指令値に変換
-            double mdCmdR, mdCmdL, mdCmdT;
-            mdCmdR = refOmegaR * _2RES_PI;
-            mdCmdL = refOmegaL * _2RES_PI;
-            mdCmdT = refOmegaT * _2RES_PI_T;
+        // モータにcmdを送り，回す
+        //MD.SpeedM1(ADR_MD1, -(int)mdCmdR);// 右車輪
+        //MD.SpeedM2(ADR_MD1,  (int)mdCmdL);// 左車輪
+        //MD.SpeedM1(ADR_MD2,  (int)mdCmdT);// ターンテーブル
+#elif DRIVE_UNIT == PLATFORM_MECHANUM
+        double refOmegaA, refOmegaB, refOmegaC, refOmegaD;
+        // 車輪の軸まわりで見て，反時計方向が正
+        refOmegaA = ( refV.x - refV.y + refV.z * ( TREAD_2 + WHEELBASE_2 ) ) / WHEEL_R;// 左前
+        refOmegaB = ( refV.x + refV.y + refV.z * ( TREAD_2 + WHEELBASE_2 ) ) / WHEEL_R;// 左後
+        refOmegaC = (-refV.x + refV.y + refV.z * ( TREAD_2 + WHEELBASE_2 ) ) / WHEEL_R;// 右後
+        refOmegaD = (-refV.x - refV.y + refV.z * ( TREAD_2 + WHEELBASE_2 ) ) / WHEEL_R;// 右前
 
-            // モータにcmdを送り，回す
-            //MD.SpeedM1(ADR_MD1, -(int)mdCmdR);// 右車輪
-            //MD.SpeedM2(ADR_MD1,  (int)mdCmdL);// 左車輪
-            //MD.SpeedM1(ADR_MD2,  (int)mdCmdT);// ターンテーブル
-        #elif DRIVE_UNIT == PLATFORM_MECHANUM
-            double refOmegaA, refOmegaB, refOmegaC, refOmegaD;
-            // 車輪の軸まわりで見て，反時計方向が正
-            refOmegaA = ( refV.x - refV.y + refV.z * ( TREAD_2 + WHEELBASE_2 ) ) / WHEEL_R;// 左前
-            refOmegaB = ( refV.x + refV.y + refV.z * ( TREAD_2 + WHEELBASE_2 ) ) / WHEEL_R;// 左後
-            refOmegaC = (-refV.x + refV.y + refV.z * ( TREAD_2 + WHEELBASE_2 ) ) / WHEEL_R;// 右後
-            refOmegaD = (-refV.x - refV.y + refV.z * ( TREAD_2 + WHEELBASE_2 ) ) / WHEEL_R;// 右前
+        // RoboClawの指令値に変換
+        double mdCmdA, mdCmdB, mdCmdC, mdCmdD;
+        mdCmdA = refOmegaA * _2RES_PI;
+        mdCmdB = refOmegaB * _2RES_PI;
+        mdCmdC = refOmegaC * _2RES_PI;
+        mdCmdD = refOmegaD * _2RES_PI;
 
-            // RoboClawの指令値に変換
-            double mdCmdA, mdCmdB, mdCmdC, mdCmdD;
-            mdCmdA = refOmegaA * _2RES_PI;
-            mdCmdB = refOmegaB * _2RES_PI;
-            mdCmdC = refOmegaC * _2RES_PI;
-            mdCmdD = refOmegaD * _2RES_PI;
-
-            //MD.SpeedM1(ADR_MD1,  (int)mdCmdA * rotateDir[0]);// 左前
-            //MD.SpeedM2(ADR_MD1,  (int)mdCmdB * rotateDir[1]);// 左後
-            //MD.SpeedM1(ADR_MD2,  (int)mdCmdC * rotateDir[2]);// 右後
-            //MD.SpeedM2(ADR_MD2,  (int)mdCmdD * rotateDir[3]);// 右前
-
-        #endif
+        //MD.SpeedM1(ADR_MD1,  (int)mdCmdA * rotateDir[0]);// 左前
+        //MD.SpeedM2(ADR_MD1,  (int)mdCmdB * rotateDir[1]);// 左後
+        //MD.SpeedM1(ADR_MD2,  (int)mdCmdC * rotateDir[2]);// 右後
+        //MD.SpeedM2(ADR_MD2,  (int)mdCmdD * rotateDir[3]);// 右前
+        
+#elif DRIVE_UNIT == PLATFORM_ACTIVECASTER3WHEEL
+		double stear_1_rad = (double)absenc_1.getEncount() / ABS_ENCRES * PI * -2;
+		double stear_2_rad = (double)absenc_2.getEncount() / ABS_ENCRES * PI * -2;
+		double stear_3_rad = (double)absenc_3.getEncount() / ABS_ENCRES * PI * -2;
+		
+		unit1.control(refV.x,refV.y,refV.z,stear_1_rad);
+		unit2.control(refV.x,refV.y,refV.z,stear_2_rad);
+		unit3.control(refV.x,refV.y,refV.z,stear_3_rad);
+		
+		
+#endif
     }
 }
\ No newline at end of file