F3RC1班
主導機 mbed用のプログラムです 改良しました
Dependencies: mbed
Fork of
F3RC_syudou_master_3
by 
日記
Diff: User.cpp
- Revision:
- 26:91dd637de4d4
- Parent:
- 25:d5588a50f069
--- a/User.cpp Fri Aug 25 12:23:18 2017 +0000
+++ b/User.cpp Thu Sep 14 01:20:12 2017 +0000
@@ -31,29 +31,35 @@
double fai=60;//φ
//個体差で出力調整
-double power_f=0.2;
-double power_l=0.2;
-double power_r=0.2;
+double power_f=0.32;
+double power_l=0.32;
+double power_r=0.32;
double M1;
double M2;
double M3;
+double accel=1.3;
//ジョイスティックの中心座標
double center=127;
-
//ジョイスティック閾値
double delta=90;
double bound_p=center+delta;
double bound_m=center-delta;
+//回転の比
+ //移動中の回転
+double roll_spd=0.4;
+ //横移動中の回転
+double roll_spd2=0.1;
+ //横移動のスピード
+double yoko_spd=0.9;
-//回転の比
-double roll_spd=0.5;
//モーターの動作
void motor_act()
{
+ //絶対値をつける関数
if(M1 >=0) {
motor_f_1=M1;
motor_f_2=0;
@@ -75,24 +81,20 @@
motor_r_1=0;
motor_r_2=-M3;
}
-
}
//ジョイスティック入力値の偏角
double sita;
//関数代入用の角度調整
double sita_2;
-
void UserLoopSetting()
{
-
motor_f_1.period_us(100);
motor_f_2.period_us(100);
motor_l_1.period_us(100);
motor_l_2.period_us(100);
motor_r_1.period_us(100);
motor_r_2.period_us(100);
-
}
void UserLoop(char n,const u8* data)
@@ -118,41 +120,38 @@
ButtonState = ((ps3report*)(data + 1))->ButtonState;
}
//ここより下にプログラムを書く
- //spi通信用プログラム
-
+ //BUS用プログラム
int flag=0;
num=0;
-
-
- if((ButtonState >> BUTTONUP)&1 == 1) {//対応するボタンを書く(今回上ボタン
+ if((ButtonState >> BUTTONTRIANGEL)&1 == 1) {//対応するボタンを書く
num = 1;
flag=flag+1;
}
- if((ButtonState >> BUTTONDOWN)&1 == 1) { //対応するボタンを書く(今回下ボタン
+ if((ButtonState >> BUTTONCROSS)&1 == 1) { //対応するボタンを書く
num = 2;
flag=flag+1;
}
- if((ButtonState >> BUTTONL1)&1 == 1) { //対応するボタンを書く(今回L1ボタン
+ if((ButtonState >> BUTTONR1)&1 == 1) { //対応するボタンを書く
num = 3;
flag=flag+1;
}
- if((ButtonState >> BUTTONL2)&1 == 1) { //対応するボタンを書く(今回L2ボタン
+ if((ButtonState >> BUTTONR2)&1 == 1) { //対応するボタンを書く
num = 4;
flag=flag+1;
}
- if((ButtonState >> BUTTONTRIANGEL)&1 == 1) { //対応するボタンを書く(今回△ボタン
+ if((ButtonState >> BUTTONUP)&1 == 1) { //対応するボタンを書く
num = 5;
flag=flag+1;
}
- if((ButtonState >> BUTTONCROSS)&1 == 1) { //対応するボタンを書く(今回×ボタン
+ if((ButtonState >> BUTTONDOWN)&1 == 1) { //対応するボタンを書く
num = 6;
flag=flag+1;
}
- if((ButtonState >> BUTTONR1)&1 == 1) { //対応するボタンを書く(今回R1ボタン
+ if((ButtonState >> BUTTONL1)&1 == 1) { //対応するボタンを書く
num = 7;
flag=flag+1;
}
- if((ButtonState >> BUTTONR2)&1 == 1) { //対応するボタンを書く(今回R2ボタン
+ if((ButtonState >> BUTTONL2)&1 == 1) { //対応するボタンを書く
num = 8;
flag=flag+1;
}
@@ -162,73 +161,79 @@
out=num;
printf("%d\r\n",num);
//オムニホイールのプログラム
+
+ if((ButtonState >> BUTTONRANALOG)&1 == 1) {//押し込みで速度変化
+ roll_spd=0.8;
+ } else {
+ roll_spd=0.4;
+ }
if(LSX>=bound_m && LSX<=bound_p && LSY>=bound_m && LSY<=bound_p) {
M1=0;
M2=0;
M3=0;
- if(RSX>=bound_p && RSX<=255) {
+ if(RSX>=bound_p && RSX<=255) {//右スティック右
M1=power_f*roll_spd;
M2=power_l*roll_spd;
M3=power_r*roll_spd;
- } else if(RSX>=0 && RSX<=bound_m) {
+ } else if(RSX>=0 && RSX<=bound_m) {//右スティック左
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;
- sita=0;
- } else {
+ } else {//左スティック全方向
sita = -1.0*(atan2((double)LSY-center,(double)LSX-center))*180/Pi;
sita_2=90-sita;
M1=sin((sita_2-(fai+0))*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();
+ if(RSX>=bound_p && RSX<=255) {//進行中の右スティック右
+ M1=M1+power_f*roll_spd;
+ M2=M2+power_l*roll_spd;
+ M3=M3+power_r*roll_spd;
+ } else if(RSX>=0 && RSX<=bound_m) {//進行中の右スティック左
+ M1=M1+-1.0*power_f*roll_spd;
+ M2=M2+-1.0*power_l*roll_spd;
+ M3=M3+-1.0*power_r*roll_spd;
+ }
}
-//真っすぐだけのプログラム(いらない）
-
- /*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();
- } */
+ //真横移動
+ if((ButtonState >> BUTTONLEFT)&1 == 1) { //十字キー左
+ sita = 180;
+ sita_2=90-sita;
+ M1=sin((sita_2-(fai+0))*Pi/180)*power_f*yoko_spd;
+ M2=sin((sita_2-(fai+240))*Pi/180)*power_l*yoko_spd;
+ M3=sin((sita_2-(fai+120))*Pi/180)*power_r*yoko_spd;
+ if(RSX>=bound_p && RSX<=255) {//横移動中の右スティック右
+ M1=M1+power_f*roll_spd2;
+ M2=M2+power_l*roll_spd2;
+ M3=M3+power_r*roll_spd2;
+ } else if(RSX>=0 && RSX<=bound_m) {//横移動中の右スティック左
+ M1=M1+-1.0*power_f*roll_spd2;
+ M2=M2+-1.0*power_l*roll_spd2;
+ M3=M3+-1.0*power_r*roll_spd2;
+ }
+ }
+ if((ButtonState >> BUTTONRIGHT)&1 == 1) { //十字キー右
+ sita = 0;
+ sita_2=90-sita;
+ M1=sin((sita_2-(fai+0))*Pi/180)*power_f*yoko_spd;
+ M2=sin((sita_2-(fai+240))*Pi/180)*power_l*yoko_spd;
+ M3=sin((sita_2-(fai+120))*Pi/180)*power_r*yoko_spd;
+ if(RSX>=bound_p && RSX<=255) {//横移動中の右スティック右
+ M1=M1+power_f*roll_spd2;
+ M2=M2+power_l*roll_spd2;
+ M3=M3+power_r*roll_spd2;
+ } else if(RSX>=0 && RSX<=bound_m) {//横移動中の右スティック左
+ M1=M1+-1.0*power_f*roll_spd2;
+ M2=M2+-1.0*power_l*roll_spd2;
+ M3=M3+-1.0*power_r*roll_spd2;
+ }
+ }
+ motor_act();
-
-
-
+ printf("motor_f:%.4f\t\motor_l:%.4f\t\motor_r:%.4f\t\sita:%f\r\n",M1,M2,M3,sita);
- printf("motor_f_1:%.4f\t\motor_l_1:%.4f\t\motor_r_1:%.4f\t\sita:%f\r\n",M1,M2,M3,sita);
-
-
-
-}
\ No newline at end of file
+}