Revision 0:450a7208c682, committed 2022-05-03

Comitter:

akihiron

Date:

Tue May 03 08:02:58 2022 +0000

Child:

1:f6a9a3a7455d

Commit message:

05/03

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MD_MDDS30_oit_.lib	Tue May 03 08:02:58 2022 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/OITRP_2022/code/MD_MDDS30_oit_/#4bf1ac8bf2c4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PS4_to_Arduino.lib	Tue May 03 08:02:58 2022 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/OITRP_2022/code/PS4_to_Arduino/#3d6274f8c368

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/URF.cpp	Tue May 03 08:02:58 2022 +0000
@@ -0,0 +1,74 @@
+#include "URF.h"
+
+#include "mbed.h"
+
+URF::URF(PinName URF_PIN)
+    : _event(URF_PIN)
+    , _cmd(URF_PIN)
+    , _timer()
+{
+    _event.rise(callback(this, &URF::_start));
+    _event.fall(callback(this, &URF::_stop));
+    _METER_PER_SECOND = 334.0;    // m/s
+    
+    _Busy = false;
+    
+    _cmd.output();
+    _cmd.write(0);
+    
+    _timer.stop();
+    _timer.reset();
+}
+
+void URF::_start(void)
+{
+//    _Valid = false;  // start the timere, and invalidate the current time.
+    _Busy = true;
+    _timer.reset();
+    _timer.start();
+}
+
+void URF::_stop(void)
+{
+//    _Valid = true;  // When it stops, update the time
+    _Busy = false;
+    _timer.stop();
+    _Time = _timer.read_us();
+}
+
+void URF::send(void)
+{
+    if(_Busy){
+        _timer.stop();
+        _Time = _timer.read_us();
+        return ;
+    }
+    _cmd.output();
+    _cmd.write(1);
+    wait_us(10);
+    _cmd.write(0);
+    _cmd.input();
+}
+
+void URF::Set_MpS(float MpS)
+{
+    _METER_PER_SECOND = MpS;
+}
+
+int URF::read(int scale)
+// -1 means not valid.
+{
+//    if(_Valid && ~_Busy)
+        return int(((_Time*(_METER_PER_SECOND / float(scale)))/2.0f) + 0.5f);
+//    else
+//        return -1;
+}
+
+int URF::read(void)
+// -1 means not valid.
+{
+//    if(_Valid && ~_Busy)
+        return int(((_Time*(_METER_PER_SECOND / 1000.0f))/2.0f) + 0.5f);
+//    else
+//        return -1;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/URF.h	Tue May 03 08:02:58 2022 +0000
@@ -0,0 +1,99 @@
+/* mbed URF Library
+ * Copyright (c) 2007-2010 rosienej
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#ifndef MBED_URF_H
+#define MBED_URF_H
+
+#include "mbed.h"
+/** URF class, based on an InterruptIn pin, and a timer
+ *  works with the UltrasonicRangeFinder sensor (Parallax PING))), HC-SR04, etc...)
+ *
+ * Example:
+ * @code
+ * // Continuously send URFs and read the sensor
+ * #include "mbed.h"
+ * #include "URF.h"
+ *
+ * URF Ping(p21);
+ *
+ * int main() {
+ *     int range;
+
+ *     while(1) {
+ *
+ *        Ping.send();
+ *        wait_ms(30);
+ *        range = Ping.read(mm);
+ *     }
+ * }
+ * @endcode
+ */
+
+#define mm 1000
+#define cm 100
+
+class URF
+{
+public:
+    /** Create a URF object connected to the specified InterruptIn pin
+     *
+     * @param URF_PIN InterruptIn pin to connect to
+     */
+    URF(PinName URF_PIN);
+
+    /** Sends a URF
+     *
+     * @param none
+     */
+    void send(void);
+
+    /** Set the meter per second, default 334.0 m/s
+     *
+     * @param Meter per second in m/s
+     */
+    void Set_MpS(float SoS_ms);
+
+    /** Read the result in centimeters
+      *
+      * @param none
+      */
+    int read(int scale);
+    int read(void);
+
+protected:
+
+    InterruptIn     _event;
+    DigitalInOut    _cmd;
+    Timer           _timer;
+
+//    bool  _Valid;
+    bool  _Busy;
+    int   _Time;
+    float _METER_PER_SECOND; /* in milliseconds */
+    float _mm_per_us;
+
+    void  _start(void);
+    void  _stop(void);
+
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/URF_check.txt	Tue May 03 08:02:58 2022 +0000
@@ -0,0 +1,87 @@
+#include "mbed.h"
+#include "URF.h"
+
+Serial pc(USBTX, USBRX, 115200);
+
+URF usdRight(D6);
+URF usdLeft(D8);
+
+Ticker update;
+
+DigitalOut led(LED1);
+DigitalOut led2(PC_4);
+ 
+DigitalIn btn(USER_BUTTON);
+
+void inter(void)
+{
+    usdRight.send();
+    usdLeft.send();
+}
+
+void URF(int dataRight, int dataLeft){
+    if(dataRight>100 && dataRight<=230){
+        pc.printf("right slow\n");
+        //Yonrin(-0.3f, 0);
+        if(dataLeft>100 && dataLeft<=230){
+            pc.printf("left slow\n");
+            //Yonrin(0.3f, 0);
+        }
+        else if(dataLeft<=100){
+            pc.printf("left stop\n");
+            //Yonrin(0);
+        }
+    }
+    else if(dataRight<=100){
+        pc.printf("right stop\n");
+        //Yonrin(0);
+        if(dataLeft>100 && dataLeft<=230){
+            pc.printf("left slow\n");
+            //Yonrin(0.3f, 0);
+        }
+        else if(dataLeft<=100){
+            pc.printf("left stop\n");
+            //Yonrin(0);
+        }
+    }
+    else if(dataLeft>100 && dataLeft<=230){
+        pc.printf("left slow\n");
+        //Yonrin(0.3f, 0);
+        if(dataRight>100 && dataRight<=230){
+            pc.printf("right slow\n");
+            //Yonrin(-0.3f, 0);
+        }
+        else if(dataRight<=100){
+            pc.printf("right stop\n");
+            //Yonrin(0);
+        }
+    }
+    else if(dataLeft<=100){
+        pc.printf("left stop\n");
+        //Yonrin(0);
+        if(dataRight>100 && dataRight<=230){
+            pc.printf("right slow\n");
+            //Yonrin(-0.3f, 0);
+        }
+        else if(dataRight<=100){
+            pc.printf("right stop\n");
+            //Yonrin(0);
+        }
+    }
+}
+
+int main(){
+    pc.printf("check1\n");
+
+    update.attach(&inter, 0.06);
+
+    while(1){
+        int dataRight = usdRight.read(mm);
+        int dataLeft = usdLeft.read(mm);
+        //pc.printf("right: %d, left: %d\n", dataRight, dataLeft);
+        
+        if(dataRight<=230 || dataLeft<=230){
+            URF(dataRight,dataLeft);
+        }
+    }
+}
\ No newline at end of file

--- /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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue May 03 08:02:58 2022 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file