Revision 27:2d7a978e70ab, committed 2022-02-03

Comitter:

LVRhase01

Date:

Thu Feb 03 08:34:06 2022 +0000

Parent:

26:223339e60e00

Commit message:

first commit haseomni

Changed in this revision

--- a/MotorSMLAP.lib	Fri Jul 23 02:16:43 2021 +0000
+++ b/MotorSMLAP.lib	Thu Feb 03 08:34:06 2022 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/MotorSMLAP/#27eb6305b5fd
+https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/MotorSMLAP/#5995b899cdcc

--- a/main.cpp	Fri Jul 23 02:16:43 2021 +0000
+++ b/main.cpp	Thu Feb 03 08:34:06 2022 +0000
@@ -2,30 +2,29 @@
 #include "mbed.h"
 #include "math.h"
 #include "omni_wheel.h"
-#include "motorsmlap.h"
 #include "jy901.h"
 #include "SerialMultiByte.h"
 #include "PID.h"
+#include "vnh5019.h"
 
 JY901 jy(PB_9, PB_8); //sda, scl
 
-PID angle(2.0,0,0,0.01);
+PID angle(2.5,0.0,0.001, 0.01);
 Serial pc(USBTX, USBRX, 115200);
 SerialMultiByte arduino(PC_12,PD_2);
 SerialMultiByte m5stack(PC_10,PC_11);
 
 PwmOut beep(PA_0);
-
 DigitalIn  sw(PC_13);
 DigitalOut led1(PB_5);
 DigitalOut led2(LED2);
 DigitalOut led3(PA_1);
 DigitalOut led4(PA_10);
 
-motorSmLap motor[] = {
-    motorSmLap(PC_6, PC_9, PC_8),
-    motorSmLap(PB_6, PA_9, PC_7),
-    motorSmLap(PA_8, PB_4, PB_7)
+VNH5019 motor[] = {
+    VNH5019(PC_6, PC_9, PC_8),
+    VNH5019(PB_6, PA_9, PC_7),
+    VNH5019(PA_8, PB_4, PB_7)
     };
     
 const double PII = 3.14159265358979;
@@ -113,7 +112,7 @@
                 setmode++;
                 ssw=0;
             }
-        if(setmode>=2){
+        if(setmode>=3){
             setmode=0;
             }
         }
@@ -184,7 +183,7 @@
                 for(int i=0; i<3; i++){
                     motor[i].setMotorSpeed(omni.wheel[i].getOutput()*0.8);
                 }
-                pc.printf("X = <%f>,Y = <%f>,angle= <%f>\r\n",x,y,now_angle);
+                //pc.printf("X = <%f>,Y = <%f>,angle= <%f>\r\n",x,y,now_angle);
                 break;
                 
 /*wii balance　boardモード*/  
@@ -231,8 +230,43 @@
                 for(int i=0; i<3; i++){
                     motor[i].setMotorSpeed(omni.wheel[i].getOutput()*0.8);
                 }
+                //pc.printf("X = <%f>,Y = <%f>,angle= <%f>\r\n",x,y,now_angle);
+                break;
+                
+/*PS4 contoroller mode*/
+            case 2:
+                led1=0;
+                led3=0;
+                led4=1;
+                arduino.getData(data);
+                y=(data[1]-127.5)*-1.0/127.5;
+                x=(data[0]-127.5)/127.5;
+                if((x>=0&&x<=0.2)||(x<=0&&x>=-0.2)){//誤差をなくす
+                    x=0;
+                }
+                
+                if((y>=0&&y<=0.2)||(y<=0&&y>=-0.2)){//誤差をなくす
+                    y=0;
+                }
+                now_angle = jy.getZaxisAngle();
+                
+                if(now_angle>180 && now_angle<360){
+                    now_angle = now_angle-360;
+                }
+                angle.setProcessValue(now_angle);
+                omni.computeXY(
+                    x,
+                    y,
+                    0,
+                    0,
+                    angle.compute()
+                    
+                );
+                for(int i=0; i<3; i++){
+                    motor[i].setMotorSpeed(omni.wheel[i].getOutput()*0.8);
+                }
                 pc.printf("X = <%f>,Y = <%f>,angle= <%f>\r\n",x,y,now_angle);
-                break;         
+                break;      
         }
     }
 }
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/memo.txt	Thu Feb 03 08:34:06 2022 +0000
@@ -0,0 +1,66 @@
+smlapを書き換える前のやつ
+
+#include "motorsmlap.h"
+#include "mbed.h"
+
+motorSmLap::motorSmLap(PinName _dirH,PinName _dirL,PinName _pwm):
+    dirH(_dirH),dirL(_dirL), pwm(_pwm) {
+    started = false;
+    mode = SM;
+    pwm.period(1/20000.0);
+    dirL = 0;
+    dirH = 0;
+    pwm=0;
+    braking=false;
+}
+
+int motorSmLap::setMode(int _mode)
+{
+        if(mode >1 || mode < 0 || started)
+            return 1;
+        mode = _mode;
+        return 0;
+}
+
+int motorSmLap::setFreq(float freq)
+{
+        if(freq <= 0)
+            return 1;
+        pwm.period(1/freq);
+        return 0;
+}
+        
+int motorSmLap::setMotorSpeed(float speed)
+{
+    started = true;
+    if(speed >=1.0)
+        speed = 1.0;
+    if(speed <= -1.0)
+        speed = -1.0;
+    if(mode == SM)
+    {
+        if(fabs(speed) > 0.02)
+        {
+            if(speed > 0){
+                dirH = 0;
+                dirL = 1;
+            }else{
+                dirH = 1;
+                dirL = 0;
+            }
+            pwm = fabs(speed);
+        }else{
+            dirH = 0;
+            dirL = 0;
+            pwm = 0;
+        }
+    }else if(mode == LAP)
+    {
+        dirH = 1;
+        dirL = 1;
+        pwm = fabs((speed+1.0)/2.0);
+    }else{
+        return 1;
+    }
+    return 0;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/vnh5019.lib	Thu Feb 03 08:34:06 2022 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/vnh5019/#666c49565378