日記
/
F3RC_syudou_master_3
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed
Fork of
F3RC_syudou_master
by 
日記
User.cpp
- Committer:
- yuto17320508
- Date:
- 2017-08-23
- Revision:
- 19:a3f57c9833b6
- Parent:
- 18:2579c275ef57
- Child:
- 20:b84beed117ef
File content as of revision 19:a3f57c9833b6:
#include "Utils.h"
#include "USBHost.h"
#include "hci.h"
#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;
//回転の比
double roll_spd=0.5;
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)
{
u16 ButtonState;
if(n==0) { //有線Ps3USB.cpp
RSX = ((ps3report*)data)->RightStickX;
RSY = ((ps3report*)data)->RightStickY;
LSX = ((ps3report*)data)->LeftStickX;
LSY = ((ps3report*)data)->LeftStickY;
BSU = (u8)(((ps3report*)data)->ButtonState & 0x00ff);
BSL = (u8)(((ps3report*)data)->ButtonState >> 8);
//ボタンの処理
ButtonState = ((ps3report*)data)->ButtonState;
} else {//無線TestShell.cpp
RSX = ((ps3report*)(data + 1))->RightStickX;
RSY = ((ps3report*)(data + 1))->RightStickY;
LSX = ((ps3report*)(data + 1))->LeftStickX;
LSY = ((ps3report*)(data + 1))->LeftStickY;
BSU = (u8)(((ps3report*)(data + 1))->ButtonState & 0x00ff);
BSL = (u8)(((ps3report*)(data + 1))->ButtonState >> 8);
//ボタンの処理
ButtonState = ((ps3report*)(data + 1))->ButtonState;
}
//ここより下にプログラムを書く
//spi通信用プログラム
int a,b,c,d,e,f,g,h;
int send = 0;
if((ButtonState >> BUTTONUP)&1 == 1) {//対応するボタンを書く(今回上ボタン
a = 1;
} else {
a = 0;
}
if((ButtonState >> BUTTONDOWN)&1 == 1) {//対応するボタンを書く(今回下ボタン
b = 2;
} else {
b = 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 >> BUTTONTRIANGEL)&1 == 1) {//対応するボタンを書く(今回△ボタン
e = 16;
} else {
e = 0;
}
if((ButtonState >> BUTTONCROSS)&1 == 1) {//対応するボタンを書く(今回×ボタン
f = 32;
} else {
f = 0;
}
if((ButtonState >> BUTTONR1)&1 == 1) {//対応するボタンを書く(今回R1ボタン
g = 64;
} else {
g = 0;
}
if((ButtonState >> BUTTONR2)&1 == 1) {//対応するボタンを書く(今回R2ボタン
h = 128;
} else {
h = 0;
}
send = a+b+c+d+e+f+g+h;
cs = 0;
spi. write(send);
cs = 1;
// printf("%d\r\n",send);
//オムニホイールのプログラム
if(LSX>=110 && LSX<=145 && LSY>=110 && LSY<=145) {
M1=0;
M2=0;
M3=0;
if(RSX>=180 && RSX<=255) {
M1=power_f*roll_spd;
M2=power_l*roll_spd;
M3=power_r*roll_spd;
} else if(RSX>=0 && RSX<=80) {
M1=-1.0*power_f*roll_spd;
M2=-1.0*power_l*roll_spd;
M3=-1.0*power_r*roll_spd;
}
motor_act();
sita=0;
} else {
if(LSX>=129 && LSX<=255 && LSY>=129 && 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<=127 && LSY>=129 && 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<=127 && LSY>=0 && LSY<=127) { //第二象限
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>=129 && LSX<=255 && LSY>=0 && LSY<=127) { //第一象限
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==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 = 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();
} */
}
printf("motor_f_1:%f\motor_l_1:%f\motor_r_1:%f\sita:%f\r\n",M1,M2,M3,sita);
}