SES
767mother
Dependencies: IncEncoder KRS mbed
Revision 0:e7fea65cf3ab, committed 24 months ago
- Comitter:
- koki_konishi
- Date:
- Tue Nov 29 07:04:04 2022 +0000
- Commit message:
- 767mother
Changed in this revision
--- /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