yuto kawamura
主導機 mbed用のプログラムです
Fork of
F3RC_syudou_master
by 
F3RC1班
Diff: User.cpp
- Revision:
- 18:2579c275ef57
- Parent:
- 17:c5c41fcf316e
- Child:
- 19:a3f57c9833b6
--- 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