F3RC1班 / Mbed 2 deprecated F3RC_syudou_BUS_master

通信変えたやつです

Dependencies:   mbed

Fork of F3RC_syudou_master_3 by F3RC1班

User.cpp@24:3610bcb8e275, 2017-08-25 (annotated)

Committer:
yuto17320508
Date:
Fri Aug 25 12:03:32 2017 +0000
Revision:
24:3610bcb8e275
Parent:
23:33a40d8c0535
Child:
25:d5588a50f069
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hirokimineshita 0:736c76a75def 1 #include "Utils.h"
hirokimineshita 0:736c76a75def 2 #include "USBHost.h"
hirokimineshita 0:736c76a75def 3 #include "hci.h"
hirokimineshita 0:736c76a75def 4 #include "ps3.h"
hirokimineshita 0:736c76a75def 5 #include "User.h"
hirokimineshita 0:736c76a75def 6 #include "mbed.h"
yuto17320508 18:2579c275ef57 7 #define _USE_MATH_DEFINES
hirokimineshita 0:736c76a75def 8
yuto17320508 18:2579c275ef57 9 #include "math.h"
yuto17320508 18:2579c275ef57 10
yuto17320508 18:2579c275ef57 11 #define Pi 3.14159
hirokimineshita 0:736c76a75def 12 int RSX,RSY,LSX,LSY,BSU,BSL;
hirokimineshita 9:e5437ba3e920 13 //これより下に関数外に書く要素を記入する
yuto17320508 24:3610bcb8e275 14 //BUS通信用
yuto17320508 24:3610bcb8e275 15 BusOut out(p5,p6,p7,p8);
yuto17320508 24:3610bcb8e275 16 int num;
yuto17320508 24:3610bcb8e275 17
yuto17320508 18:2579c275ef57 18 //オムニホイール
yuto17320508 18:2579c275ef57 19
yuto17320508 18:2579c275ef57 20 /*　　　正転の向き
yuto17320508 21:d9f5b74d5034 21 　　　　ｌ↙　　　↖ｆ　　　　
yuto17320508 21:d9f5b74d5034 22 　　　　　　　　　　　　　　
yuto17320508 21:d9f5b74d5034 23 　　　　　 　→
yuto17320508 21:d9f5b74d5034 24 　　　　　　ｒ　　　　　　 */
yuto17320508 18:2579c275ef57 25 PwmOut motor_f_1(p21);
yuto17320508 18:2579c275ef57 26 PwmOut motor_f_2(p22);
yuto17320508 18:2579c275ef57 27 PwmOut motor_l_1(p23);
yuto17320508 18:2579c275ef57 28 PwmOut motor_l_2(p24);
yuto17320508 18:2579c275ef57 29 PwmOut motor_r_1(p25);
yuto17320508 18:2579c275ef57 30 PwmOut motor_r_2(p26);
yuto17320508 18:2579c275ef57 31
yuto17320508 21:d9f5b74d5034 32 double fai=60;//φ
yuto17320508 21:d9f5b74d5034 33 //個体差で出力調整
yuto17320508 23:33a40d8c0535 34 double power_f=0.2;
yuto17320508 23:33a40d8c0535 35 double power_l=0.2;
yuto17320508 23:33a40d8c0535 36 double power_r=0.2;
yuto17320508 18:2579c275ef57 37
yuto17320508 18:2579c275ef57 38 double M1;
yuto17320508 18:2579c275ef57 39 double M2;
yuto17320508 18:2579c275ef57 40 double M3;
yuto17320508 20:b84beed117ef 41
yuto17320508 20:b84beed117ef 42 //ジョイスティックの中心座標
yuto17320508 20:b84beed117ef 43 double center=127;
yuto17320508 20:b84beed117ef 44
yuto17320508 20:b84beed117ef 45
yuto17320508 20:b84beed117ef 46 //ジョイスティック閾値
yuto17320508 20:b84beed117ef 47 double delta=90;
yuto17320508 20:b84beed117ef 48 double bound_p=center+delta;
yuto17320508 20:b84beed117ef 49 double bound_m=center-delta;
yuto17320508 20:b84beed117ef 50
yuto17320508 20:b84beed117ef 51
yuto17320508 19:a3f57c9833b6 52 //回転の比
yuto17320508 19:a3f57c9833b6 53 double roll_spd=0.5;
yuto17320508 22:e88dd3acec2b 54 //モーターの動作
yuto17320508 18:2579c275ef57 55 void motor_act()
yuto17320508 18:2579c275ef57 56 {
yuto17320508 18:2579c275ef57 57 if(M1 >=0) {
yuto17320508 18:2579c275ef57 58 motor_f_1=M1;
yuto17320508 18:2579c275ef57 59 motor_f_2=0;
yuto17320508 18:2579c275ef57 60 } else {
yuto17320508 18:2579c275ef57 61 motor_f_1=0;
yuto17320508 18:2579c275ef57 62 motor_f_2=-M1;
yuto17320508 18:2579c275ef57 63 }
yuto17320508 18:2579c275ef57 64 if(M2 >=0) {
yuto17320508 18:2579c275ef57 65 motor_l_1=M2;
yuto17320508 18:2579c275ef57 66 motor_l_2=0;
yuto17320508 18:2579c275ef57 67 } else {
yuto17320508 18:2579c275ef57 68 motor_l_1=0;
yuto17320508 18:2579c275ef57 69 motor_l_2=-M2;
yuto17320508 18:2579c275ef57 70 }
yuto17320508 18:2579c275ef57 71 if(M3 >=0) {
yuto17320508 18:2579c275ef57 72 motor_r_1=M3;
yuto17320508 18:2579c275ef57 73 motor_r_2=0;
yuto17320508 18:2579c275ef57 74 } else {
yuto17320508 18:2579c275ef57 75 motor_r_1=0;
yuto17320508 18:2579c275ef57 76 motor_r_2=-M3;
yuto17320508 18:2579c275ef57 77 }
yuto17320508 18:2579c275ef57 78
yuto17320508 18:2579c275ef57 79 }
yuto17320508 22:e88dd3acec2b 80 //ジョイスティック入力値の偏角
yuto17320508 18:2579c275ef57 81 double sita;
yuto17320508 22:e88dd3acec2b 82 //関数代入用の角度調整
yuto17320508 18:2579c275ef57 83 double sita_2;
yuto17320508 18:2579c275ef57 84
yuto17320508 16:b232fd9ee9c2 85
yuto17320508 16:b232fd9ee9c2 86 void UserLoopSetting()
yuto17320508 16:b232fd9ee9c2 87 {
yuto17320508 24:3610bcb8e275 88
yuto17320508 23:33a40d8c0535 89 motor_f_1.period_us(100);
yuto17320508 23:33a40d8c0535 90 motor_f_2.period_us(100);
yuto17320508 23:33a40d8c0535 91 motor_l_1.period_us(100);
yuto17320508 23:33a40d8c0535 92 motor_l_2.period_us(100);
yuto17320508 23:33a40d8c0535 93 motor_r_1.period_us(100);
yuto17320508 23:33a40d8c0535 94 motor_r_2.period_us(100);
yuto17320508 18:2579c275ef57 95
hirokimineshita 0:736c76a75def 96 }
hirokimineshita 0:736c76a75def 97
yuto17320508 16:b232fd9ee9c2 98 void UserLoop(char n,const u8* data)
yuto17320508 16:b232fd9ee9c2 99 {
hirokimineshita 0:736c76a75def 100 u16 ButtonState;
yuto17320508 16:b232fd9ee9c2 101 if(n==0) { //有線Ps3USB.cpp
hirokimineshita 0:736c76a75def 102 RSX = ((ps3report*)data)->RightStickX;
hirokimineshita 0:736c76a75def 103 RSY = ((ps3report*)data)->RightStickY;
hirokimineshita 0:736c76a75def 104 LSX = ((ps3report*)data)->LeftStickX;
hirokimineshita 0:736c76a75def 105 LSY = ((ps3report*)data)->LeftStickY;
hirokimineshita 0:736c76a75def 106 BSU = (u8)(((ps3report*)data)->ButtonState & 0x00ff);
hirokimineshita 0:736c76a75def 107 BSL = (u8)(((ps3report*)data)->ButtonState >> 8);
hirokimineshita 0:736c76a75def 108 //ボタンの処理
hirokimineshita 0:736c76a75def 109 ButtonState = ((ps3report*)data)->ButtonState;
yuto17320508 16:b232fd9ee9c2 110 } else {//無線TestShell.cpp
hirokimineshita 0:736c76a75def 111 RSX = ((ps3report*)(data + 1))->RightStickX;
hirokimineshita 0:736c76a75def 112 RSY = ((ps3report*)(data + 1))->RightStickY;
hirokimineshita 0:736c76a75def 113 LSX = ((ps3report*)(data + 1))->LeftStickX;
hirokimineshita 0:736c76a75def 114 LSY = ((ps3report*)(data + 1))->LeftStickY;
hirokimineshita 0:736c76a75def 115 BSU = (u8)(((ps3report*)(data + 1))->ButtonState & 0x00ff);
hirokimineshita 0:736c76a75def 116 BSL = (u8)(((ps3report*)(data + 1))->ButtonState >> 8);
hirokimineshita 0:736c76a75def 117 //ボタンの処理
hirokimineshita 0:736c76a75def 118 ButtonState = ((ps3report*)(data + 1))->ButtonState;
hirokimineshita 0:736c76a75def 119 }
hirokimineshita 0:736c76a75def 120 //ここより下にプログラムを書く
yuto17320508 16:b232fd9ee9c2 121 //spi通信用プログラム
yuto17320508 16:b232fd9ee9c2 122
yuto17320508 20:b84beed117ef 123 int L_up,L_down,L_open,L_close,R_up,R_down,R_open,R_close;
yuto17320508 16:b232fd9ee9c2 124 int send = 0;
yuto17320508 16:b232fd9ee9c2 125
yuto17320508 16:b232fd9ee9c2 126
yuto17320508 18:2579c275ef57 127
yuto17320508 18:2579c275ef57 128 if((ButtonState >> BUTTONUP)&1 == 1) {//対応するボタンを書く(今回上ボタン
yuto17320508 24:3610bcb8e275 129 num = 1;
yuto17320508 24:3610bcb8e275 130 } else if((ButtonState >> BUTTONDOWN)&1 == 1) { //対応するボタンを書く(今回下ボタン
yuto17320508 24:3610bcb8e275 131 num = 2;
yuto17320508 24:3610bcb8e275 132 } else if((ButtonState >> BUTTONL1)&1 == 1) { //対応するボタンを書く(今回L1ボタン
yuto17320508 24:3610bcb8e275 133 num = 3;
yuto17320508 24:3610bcb8e275 134 } else if((ButtonState >> BUTTONL2)&1 == 1) { //対応するボタンを書く(今回L2ボタン
yuto17320508 24:3610bcb8e275 135 num = 4;
yuto17320508 24:3610bcb8e275 136 } else if((ButtonState >> BUTTONTRIANGEL)&1 == 1) { //対応するボタンを書く(今回△ボタン
yuto17320508 24:3610bcb8e275 137 num = 5;
yuto17320508 24:3610bcb8e275 138 } else if((ButtonState >> BUTTONCROSS)&1 == 1) { //対応するボタンを書く(今回×ボタン
yuto17320508 24:3610bcb8e275 139 num = 6;
yuto17320508 24:3610bcb8e275 140 } else if((ButtonState >> BUTTONR1)&1 == 1) { //対応するボタンを書く(今回R1ボタン
yuto17320508 24:3610bcb8e275 141 num = 7;
yuto17320508 24:3610bcb8e275 142 } else if((ButtonState >> BUTTONR2)&1 == 1) { //対応するボタンを書く(今回R2ボタン
yuto17320508 24:3610bcb8e275 143 num = 8;
yuto17320508 24:3610bcb8e275 144 } else
yuto17320508 24:3610bcb8e275 145 num=0;
yuto17320508 16:b232fd9ee9c2 146
yuto17320508 24:3610bcb8e275 147 out=num;
yuto17320508 24:3610bcb8e275 148 printf("%d\r\n",num);
yuto17320508 22:e88dd3acec2b 149 //オムニホイールのプログラム
yuto17320508 20:b84beed117ef 150 if(LSX>=bound_m && LSX<=bound_p && LSY>=bound_m && LSY<=bound_p) {
yuto17320508 19:a3f57c9833b6 151 M1=0;
yuto17320508 19:a3f57c9833b6 152 M2=0;
yuto17320508 19:a3f57c9833b6 153 M3=0;
yuto17320508 20:b84beed117ef 154 if(RSX>=bound_p && RSX<=255) {
yuto17320508 19:a3f57c9833b6 155 M1=power_f*roll_spd;
yuto17320508 19:a3f57c9833b6 156 M2=power_l*roll_spd;
yuto17320508 19:a3f57c9833b6 157 M3=power_r*roll_spd;
yuto17320508 20:b84beed117ef 158 } else if(RSX>=0 && RSX<=bound_m) {
yuto17320508 19:a3f57c9833b6 159 M1=-1.0*power_f*roll_spd;
yuto17320508 19:a3f57c9833b6 160 M2=-1.0*power_l*roll_spd;
yuto17320508 19:a3f57c9833b6 161 M3=-1.0*power_r*roll_spd;
yuto17320508 19:a3f57c9833b6 162 }
yuto17320508 19:a3f57c9833b6 163
yuto17320508 18:2579c275ef57 164 motor_act();
yuto17320508 16:b232fd9ee9c2 165
yuto17320508 19:a3f57c9833b6 166 sita=0;
yuto17320508 19:a3f57c9833b6 167 } else {
yuto17320508 24:3610bcb8e275 168 sita = -1.0*(atan2((double)LSY-center,(double)LSX-center))*180/Pi;
yuto17320508 24:3610bcb8e275 169 sita_2=90-sita;
yuto17320508 24:3610bcb8e275 170 M1=sin((sita_2-(fai+0))*Pi/180)*power_f;
yuto17320508 24:3610bcb8e275 171 M2=sin((sita_2-(fai+240))*Pi/180)*power_l;
yuto17320508 24:3610bcb8e275 172 M3=sin((sita_2-(fai+120))*Pi/180)*power_r;
yuto17320508 24:3610bcb8e275 173 motor_act();
yuto17320508 24:3610bcb8e275 174 }
yuto17320508 19:a3f57c9833b6 175 //真っすぐだけのプログラム(いらない）
yuto17320508 20:b84beed117ef 176
yuto17320508 24:3610bcb8e275 177 /*else if(LSX==255) {
yuto17320508 24:3610bcb8e275 178 sita = 0;
yuto17320508 24:3610bcb8e275 179 sita_2=90-sita;
yuto17320508 24:3610bcb8e275 180 M1=sin((sita_2-fai)*Pi/180)*power_f;
yuto17320508 24:3610bcb8e275 181 M2=sin((sita_2-fai+240)*Pi/180)*power_l;
yuto17320508 24:3610bcb8e275 182 M3=sin((sita_2-fai+120)*Pi/180)*power_r;
yuto17320508 24:3610bcb8e275 183 motor_act();
yuto17320508 18:2579c275ef57 184
yuto17320508 24:3610bcb8e275 185 } else if(LSY==255) {
yuto17320508 24:3610bcb8e275 186 sita = -90;
yuto17320508 24:3610bcb8e275 187 sita_2=90-sita;
yuto17320508 24:3610bcb8e275 188 M1=sin((sita_2-fai)*Pi/180)*power_f;
yuto17320508 24:3610bcb8e275 189 M2=sin((sita_2-fai+240)*Pi/180)*power_l;
yuto17320508 24:3610bcb8e275 190 M3=sin((sita_2-fai+120)*Pi/180)*power_r;
yuto17320508 24:3610bcb8e275 191 motor_act();
yuto17320508 19:a3f57c9833b6 192
yuto17320508 24:3610bcb8e275 193 } else if(LSX==0) {
yuto17320508 24:3610bcb8e275 194 sita = 180;
yuto17320508 24:3610bcb8e275 195 sita_2=90-sita;
yuto17320508 24:3610bcb8e275 196 M1=sin((sita_2-fai)*Pi/180)*power_f;
yuto17320508 24:3610bcb8e275 197 M2=sin((sita_2-fai+240)*Pi/180)*power_l;
yuto17320508 24:3610bcb8e275 198 M3=sin((sita_2-fai+120)*Pi/180)*power_r;
yuto17320508 24:3610bcb8e275 199 motor_act();
yuto17320508 19:a3f57c9833b6 200
yuto17320508 24:3610bcb8e275 201 } else if(LSY==0) {
yuto17320508 24:3610bcb8e275 202 sita = 90;
yuto17320508 24:3610bcb8e275 203 sita_2=90-sita;
yuto17320508 24:3610bcb8e275 204 M1=sin((sita_2-fai)*Pi/180)*power_f;
yuto17320508 24:3610bcb8e275 205 M2=sin((sita_2-fai+240)*Pi/180)*power_l;
yuto17320508 24:3610bcb8e275 206 M3=sin((sita_2-fai+120)*Pi/180)*power_r;
yuto17320508 24:3610bcb8e275 207 motor_act();
yuto17320508 24:3610bcb8e275 208 } */
yuto17320508 16:b232fd9ee9c2 209
yuto17320508 24:3610bcb8e275 210
yuto17320508 18:2579c275ef57 211
yuto17320508 18:2579c275ef57 212
yuto17320508 19:a3f57c9833b6 213
yuto17320508 19:a3f57c9833b6 214
yuto17320508 20:b84beed117ef 215 printf("motor_f_1:%.4f\t\motor_l_1:%.4f\t\motor_r_1:%.4f\t\sita:%f\r\n",M1,M2,M3,sita);
yuto17320508 18:2579c275ef57 216
yuto17320508 18:2579c275ef57 217
yuto17320508 18:2579c275ef57 218
hirokimineshita 0:736c76a75def 219 }