Revision 2:82c337a18500, committed 2017-08-05

Comitter:

hirotayamato

Date:

Sat Aug 05 06:11:16 2017 +0000

Parent:

1:fa8227369eb0

Commit message:

Omni_2017_a

Changed in this revision

diff -r fa8227369eb0 -r 82c337a18500 2017_3/2017_3_z.cpp
--- a/2017_3/2017_3_z.cpp	Sat Aug 05 02:36:16 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,87 +0,0 @@
-#include "2017_3_z.h"
-#include "mbed.h"
-#include <math.h>
-
-Omni_3::Omni_3(PinName pin_pwm_F1, PinName pin_pwm_F2, PinName pin_dere_F1, PinName pin_dere_F2, PinName pin_channelA_F, PinName pin_channelB_F,
-               PinName pin_pwm_L1, PinName pin_pwm_L2, PinName pin_dere_L1, PinName pin_dere_L2, PinName pin_channelA_L, PinName pin_channelB_L,
-               PinName pin_pwm_R1, PinName pin_pwm_R2, PinName pin_dere_R1, PinName pin_dere_R2, PinName pin_channelA_R, PinName pin_channelB_R,
-               int arg_rev)
-{
-    md_f1 = Create_MD_PID(pin_channelA_F, pin_channelB_F, NC, 500, QEI::X4_ENCODING, 
-                         0.095 * 2, 0, 0.002 * 4, 500, 
-                         pin_pwm_F1, pin_dere_F1);
-                         
-    md_f2 = Create_MD_PID(pin_channelA_F, pin_channelB_F, NC, 500, QEI::X4_ENCODING,
-    					 0.095 * 2, 0, 0.002 * 4, 500,
-    					 pin_pwm_F2, pin_dere_F2);
-    
-    md_l1 = Create_MD_PID(pin_channelA_L, pin_channelB_L, NC, 500, QEI::X4_ENCODING,
-    					 0.095 * 2, 0, 0.002 * 4, 500,
-    					 pin_pwm_L1, pin_dere_L1);
-    
-    md_l2 = Create_MD_PID(pin_channelA_L, pin_channelB_L, NC, 500, QEI::X4_ENCODING,
-    					 0.095 * 2, 0, 0.002 * 4, 500,
-    					 pin_pwm_L2, pin_dere_L2);
-    
-    md_r1 = Create_MD_PID(pin_channelA_R, pin_channelB_R, NC, 500, QEI::X4_ENCODING,
-    					 0.095 * 2, 0, 0.002 * 4, 500,
-    					 pin_pwm_R1, pin_dere_R1);
-    					 
-    md_r2 = Create_MD_PID(pin_channelA_R, pin_channelB_R, NC, 500, QEI::X4_ENCODING,
-    					 0.095 * 2, 0, 0.002 * 4, 500,
-    					 pin_pwm_R2, pin_dere_R2);
-    
-    rev = arg_rev;
-}
-
-void Omni_3::Drive( double arg_x, double arg_y, double arg_rota,
-					double F_Gain, double L_Gain, double R_Gain)
-{
-    double dere, sp;
-    double duty[3];
-    double speed[3];
-    
-    dere = atan2(arg_y, arg_x);
-    sp = sqrt(pow(arg_x, 2.0) + pow(arg_y, 2.0));
-    
-    speed[0] = cos(dere) * sp;   // vx
-    speed[1] = sin(dere) * sp;   // vy
-    speed[2] = arg_rota;                // omega
-    
-    Matrix(speed, duty);
-    for(int i = 0; i < 3; i++){
-        if(fabs(duty[i]) > 1.0){
-            int inv = fabs(duty[i]);
-            for(int j = 0; j < 3; j++){
-                duty[j] *= 1.0 / (double)inv;
-            }
-        }
-    }
-    md_f1->Drive(duty[0] * F_Gain, 100);
-    md_f2->Drive(duty[0] * F_Gain, 100);
-    md_l1->Drive(duty[1] * L_Gain, 100);
-    md_l2->Drive(duty[1] * L_Gain, 100);
-    md_r1->Drive(duty[2] * R_Gain, 100);
-    md_r2->Drive(duty[2] * R_Gain, 100);
-}
-
-void Omni_3::Matrix(double speed[3], double duty[3])
-{
-    double keisu[3][3];
-    keisu[0][0] = -1.0;
-    keisu[0][1] = 0.0;
-    keisu[0][2] = 1.0;
-    keisu[1][0] = 1.0 / 2.0;
-    keisu[1][1] = -sqrt(3.0) / 2.0;
-    keisu[1][2] = 1.0;
-    keisu[2][0] = 1.0 / 2.0;
-    keisu[2][1] = sqrt(3.0) / 2.0;
-    keisu[2][2] = 1.0;
-    
-    for(int i = 0; i < 3; i++){
-        duty[i] = 0.0;
-        for(int j = 0; j < 3; j++){
-            duty[i] += keisu[i][j] * speed[j] * rev;
-        }
-    }
-}
\ No newline at end of file

diff -r fa8227369eb0 -r 82c337a18500 2017_3/2017_3_z.h
--- a/2017_3/2017_3_z.h	Sat Aug 05 02:36:16 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,27 +0,0 @@
-#ifndef _OMNI_3_H_
-#define _OMNI_3_H_
-#include "mbed.h"
-#include "MD.h"
-#include "MD_PID.h"
-
-class Omni_3{
-public:
-    Omni_3(PinName pin_pwm_F1, PinName pin_pwm_F2, PinName pin_dere_F1, PinName pin_dere_F2, PinName pin_channelA_F, PinName pin_channelB_F,
-           PinName pin_pwm_L1, PinName pin_pwm_L2, PinName pin_dere_L1, PinName pin_dere_L2, PinName pin_channelA_L, PinName pin_channelB_L,
-           PinName pin_pwm_R1, PinName pin_pwm_R2, PinName pin_dere_R1, PinName pin_dere_R2, PinName pin_channelA_R, PinName pin_channelB_R,
-           int rev = 1);
-    void Drive( double arg_x, double arg_y, double arg_rota,
-    			double F_Gain = 1.0, double L_Gain = 1.0, double R_Gain = 1.0);
-    			
-private:
-    void Matrix(double speed[3], double duty[3]);
-    int rev;
-    MD_PID *md_f1;
-    MD_PID *md_f2;
-    MD_PID *md_l1;
-    MD_PID *md_l2;
-    MD_PID *md_r1;
-    MD_PID *md_r2;
-};
-#endif
-

diff -r fa8227369eb0 -r 82c337a18500 2017_3/MD/MD.cpp
--- a/2017_3/MD/MD.cpp	Sat Aug 05 02:36:16 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,20 +0,0 @@
-#include"mbed.h"
-#include"MD.h"
-MD::MD(PinName pwm,PinName dere):Pwm(pwm),Dere(dere)
-{
-    Pwm.period(0.0001);
-}
-
-void MD::rotate(double duty)
-{
-    if(duty > 0)
-    {
-        Dere = 0;
-        Pwm = duty;
-    }
-    else
-    {
-        Dere = 1;
-        Pwm = -duty;
-    }
-}

diff -r fa8227369eb0 -r 82c337a18500 2017_3/MD/MD.h
--- a/2017_3/MD/MD.h	Sat Aug 05 02:36:16 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,15 +0,0 @@
-#ifndef _MD_H_
-#define _MD_H_
-#include "mbed.h"
-
-class MD
-{
-public:
-    MD(PinName pwm,PinName dere);
-    void rotate(double duty);
-private:
-    PwmOut Pwm;
-    DigitalOut Dere;
-};
-
-#endif

diff -r fa8227369eb0 -r 82c337a18500 2017_3_a/2017_3_a.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/2017_3_a/2017_3_a.cpp	Sat Aug 05 06:11:16 2017 +0000
@@ -0,0 +1,75 @@
+#include "2017_3_a.h"
+#include "mbed.h"
+#include <math.h>
+
+Omni_3::Omni_3(PinName pin_pwm_F1, PinName pin_pwm_F2, PinName pin_dere_F1, PinName pin_dere_F2,/* PinName pin_channelA_F, PinName pin_channelB_F,*/
+               PinName pin_pwm_L1, PinName pin_pwm_L2, PinName pin_dere_L1, PinName pin_dere_L2,/* PinName pin_channelA_L, PinName pin_channelB_L,*/
+               PinName pin_pwm_R1, PinName pin_pwm_R2, PinName pin_dere_R1, PinName pin_dere_R2,/* PinName pin_channelA_R, PinName pin_channelB_R,*/
+               int arg_rev)
+{
+    md_f1 = new MD(pin_pwm_F1, pin_dere_F1);
+                         
+    md_f2 = new MD(pin_pwm_F2, pin_dere_F2);
+    
+    md_l1 = new MD(pin_pwm_L1, pin_dere_L1);
+    
+    md_l2 = new MD(pin_pwm_L2, pin_dere_L2);
+    
+    md_r1 = new MD(pin_pwm_R1, pin_dere_R1);
+    					 
+    md_r2 = new MD(pin_pwm_R2, pin_dere_R2);
+    
+    rev = arg_rev;
+}
+
+void Omni_3::Drive( double arg_x, double arg_y, double arg_rota,
+					double F_Gain, double L_Gain, double R_Gain)
+{
+    double dere, sp;
+    double duty[3];
+    double speed[3];
+    
+    dere = atan2(arg_y, arg_x);
+    sp = sqrt(pow(arg_x, 2.0) + pow(arg_y, 2.0));
+    
+    speed[0] = cos(dere) * sp;   // vx
+    speed[1] = sin(dere) * sp;   // vy
+    speed[2] = arg_rota;                // omega
+    
+    Matrix(speed, duty);
+    for(int i = 0; i < 3; i++){
+        if(fabs(duty[i]) > 1.0){
+            int inv = fabs(duty[i]);
+            for(int j = 0; j < 3; j++){
+                duty[j] *= 1.0 / (double)inv;
+            }
+        }
+    }
+    md_f1->rotate(duty[0] * F_Gain);
+    md_f2->rotate(duty[0] * F_Gain);
+    md_l1->rotate(duty[1] * L_Gain);
+    md_l2->rotate(duty[1] * L_Gain);
+    md_r1->rotate(duty[2] * R_Gain);
+    md_r2->rotate(duty[2] * R_Gain);
+}
+
+void Omni_3::Matrix(double speed[3], double duty[3])
+{
+    double keisu[3][3];
+    keisu[0][0] = -1.0;
+    keisu[0][1] = 0.0;
+    keisu[0][2] = 1.0;
+    keisu[1][0] = 1.0 / 2.0;
+    keisu[1][1] = -sqrt(3.0) / 2.0;
+    keisu[1][2] = 1.0;
+    keisu[2][0] = 1.0 / 2.0;
+    keisu[2][1] = sqrt(3.0) / 2.0;
+    keisu[2][2] = 1.0;
+    
+    for(int i = 0; i < 3; i++){
+        duty[i] = 0.0;
+        for(int j = 0; j < 3; j++){
+            duty[i] += keisu[i][j] * speed[j] * rev;
+        }
+    }
+}
\ No newline at end of file

diff -r fa8227369eb0 -r 82c337a18500 2017_3_a/2017_3_a.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/2017_3_a/2017_3_a.h	Sat Aug 05 06:11:16 2017 +0000
@@ -0,0 +1,26 @@
+#ifndef _OMNI_3_H_
+#define _OMNI_3_H_
+#include "mbed.h"
+#include "MD.h"
+
+class Omni_3{
+public:
+    Omni_3(PinName pin_pwm_F1, PinName pin_pwm_F2, PinName pin_dere_F1, PinName pin_dere_F2,
+           PinName pin_pwm_L1, PinName pin_pwm_L2, PinName pin_dere_L1, PinName pin_dere_L2,
+           PinName pin_pwm_R1, PinName pin_pwm_R2, PinName pin_dere_R1, PinName pin_dere_R2,
+           int rev = 1);
+    void Drive( double arg_x, double arg_y, double arg_rota,
+    			double F_Gain = 1.0, double L_Gain = 1.0, double R_Gain = 1.0);
+    			
+private:
+    void Matrix(double speed[3], double duty[3]);
+    int rev;
+    MD *md_f1;
+    MD *md_f2;
+    MD *md_l1;
+    MD *md_l2;
+    MD *md_r1;
+    MD *md_r2;
+};
+#endif
+

diff -r fa8227369eb0 -r 82c337a18500 2017_3_a/MD/MD.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/2017_3_a/MD/MD.cpp	Sat Aug 05 06:11:16 2017 +0000
@@ -0,0 +1,20 @@
+#include"mbed.h"
+#include"MD.h"
+MD::MD(PinName pwm,PinName dere):Pwm(pwm),Dere(dere)
+{
+    Pwm.period(0.0001);
+}
+
+void MD::rotate(double duty)
+{
+    if(duty > 0)
+    {
+        Dere = 0;
+        Pwm = duty;
+    }
+    else
+    {
+        Dere = 1;
+        Pwm = -duty;
+    }
+}

diff -r fa8227369eb0 -r 82c337a18500 2017_3_a/MD/MD.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/2017_3_a/MD/MD.h	Sat Aug 05 06:11:16 2017 +0000
@@ -0,0 +1,15 @@
+#ifndef _MD_H_
+#define _MD_H_
+#include "mbed.h"
+
+class MD
+{
+public:
+    MD(PinName pwm,PinName dere);
+    void rotate(double duty);
+private:
+    PwmOut Pwm;
+    DigitalOut Dere;
+};
+
+#endif

diff -r fa8227369eb0 -r 82c337a18500 MD_PID.lib
--- a/MD_PID.lib	Sat Aug 05 02:36:16 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://developer.mbed.org/users/hirotayamato/code/MD_PID/#89d0a912d972

diff -r fa8227369eb0 -r 82c337a18500 main.cpp
--- a/main.cpp	Sat Aug 05 02:36:16 2017 +0000
+++ b/main.cpp	Sat Aug 05 06:11:16 2017 +0000
@@ -1,10 +1,9 @@
-#include "2017_3_z.h"
+#include "2017_3_a.h"
 #include "mbed.h"
 #include <math.h>
-#include "MD_PID.h"
-#include "QEI.h"
+#include "MD.h"
 
-Omni_3 Omni(p21, p22, p18, p17, p11, p12, p23, p24, p16, p15, p27, p28, p25, p26, p14, p13, p29, p30, 1);
+Omni_3 Omni(p21, p22, p18, p17, p23, p24, p16, p15, p25, p26, p14, p13, 1);
 
 Serial pc (USBTX, USBRX);
 I2CSlave slave(p9, p10);
@@ -50,3 +49,4 @@
         
     }
 }
+