Revision 0:772b2085fe23, committed 2018-10-18

Comitter:

tajiri1999

Date:

Thu Oct 18 09:57:12 2018 +0000

Commit message:

????

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor.cpp	Thu Oct 18 09:57:12 2018 +0000
@@ -0,0 +1,207 @@
+#include "mbed.h"
+#include "Motor.h"
+
+Motor::Motor(PinName p01,PinName p10,PinName p02,PinName p20,PinName p03,PinName p30,PinName p04,PinName p40):
+_p01(p01),_p10(p10),_p02(p02),_p20(p20),_p03(p03),_p30(p30),_p04(p04),_p40(p40)
+{
+    _p01 = 0;
+    _p10 = 0;
+    _p02 = 0;
+    _p20 = 0;
+    _p03 = 0;
+    _p30 = 0;
+    _p04 = 0;
+    _p40 = 0;
+}
+
+/*モーター周波数変更初期設定*/
+void Motor::setPwmPeriod(float hz){
+    _p01.period(hz);
+    _p10.period(hz);
+    _p02.period(hz);
+    _p20.period(hz);
+    _p03.period(hz);
+    _p30.period(hz);
+    _p04.period(hz);
+    _p40.period(hz);
+}
+
+/*モーター制御*/
+void Motor::setPower(float a,float b,float c,float d){
+    
+    a = a / 100;
+    b = b / 100;
+    c = c / 100;
+    d = d / 100;
+    
+    if(a > 0) { //正回転
+        _p01 = a;
+        _p10 = 0;
+    } else if(a == 0) { //ブレーキ
+        _p01 = 1;
+        _p10 = 1;//0か1だと思う
+    } else { //負回転
+        _p01 = 0;
+        _p10 = -1*a;
+    }
+    if(b > 0) {
+        _p02 = b;
+        _p20 = 0;
+    } else if(b == 0) {
+        _p02 = 1;
+        _p20 = 1;
+    } else {
+        _p02 = 0;
+        _p20 = -1*b;
+    }
+    if(c > 0) {
+        _p03 = c;
+        _p30 = 0;
+    } else if(c == 0) {
+        _p03 = 1;
+        _p30 = 1;
+    } else {
+        _p03 = 0;
+        _p30 = -1*c;
+
+    }
+    if(d > 0) {
+        _p04 = d;
+        _p40 = 0;
+    } else if(d == 0) {
+        _p04 = 1;
+        _p40 = 1;
+    } else {
+        _p04 = 0;
+        _p40 = -1*d;
+    }
+}
+
+//*********************************************************************************/////////
+///////モーター制御θ(degree)に進行方向,powerにPWMの値(0～100),修正値をmodに代入,右回転基準//////////
+//********************************************************************************://///////
+/*4輪用*/
+void Motor::omni4Wheels(int degree,int power,int mod){
+    float motor[4];
+    float Max[3];
+    if(power == 0) { //パワー0のとき
+
+        motor[0] = -mod; //モーター右前
+        motor[1] = -mod; //モーター右後
+        motor[2] = -mod; //モーター左後
+        motor[3] = -mod; //モーター左前
+    } else {
+
+        motor[0] = sin((degree-45)*Pi/180) - mod; //モーター右前
+        motor[1] = sin((degree-135)*Pi/180) - mod; //モーター右後
+        motor[2] = sin((degree-225)*Pi/180) - mod; //モーター左後
+        motor[3] = sin((degree-315)*Pi/180) - mod; //モーター左前
+
+        if(motor[0]>1) {
+            motor[0] = 1;
+        } else if(motor[0] < -1) {
+            motor[0] = -1;
+        }
+        if(motor[1]>1) {
+            motor[1] = 1;
+        } else if(motor[1] < -1) {
+            motor[1] = -1;
+        }
+        if(motor[2]>1) {
+            motor[2] = 1;
+        } else if(motor[2] < -1) {
+            motor[2] = -1;
+        }
+        if(motor[3]>1) {
+            motor[3] = 1;
+        } else if(motor[3] < -1) {
+            motor[3] = -1;
+        }
+
+
+        if(fabs(motor[0]) >= fabs(motor[1])) { //モーターパワー最大値計算
+            Max[0] = fabs(motor[0]);
+        } else {
+            Max[0] = fabs(motor[1]);
+        }
+        if(fabs(motor[2]) >= fabs(motor[3])) {
+            Max[1] = fabs(motor[2]);
+        } else {
+            Max[1] = fabs(motor[3]);
+        }
+        if(Max[0] >= Max[1]) {
+            Max[2] = Max[0];
+        } else {
+            Max[2] = Max[1];
+        }
+
+        motor[0] = (power*(motor[0]/Max[2]));//モータパワー最大値修正
+        motor[1] = (power*(motor[1]/Max[2]));
+        motor[2] = (power*(motor[2]/Max[2]));
+        motor[3] = (power*(motor[3]/Max[2]));
+    }
+    
+    setPower(motor[0],motor[1],motor[2],motor[3]);
+}
+
+
+/*3輪用*/
+void Motor::omni3Wheels(int degree,int power,int mod){
+    float motor[3];//モーター用変数
+    float Max[2];//最大値用変数
+    
+    if(power == 0){//パワー0のとき
+        
+        motor[0] = -mod; //モーター右
+        motor[1] = -mod; //モーター後
+        motor[2] = -mod; //モーター左
+    }
+    else{ 
+    
+        motor[0] = sin((degree-60)*Pi/180) - mod; //モーター右
+        motor[1] = sin((degree-180)*Pi/180) - mod; //モーター後
+        motor[2] = sin((degree-300)*Pi/180) - mod; //モーター左
+        
+        if(motor[0]>1){
+            motor[0] = 1;
+        }
+        else if(motor[0] < -1){
+            motor[0] = -1;
+        }
+        if(motor[1]>1){
+            motor[1] = 1;
+        }
+        else if(motor[1] < -1){
+            motor[1] = -1;
+        }
+        if(motor[2]>1){
+            motor[2] = 1;
+        }
+        else if(motor[2] < -1){
+            motor[2] = -1;
+        }
+        
+    
+        if(fabs(motor[0]) >= fabs(motor[1])){ //モーターパワー最大値計算
+            Max[0] = fabs(motor[0]);
+        }
+        else{
+            Max[0] = fabs(motor[1]);
+        }
+        if(fabs(motor[2]) >= Max[0]){
+            Max[1] = fabs(motor[2]);
+        }
+        else{
+            Max[1] = Max[0];//最大値
+        }
+        
+    
+        motor[0] = (power*(motor[0]/Max[1]));//モータパワー最大値修正
+        motor[1] = (power*(motor[1]/Max[1]));
+        motor[2] = (power*(motor[2]/Max[1]));
+    }
+    
+    setPower(motor[0],motor[1],motor[2],0);
+    
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor.h	Thu Oct 18 09:57:12 2018 +0000
@@ -0,0 +1,35 @@
+#include "mbed.h"
+
+class Motor
+{
+public:
+
+    Motor::Motor(PinName p01,PinName p10,PinName p02,PinName p20,PinName p03,PinName p30,PinName p04,PinName p40);
+    
+    /*******************************************/
+    /*初期設定(1周期を引数に取る)*/
+    /******************************************/
+    void setPwmPeriod(float hz);
+    
+    /****************************************************/
+    /*モーターを動かす*/
+    /****************************************************/
+    void setPower(float a,float b, float c, float d);
+    
+    //*********************************************************************************/////////
+    ///////モーター制御θ(degree)に進行方向,powerにPWMの値(0～100),修正値をmodに代入,右回転基準//////////
+    //********************************************************************************://///////
+    void omni4Wheels(int degree,int power,int mod);
+    void omni3Wheels(int degree,int power,int mod);
+
+private:
+    PwmOut _p01;
+    PwmOut _p10;
+    PwmOut _p02; 
+    PwmOut _p20;
+    PwmOut _p03;
+    PwmOut _p30;
+    PwmOut _p04;
+    PwmOut _p40;
+    const float Pi = 3.14159265; 
+};
\ No newline at end of file