F3RC1班 / Mbed 2 deprecated F3RC_syudou_master

主導機　mbed用のプログラムです

Dependencies:   mbed

Fork of PS3_BlueUSB_user_ver2 by 2017 　部内対抗4班　ROBOSTEP

Files at this revision

API Documentation at this revision

Revision 18:2579c275ef57, committed 2017-08-23

Comitter:
yuto17320508
Date:
Wed Aug 23 03:31:34 2017 +0000
Parent:
17:c5c41fcf316e
Commit message:
a

Changed in this revision

User.cpp Show annotated file Show diff for this revision Revisions of this file 
diff -r c5c41fcf316e -r 2579c275ef57 User.cpp
--- a/User.cpp	Mon Aug 21 08:43:52 2017 +0000
+++ b/User.cpp	Wed Aug 23 03:31:34 2017 +0000
@@ -4,18 +4,85 @@
 #include "ps3.h"
 #include "User.h"
 #include "mbed.h"
+#define _USE_MATH_DEFINES
 
+#include "math.h"
+
+#define Pi 3.14159
 int RSX,RSY,LSX,LSY,BSU,BSL;
 //これより下に関数外に書く要素を記入する
 //spi通信用
 SPI spi(p5,p6,p7);
 DigitalOut cs(p8);
+//オムニホイール
+
+/*　　　正転の向き
+    　　　　←
+
+　　　　　↘    ↗      */
+PwmOut motor_f_1(p21);
+PwmOut motor_f_2(p22);
+PwmOut motor_l_1(p23);
+PwmOut motor_l_2(p24);
+PwmOut motor_r_1(p25);
+PwmOut motor_r_2(p26);
+
+double fai=1/3*Pi;//φ
+
+
+double power_f=1;
+double power_l=1;
+double power_r=1;
+
+double M1;
+double M2;
+double M3;
+
+void motor_act()
+{
+    if(M1 >=0) {
+        motor_f_1=M1;
+        motor_f_2=0;
+    } else {
+        motor_f_1=0;
+        motor_f_2=-M1;
+    }
+
+    if(M2 >=0) {
+        motor_l_1=M2;
+        motor_l_2=0;
+    } else {
+        motor_l_1=0;
+        motor_l_2=-M2;
+    }
+
+    if(M3 >=0) {
+        motor_r_1=M3;
+        motor_r_2=0;
+    } else {
+        motor_r_1=0;
+        motor_r_2=-M3;
+    }
+
+
+
+}
+double sita;
+double sita_2;
+
 
 
 void UserLoopSetting()
 {
     spi.format(8,3);
     spi.frequency(1000000);
+    motor_f_1.period_us(50);
+    motor_f_2.period_us(50);
+    motor_l_1.period_us(50);
+    motor_l_2.period_us(50);
+    motor_r_1.period_us(50);
+    motor_r_2.period_us(50);
+
 }
 
 void UserLoop(char n,const u8* data)
@@ -47,63 +114,138 @@
     int send = 0;
 
 
-    while(1) {
-        if((ButtonState >> BUTTONUP)&1 == 1) {//対応するボタンを書く(今回上ボタン
-            a = 1;
-        } else {
-            a = 0;
-        }
+
+    if((ButtonState >> BUTTONUP)&1 == 1) {//対応するボタンを書く(今回上ボタン
+        a = 1;
+    } else {
+        a = 0;
+    }
 
-        if((ButtonState >> BUTTONDOWN)&1 == 1) {//対応するボタンを書く(今回下ボタン
-            b = 2;
-        } else {
-            b = 0;
-        }
+    if((ButtonState >> BUTTONDOWN)&1 == 1) {//対応するボタンを書く(今回下ボタン
+        b = 2;
+    } else {
+        b = 0;
+    }
 
-        if((ButtonState >> BUTTONL1)&1 == 1) {//対応するボタンを書く(今回L1ボタン
-            c = 4;
-        } else {
-            c = 0;
-        }
+    if((ButtonState >> BUTTONL1)&1 == 1) {//対応するボタンを書く(今回L1ボタン
+        c = 4;
+    } else {
+        c = 0;
+    }
 
-        if((ButtonState >> BUTTONL2)&1 == 1) {//対応するボタンを書く(今回L2ボタン
-            d = 8;
-        } else {
-            d = 0;
-        }
+    if((ButtonState >> BUTTONL2)&1 == 1) {//対応するボタンを書く(今回L2ボタン
+        d = 8;
+    } else {
+        d = 0;
+    }
 
-        if((ButtonState >> BUTTONTRIANGEL)&1 == 1) {//対応するボタンを書く(今回△ボタン
-            e = 16;
-        } else {
-            e = 0;
-        }
+    if((ButtonState >> BUTTONTRIANGEL)&1 == 1) {//対応するボタンを書く(今回△ボタン
+        e = 16;
+    } else {
+        e = 0;
+    }
 
-        if((ButtonState >> BUTTONCROSS)&1 == 1) {//対応するボタンを書く(今回×ボタン
-            f = 32;
-        } else {
-            f = 0;
-        }
+    if((ButtonState >> BUTTONCROSS)&1 == 1) {//対応するボタンを書く(今回×ボタン
+        f = 32;
+    } else {
+        f = 0;
+    }
 
-        if((ButtonState >> BUTTONR1)&1 == 1) {//対応するボタンを書く(今回R1ボタン
-            g = 64;
-        } else {
-            g = 0;
-        }
+    if((ButtonState >> BUTTONR1)&1 == 1) {//対応するボタンを書く(今回R1ボタン
+        g = 64;
+    } else {
+        g = 0;
+    }
 
-        if((ButtonState >> BUTTONR2)&1 == 1) {//対応するボタンを書く(今回R2ボタン
-            h = 128;
-        } else {
-            h = 0;
-        }
+    if((ButtonState >> BUTTONR2)&1 == 1) {//対応するボタンを書く(今回R2ボタン
+        h = 128;
+    } else {
+        h = 0;
+    }
 
 
 
-        send = a+b+c+d+e+f+g+h;
+    send = a+b+c+d+e+f+g+h;
+
+    cs = 0;
+    spi. write(send);
+    cs = 1;
+    printf("%d\r\n",send);
+
+
+    if(LSX>=150 && LSX<=255 && LSY>=150 && LSY<=255) { //第一象限
+        sita = -1.0*(atan2((double)LSY-128,(double)LSX-128))*180/Pi;
+        sita_2=90-sita;
+        M1=sin((sita_2-fai)*Pi/180)*power_f;
+        M2=sin((sita_2-fai+240)*Pi/180)*power_l;
+        M3=sin((sita_2-fai+120)*Pi/180)*power_r;
+        motor_act();
+    } else if(LSX>=0 && LSX<=80 && LSY>=150 && LSY<=255) { //第二象限
+        sita = -1.0*(atan2((double)LSY-128,(double)LSX-128))*180/Pi;
+        sita_2=90-sita;
+        M1=sin((sita_2-fai)*Pi/180)*power_f;
+        M2=sin((sita_2-fai+240)*Pi/180)*power_l;
+        M3=sin((sita_2-fai+120)*Pi/180)*power_r;
+        motor_act();
+    } else if(LSX>=0 && LSX<=80 && LSY>=0 && LSY<=80) { //第三象限
+        sita = -1.0*(atan2((double)LSY-128,(double)LSX-128))*180/Pi;
+        sita_2=90-sita;
+        M1=sin((sita_2-fai)*Pi/180)*power_f;
+        M2=sin((sita_2-fai+240)*Pi/180)*power_l;
+        M3=sin((sita_2-fai+120)*Pi/180)*power_r;
+        motor_act();
+    } else if(LSX>=150 && LSX<=255 && LSY>=0 && LSY<=80) { //第四象限
+        sita = -1.0*(atan2((double)LSY-128,(double)LSX-128))*180/Pi;
+        sita_2=90-sita;
+        M1=sin((sita_2-fai)*Pi/180)*power_f;
+        M2=sin((sita_2-fai+240)*Pi/180)*power_l;
+        M3=sin((sita_2-fai+120)*Pi/180)*power_r;
+        motor_act();
 
-        cs = 0;
-        spi. write(send);
-        cs = 1;
-        printf("%d\r\n",send);
+    } else if(LSX==255) {
+        sita = 0;
+        sita_2=90-sita;
+        M1=sin((sita_2-fai)*Pi/180)*power_f;
+        M2=sin((sita_2-fai+240)*Pi/180)*power_l;
+        M3=sin((sita_2-fai+120)*Pi/180)*power_r;
+        motor_act();
+
+    } else if(LSY==255) {
+        sita = 90;
+        sita_2=90-sita;
+        M1=sin((sita_2-fai)*Pi/180)*power_f;
+        M2=sin((sita_2-fai+240)*Pi/180)*power_l;
+        M3=sin((sita_2-fai+120)*Pi/180)*power_r;
+        motor_act();
+
+    } else if(LSX==0) {
+        sita = 180;
+        sita_2=90-sita;
+        M1=sin((sita_2-fai)*Pi/180)*power_f;
+        M2=sin((sita_2-fai+240)*Pi/180)*power_l;
+        M3=sin((sita_2-fai+120)*Pi/180)*power_r;
+        motor_act();
+
+    } else if(LSY==0) {
+        sita = 270;
+        sita_2=90-sita;
+        M1=sin((sita_2-fai)*Pi/180)*power_f;
+        M2=sin((sita_2-fai+240)*Pi/180)*power_l;
+        M3=sin((sita_2-fai+120)*Pi/180)*power_r;
+        motor_act();
+    } else {
+        motor_f_1=0;
+        motor_f_2=0;
+        motor_l_1=0;
+        motor_l_2=0;
+        motor_r_1=0;
+        motor_r_2=0;
 
     }
+
+
+    printf("motor_f_1:%f\motor_l_1:%f\motor_r_1:%f\sita:%f\r\n",M1,M2,M3,sita);
+
+
+
 }
\ No newline at end of file