767mother

Dependencies:   IncEncoder KRS mbed

Files at this revision

API Documentation at this revision

Comitter:
koki_konishi
Date:
Tue Nov 29 07:04:04 2022 +0000
Commit message:
767mother

Changed in this revision

.gitignore Show annotated file Show diff for this revision Revisions of this file
IncEncoder.lib Show annotated file Show diff for this revision Revisions of this file
KRS.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/.gitignore	Tue Nov 29 07:04:04 2022 +0000
@@ -0,0 +1,4 @@
+.build
+.mbed
+projectfiles
+*.py*
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IncEncoder.lib	Tue Nov 29 07:04:04 2022 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/koki_konishi/code/IncEncoder/#c40c29a063c0
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/KRS.lib	Tue Nov 29 07:04:04 2022 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/koki_konishi/code/KRS/#431a802d01e2
--- /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
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Nov 29 07:04:04 2022 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file