Diff: main.cpp

Revision:

0:450a7208c682

Child:

1:f6a9a3a7455d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue May 03 08:02:58 2022 +0000
@@ -0,0 +1,153 @@
+#include "mbed.h"
+#include "CAN.h"
+#include "PS4_Controller.h"
+#include "MDDS30_oit.h"
+#include "URF.h"
+
+PS4_controller PS4(PC_12,PD_2);
+
+Serial pc(USBTX, USBRX, 115200);
+
+URF usdRight(D6);
+URF usdLeft(D8);
+
+Ticker update;
+
+DigitalOut led(LED1);
+DigitalOut led2(PC_4);
+ 
+DigitalIn btn(USER_BUTTON);
+MDDS30oit MD1(PC_10, PC_11);  //left:モーター番号1（右後輪）、right:モーター番号2（左後輪）
+MDDS30oit MD2(PA_0, PA_1);  //left:モーター番号3（右前輪）、right:モーター番号4（左前輪）
+
+void inter(void)
+{
+    usdRight.send();
+    usdLeft.send();
+}
+
+int i;
+int right = 0, left = 0;
+float duty[4]={0.0f};
+
+void Yonrin(float x,float y)       //四輪オムニのpwm値出し
+{
+    duty[0] =-x/sqrtf(2)+y/sqrtf(2);
+    duty[1] =-x/sqrtf(2)-y/sqrtf(2);
+    duty[2] = x/sqrtf(2)-y/sqrtf(2);
+    duty[3] = x/sqrtf(2)+y/sqrtf(2);
+    //pc.printf("a=%f, b=%f, c=%f, d=%f\r\n",duty[0],duty[1],duty[2],duty[3]);
+        
+        MD1.left(duty[0]);
+        MD1.right(duty[1]);
+        MD2.left(duty[2]);
+        MD2.right(duty[3]);
+        wait(0.01);
+}
+
+void Yonrin(float x)
+{
+        //pc.printf("senkai = %f\r\n",x);
+        
+        MD1.left(x);
+        MD1.right(x);
+        MD2.left(x);
+        MD2.right(x);
+        
+        wait(0.01);
+}
+
+void Yonrin(int mode)
+{
+    switch(mode) {
+        case 0:
+        MD1.left(0);
+        MD1.right(0);
+        MD2.left(0);
+        MD2.right(0);
+    }
+}
+
+int main(){
+    
+    wait_ms(100);
+    
+    PS4.send_UNO(1);
+
+    while(!PS4.connect()){
+        pc.printf("1");
+        led=!led;
+        PS4.read_PAD();
+        pc.printf("2");
+        wait_ms(500);
+    }
+    led = 1;
+    
+    update.attach(&inter, 0.06);
+
+    while(1){
+        int dataRight = usdRight.read(mm);
+        int dataLeft = usdLeft.read(mm);
+        pc.printf("Left:%d, Right:%d\n", dataLeft, dataRight);
+        //pc.printf("right: %d, left: %d\n", dataRight, dataLeft);
+
+        //right = 0;  //機体の右側に障害物なし
+        //left = 0;   //機体の左側に障害物なし
+
+        PS4.read_PAD();
+        if(PS4.button(PS))PS4.disconnect();
+        if(PS4.connect()){
+            if(PS4.button(R1)==1){                
+                if(PS4.analog(PS_LX)<100||PS4.analog(PS_LX)>146||PS4.analog(PS_LY)<100||PS4.analog(PS_LY)>146){
+                float x_duty = ((PS4.analog(PS_LX)-128.0f)/127.0f)*0.7f;
+                float y_duty = -((PS4.analog(PS_LY)-128.0f)/127.0f)*0.7f;
+                //pc.printf("x_duty=%f y_duty=%f\r\n", x_duty, y_duty);
+                Yonrin(x_duty, y_duty);
+                /*wait(1.0);
+                Yonrin(0);*/
+                }
+                else if(PS4.analog(PS_R2)>10){
+                    float x=-((PS4.analog(PS_R2)/255.0f))*0.7f;
+                    Yonrin(x);
+                }
+                else if(PS4.analog(PS_L2)>10){
+                    float x=((PS4.analog(PS_L2)/255.0f))*0.7f;
+                    Yonrin(x);
+                }
+                else if(PS4.button(UP)==1){
+                    Yonrin(0,0.5f);
+                }
+                else if(PS4.button(DOWN)==1){
+                    Yonrin(0,-0.5f);
+                }
+                else if(PS4.button(RIGHT)==1){
+                    if (dataRight >= 400){
+                        Yonrin(-0.5f,0);
+                    }
+                    else{
+                        if (dataRight <= 400 && dataRight >= 200){
+                            Yonrin(-0.3f, 0);
+                       }
+                    }
+                }
+                else if(PS4.button(LEFT)==1){
+                    if (dataLeft >= 400){
+                        Yonrin(0.5f,0);
+                    }else{
+                        if (dataLeft <= 400 && dataLeft >= 200){
+                            Yonrin(0.3f, 0);
+                        }
+                    }
+                    
+                }
+                else Yonrin(0);
+            }
+            else Yonrin(0);
+        }
+        else Yonrin(0);
+    }
+    /*if(PS4.button(PS)){
+        PS4.disconnect();
+    }
+    Yonrin(0);*/
+}
\ No newline at end of file