Diff: main.cpp

Revision:

0:e7fea65cf3ab

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Nov 29 07:04:04 2022 +0000
@@ -0,0 +1,264 @@
+#include "mbed.h"
+/*
+#include "IncEncoder.h" //インクリメント方式エンコーダ用ライブラリ
+#include "KRS.h" //近藤ICSサーボ用ライブラリ
+*/
+#include <math.h>
+
+//円周率
+#define PI 3.14159265358979
+
+//モータードライバのアドレス
+#define DR1_MOTOR 0x01
+#define DR2_MOTOR 0x02
+#define DR3_MOTOR 0x03
+#define DR4_MOTOR 0x04
+
+//モータードライバへの命令
+#define STOP 0
+#define CW 1
+#define CCW -1
+#define bSTOP 10
+#define bCW 11
+#define bCCW -11
+
+#define sCW 21
+#define sCCW -21
+#define sSTOP 20
+#define shCW 31
+#define shCCW -31
+
+#define lCW 51
+#define lCCW -51
+#define lSTOP 50
+#define lsCW 61
+#define lsCCW -61
+#define lsSTOP 60
+
+#define rON 41
+#define rOFF -41
+
+Ticker timer; //767マザー動作不安定
+
+Serial controller_uart(PA_0,PA_1,19200);
+I2C md_i2c_tx(PD_13,PD_12);
+
+DigitalOut led1(PC_10);
+DigitalOut led2(PD_7);
+//DigitalOut led3(PD_6);
+//DigitalOut led4(PD_4);
+
+volatile unsigned char controller_data[10] = { 0x00, 0x00, 0x00, 0x00, 0x80, 0x80, 0x80, 0x80 };
+volatile unsigned char controller_data_number = 0;
+volatile unsigned char controller_cmd_getflag = 0;
+
+double lx, ly, lraw_rad, lclearance, ldistance, langle;
+double rx, ry, rraw_rad, rclearance, rdistance, rangle;
+
+//プロトタイプ関数宣言
+void read_controller_data();
+void con_check();
+void joy_cal();
+void md_setoutput(unsigned char address, int rotate, int duty);
+
+//main
+int main(){
+    md_i2c_tx.frequency(400000); //MD用I2C
+    timer.attach_us(&con_check, 100000);
+    controller_uart.attach(&read_controller_data, Serial::RxIrq); //受信割り込み
+    
+    int rev1, rev2, rev3, rev4;
+    
+    double power1, power2, power3, power4;
+    double power_up = 0;
+    
+    //ジョイスティックのオフセット
+    lclearance = 60;
+    rclearance = 90;
+    
+    led1 = 1;
+    
+    while(true){
+        //モータ出力比計算
+        power1 = sin((langle - 45)*PI/180);
+        power2 = sin((langle - 135)*PI/180);
+        power3 = sin((langle - 225)*PI/180);
+        power4 = sin((langle - 315)*PI/180);
+    
+        if(power1 >= 0){
+            rev1 = bCW;
+        }else{
+            rev1 = bCCW;
+            power1 = -power1;
+        }
+        if(power2 >= 0){
+            rev2 = bCW;
+        }else{
+            rev2 = bCCW;
+            power2 = -power2;
+        }
+        if(power3 >= 0){
+            rev3 = bCW;
+        }else{
+            rev3 = bCCW;
+            power3 = -power3;
+        }
+        if(power4 >= 0){
+            rev4 = bCW;
+        }else{
+            rev4 = bCCW;
+            power4 = -power4;
+        }
+            
+        //速度チェンジ
+        if (controller_data[2] & 0x04){
+            power_up = 1;
+        }else{
+            power_up = 0.8;
+        }
+        
+        //足回り処理
+        if(rx > 0){
+            md_setoutput(DR1_MOTOR, bCW, rx * 0.5);
+            md_setoutput(DR2_MOTOR, bCW, rx * 0.5);
+            md_setoutput(DR3_MOTOR, bCW, rx * 0.5);
+            md_setoutput(DR4_MOTOR, bCW, rx * 0.5);
+        }else if(rx < 0){
+            md_setoutput(DR1_MOTOR, bCCW, -rx * 0.5);
+            md_setoutput(DR2_MOTOR, bCCW, -rx * 0.5);
+            md_setoutput(DR3_MOTOR, bCCW, -rx * 0.5);
+            md_setoutput(DR4_MOTOR, bCCW, -rx * 0.5);
+        }else{
+            if(ldistance > 50){
+                md_setoutput(DR1_MOTOR, rev1, (char)power1*ldistance*power_up);
+                md_setoutput(DR2_MOTOR, rev2, (char)power2*ldistance*power_up);
+                md_setoutput(DR3_MOTOR, rev3, (char)power3*ldistance*power_up);
+                md_setoutput(DR4_MOTOR, rev4, (char)power4*ldistance*power_up);
+            }else{
+                md_setoutput(DR1_MOTOR, bSTOP, 0);
+                md_setoutput(DR2_MOTOR, bSTOP, 0);
+                md_setoutput(DR3_MOTOR, bSTOP, 0);
+                md_setoutput(DR4_MOTOR, bSTOP, 0);
+            }
+        }
+        
+        wait_ms(10);
+    }
+}
+
+//コントローラー受信処理
+void read_controller_data() {
+    unsigned char rxdata;
+    
+    rxdata = controller_uart.getc();
+    
+    if (controller_data_number == 0 && rxdata == 'S')
+        controller_data_number++;
+    else if (controller_data_number >= 1 && controller_data_number <= 7) {
+        if (controller_data_number <= 3)
+            controller_data[controller_data_number] = ~rxdata;
+        else
+            controller_data[controller_data_number] = rxdata;
+        controller_data_number++;
+    }
+    else if (controller_data_number == 8 && rxdata == 'E') {
+        controller_cmd_getflag = 0x01;
+        controller_data_number = 0;
+        led2 = 1;
+        joy_cal();
+    }
+}
+
+void con_check(){
+    if(controller_cmd_getflag & 0x02) {
+        controller_cmd_getflag = 0;
+        
+        controller_data[1] = 0x00;
+        controller_data[2] = 0x00;
+        controller_data[3] = 0x00;
+        controller_data[4] = 0x80;
+        controller_data[5] = 0x80;
+        controller_data[6] = 0x80;
+        controller_data[7] = 0x80;
+        
+        led2 = 0;
+    }else
+        controller_cmd_getflag |= 0x02;
+}
+
+//dualsenceジョイスティック計算
+void joy_cal() {
+    rx = -(128 - controller_data[4]) * 2;
+    ry = (128 - controller_data[5]) * 2;
+    if (rx >= 255) rx = 255;
+    if (rx <= -255) rx = -255;
+    if (ry >= 255) ry = 255;
+    if (ry <= -255) ry = -255;
+    rraw_rad = atan2(ry, rx) / PI;
+    rdistance = hypot(rx, ry) - rclearance;
+    rdistance *= (255 / (255 - rclearance));
+    if (rdistance < 0) rdistance = 0;
+    if (rdistance >= 255) rdistance = 255;
+    rangle = (rraw_rad < 0 ? 2 + rraw_rad : rraw_rad) * 180;
+    rangle = rdistance == 0 ? 0 : rangle;
+
+    if (rx > 0) {
+        rx -= rclearance;
+        if (rx < 0) rx = 0;
+    }
+    else if (rx < 0) {
+        rx += rclearance;
+        if (rx > 0) rx = 0;
+    }
+    rx *= (255 / (255 - rclearance));
+
+    lx = -(128 - controller_data[6]) * 2;
+    ly = (128 - controller_data[7]) * 2;
+    if (lx >= 255) lx = 255;
+    if (lx <= -255) lx = -255;
+    if (ly >= 255) ly = 255;
+    if (ly <= -255) ly = -255;
+    lraw_rad = atan2(ly, lx) / PI;
+    ldistance = hypot(lx, ly) - lclearance;
+    ldistance *= (255 / (255 - lclearance));
+    if (ldistance < 0) ldistance = 0;
+    if (ldistance >= 255) ldistance = 255;
+    langle = (lraw_rad < 0 ? 2 + lraw_rad : lraw_rad) * 180;
+    langle = ldistance == 0 ? 0 : langle;
+}
+
+//MD用I2C通信処理
+void md_setoutput(unsigned char address, int rotate, int duty){
+    char data[2];
+ 
+    // モーター駆動制御モード
+    switch (rotate) {
+        case CW:    data[0] = 'F'; break;// 電圧比例制御モード（PWMがLの区間はフリー）
+        case CCW:   data[0] = 'R'; break;
+        case STOP:  data[0] = 'S'; break;
+    
+        case bCW:   data[0] = 'f'; break; // 電圧比例制御モード（PWMがLの区間はブレーキ）
+        case bCCW:  data[0] = 'r'; break;
+        case bSTOP: data[0] = 's'; break;
+    
+        case sCW:   data[0] = 'A'; break; // 速度比例制御モード
+        case sCCW:  data[0] = 'B'; break;
+        case sSTOP: data[0] = 'T'; break;
+    
+        case shCW:   data[0] = 'a'; break; // 速度比例制御モード（命令パケット最上位ビットに指令値9ビット目を入れることで分解能2倍）
+        case shCCW:  data[0] = 'b'; break;
+    
+        case lCW:   data[0] = 'L'; break; // 通常LAP
+        case lCCW:  data[0] = 'M'; break;
+        case lSTOP: data[0] = 'N'; break;
+        case lsCW:   data[0] = 'l'; break; // 速調LAP
+        case lsCCW:  data[0] = 'm'; break;
+        case lsSTOP: data[0] = 'n'; break;
+    
+        case rON: data[0] = 'P'; break; //リレー駆動モード
+        case rOFF: data[0] = 'p'; break;
+    }
+    
+    data[1] = duty & 0xFF;
+    md_i2c_tx.write(address << 1, data, 2); //duty送信
+}
\ No newline at end of file