F3RC1班 / Mbed 2 deprecated F3RC_syudou_BUS_master

通信変えたやつです

Dependencies:   mbed

Fork of F3RC_syudou_master_3 by F3RC1班

User.cpp@21:d9f5b74d5034, 2017-08-23 (annotated)

Committer:
yuto17320508
Date:
Wed Aug 23 14:29:18 2017 +0000
Revision:
21:d9f5b74d5034
Parent:
20:b84beed117ef
Child:
22:e88dd3acec2b
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 16:b232fd9ee9c2 14 //spi通信用
yuto17320508 16:b232fd9ee9c2 15 SPI spi(p5,p6,p7);
yuto17320508 16:b232fd9ee9c2 16 DigitalOut cs(p8);
yuto17320508 18:2579c275ef57 17 //オムニホイール
yuto17320508 18:2579c275ef57 18
yuto17320508 18:2579c275ef57 19 /*　　　正転の向き
yuto17320508 21:d9f5b74d5034 20 　　　　ｌ↙　　　↖ｆ　　　　
yuto17320508 21:d9f5b74d5034 21 　　　　　　　　　　　　　　
yuto17320508 21:d9f5b74d5034 22 　　　　　 　→
yuto17320508 21:d9f5b74d5034 23 　　　　　　ｒ　　　　　　 */
yuto17320508 18:2579c275ef57 24 PwmOut motor_f_1(p21);
yuto17320508 18:2579c275ef57 25 PwmOut motor_f_2(p22);
yuto17320508 18:2579c275ef57 26 PwmOut motor_l_1(p23);
yuto17320508 18:2579c275ef57 27 PwmOut motor_l_2(p24);
yuto17320508 18:2579c275ef57 28 PwmOut motor_r_1(p25);
yuto17320508 18:2579c275ef57 29 PwmOut motor_r_2(p26);
yuto17320508 18:2579c275ef57 30
yuto17320508 21:d9f5b74d5034 31 double fai=60;//φ
yuto17320508 21:d9f5b74d5034 32 //個体差で出力調整
yuto17320508 20:b84beed117ef 33 double power_f=0.8;
yuto17320508 20:b84beed117ef 34 double power_l=0.8;
yuto17320508 20:b84beed117ef 35 double power_r=0.8;
yuto17320508 18:2579c275ef57 36
yuto17320508 18:2579c275ef57 37 double M1;
yuto17320508 18:2579c275ef57 38 double M2;
yuto17320508 18:2579c275ef57 39 double M3;
yuto17320508 20:b84beed117ef 40
yuto17320508 20:b84beed117ef 41 //ジョイスティックの中心座標
yuto17320508 20:b84beed117ef 42 double center=127;
yuto17320508 20:b84beed117ef 43
yuto17320508 20:b84beed117ef 44
yuto17320508 20:b84beed117ef 45 //ジョイスティック閾値
yuto17320508 20:b84beed117ef 46 double delta=90;
yuto17320508 20:b84beed117ef 47 double bound_p=center+delta;
yuto17320508 20:b84beed117ef 48 double bound_m=center-delta;
yuto17320508 20:b84beed117ef 49
yuto17320508 20:b84beed117ef 50
yuto17320508 19:a3f57c9833b6 51 //回転の比
yuto17320508 19:a3f57c9833b6 52 double roll_spd=0.5;
yuto17320508 18:2579c275ef57 53 void motor_act()
yuto17320508 18:2579c275ef57 54 {
yuto17320508 18:2579c275ef57 55 if(M1 >=0) {
yuto17320508 18:2579c275ef57 56 motor_f_1=M1;
yuto17320508 18:2579c275ef57 57 motor_f_2=0;
yuto17320508 18:2579c275ef57 58 } else {
yuto17320508 18:2579c275ef57 59 motor_f_1=0;
yuto17320508 18:2579c275ef57 60 motor_f_2=-M1;
yuto17320508 18:2579c275ef57 61 }
yuto17320508 18:2579c275ef57 62
yuto17320508 18:2579c275ef57 63 if(M2 >=0) {
yuto17320508 18:2579c275ef57 64 motor_l_1=M2;
yuto17320508 18:2579c275ef57 65 motor_l_2=0;
yuto17320508 18:2579c275ef57 66 } else {
yuto17320508 18:2579c275ef57 67 motor_l_1=0;
yuto17320508 18:2579c275ef57 68 motor_l_2=-M2;
yuto17320508 18:2579c275ef57 69 }
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 18:2579c275ef57 80
yuto17320508 18:2579c275ef57 81 }
yuto17320508 18:2579c275ef57 82 double sita;
yuto17320508 18:2579c275ef57 83 double sita_2;
yuto17320508 18:2579c275ef57 84
yuto17320508 16:b232fd9ee9c2 85
baba2357 11:86d717718dbf 86
yuto17320508 16:b232fd9ee9c2 87 void UserLoopSetting()
yuto17320508 16:b232fd9ee9c2 88 {
yuto17320508 17:c5c41fcf316e 89 spi.format(8,3);
yuto17320508 16:b232fd9ee9c2 90 spi.frequency(1000000);
yuto17320508 18:2579c275ef57 91 motor_f_1.period_us(50);
yuto17320508 18:2579c275ef57 92 motor_f_2.period_us(50);
yuto17320508 18:2579c275ef57 93 motor_l_1.period_us(50);
yuto17320508 18:2579c275ef57 94 motor_l_2.period_us(50);
yuto17320508 18:2579c275ef57 95 motor_r_1.period_us(50);
yuto17320508 18:2579c275ef57 96 motor_r_2.period_us(50);
yuto17320508 18:2579c275ef57 97
hirokimineshita 0:736c76a75def 98 }
hirokimineshita 0:736c76a75def 99
yuto17320508 16:b232fd9ee9c2 100 void UserLoop(char n,const u8* data)
yuto17320508 16:b232fd9ee9c2 101 {
hirokimineshita 0:736c76a75def 102 u16 ButtonState;
yuto17320508 16:b232fd9ee9c2 103 if(n==0) { //有線Ps3USB.cpp
hirokimineshita 0:736c76a75def 104 RSX = ((ps3report*)data)->RightStickX;
hirokimineshita 0:736c76a75def 105 RSY = ((ps3report*)data)->RightStickY;
hirokimineshita 0:736c76a75def 106 LSX = ((ps3report*)data)->LeftStickX;
hirokimineshita 0:736c76a75def 107 LSY = ((ps3report*)data)->LeftStickY;
hirokimineshita 0:736c76a75def 108 BSU = (u8)(((ps3report*)data)->ButtonState & 0x00ff);
hirokimineshita 0:736c76a75def 109 BSL = (u8)(((ps3report*)data)->ButtonState >> 8);
hirokimineshita 0:736c76a75def 110 //ボタンの処理
hirokimineshita 0:736c76a75def 111 ButtonState = ((ps3report*)data)->ButtonState;
yuto17320508 16:b232fd9ee9c2 112 } else {//無線TestShell.cpp
hirokimineshita 0:736c76a75def 113 RSX = ((ps3report*)(data + 1))->RightStickX;
hirokimineshita 0:736c76a75def 114 RSY = ((ps3report*)(data + 1))->RightStickY;
hirokimineshita 0:736c76a75def 115 LSX = ((ps3report*)(data + 1))->LeftStickX;
hirokimineshita 0:736c76a75def 116 LSY = ((ps3report*)(data + 1))->LeftStickY;
hirokimineshita 0:736c76a75def 117 BSU = (u8)(((ps3report*)(data + 1))->ButtonState & 0x00ff);
hirokimineshita 0:736c76a75def 118 BSL = (u8)(((ps3report*)(data + 1))->ButtonState >> 8);
hirokimineshita 0:736c76a75def 119 //ボタンの処理
hirokimineshita 0:736c76a75def 120 ButtonState = ((ps3report*)(data + 1))->ButtonState;
hirokimineshita 0:736c76a75def 121 }
hirokimineshita 0:736c76a75def 122 //ここより下にプログラムを書く
yuto17320508 16:b232fd9ee9c2 123 //spi通信用プログラム
yuto17320508 16:b232fd9ee9c2 124
yuto17320508 20:b84beed117ef 125 int L_up,L_down,L_open,L_close,R_up,R_down,R_open,R_close;
yuto17320508 16:b232fd9ee9c2 126 int send = 0;
yuto17320508 16:b232fd9ee9c2 127
yuto17320508 16:b232fd9ee9c2 128
yuto17320508 18:2579c275ef57 129
yuto17320508 18:2579c275ef57 130 if((ButtonState >> BUTTONUP)&1 == 1) {//対応するボタンを書く(今回上ボタン
yuto17320508 20:b84beed117ef 131 L_up = 1;
yuto17320508 18:2579c275ef57 132 } else {
yuto17320508 20:b84beed117ef 133 L_up = 0;
yuto17320508 18:2579c275ef57 134 }
yuto17320508 16:b232fd9ee9c2 135
yuto17320508 18:2579c275ef57 136 if((ButtonState >> BUTTONDOWN)&1 == 1) {//対応するボタンを書く(今回下ボタン
yuto17320508 20:b84beed117ef 137 L_down = 2;
yuto17320508 18:2579c275ef57 138 } else {
yuto17320508 20:b84beed117ef 139 L_down = 0;
yuto17320508 18:2579c275ef57 140 }
yuto17320508 16:b232fd9ee9c2 141
yuto17320508 18:2579c275ef57 142 if((ButtonState >> BUTTONL1)&1 == 1) {//対応するボタンを書く(今回L1ボタン
yuto17320508 20:b84beed117ef 143 L_open = 4;
yuto17320508 18:2579c275ef57 144 } else {
yuto17320508 20:b84beed117ef 145 L_open = 0;
yuto17320508 18:2579c275ef57 146 }
yuto17320508 16:b232fd9ee9c2 147
yuto17320508 18:2579c275ef57 148 if((ButtonState >> BUTTONL2)&1 == 1) {//対応するボタンを書く(今回L2ボタン
yuto17320508 20:b84beed117ef 149 L_close = 8;
yuto17320508 18:2579c275ef57 150 } else {
yuto17320508 20:b84beed117ef 151 L_close = 0;
yuto17320508 18:2579c275ef57 152 }
yuto17320508 16:b232fd9ee9c2 153
yuto17320508 18:2579c275ef57 154 if((ButtonState >> BUTTONTRIANGEL)&1 == 1) {//対応するボタンを書く(今回△ボタン
yuto17320508 20:b84beed117ef 155 R_up = 16;
yuto17320508 18:2579c275ef57 156 } else {
yuto17320508 20:b84beed117ef 157 R_up = 0;
yuto17320508 18:2579c275ef57 158 }
yuto17320508 16:b232fd9ee9c2 159
yuto17320508 18:2579c275ef57 160 if((ButtonState >> BUTTONCROSS)&1 == 1) {//対応するボタンを書く(今回×ボタン
yuto17320508 20:b84beed117ef 161 R_down = 32;
yuto17320508 18:2579c275ef57 162 } else {
yuto17320508 20:b84beed117ef 163 R_down = 0;
yuto17320508 18:2579c275ef57 164 }
yuto17320508 16:b232fd9ee9c2 165
yuto17320508 18:2579c275ef57 166 if((ButtonState >> BUTTONR1)&1 == 1) {//対応するボタンを書く(今回R1ボタン
yuto17320508 20:b84beed117ef 167 R_open = 64;
yuto17320508 18:2579c275ef57 168 } else {
yuto17320508 20:b84beed117ef 169 R_open = 0;
yuto17320508 18:2579c275ef57 170 }
yuto17320508 16:b232fd9ee9c2 171
yuto17320508 18:2579c275ef57 172 if((ButtonState >> BUTTONR2)&1 == 1) {//対応するボタンを書く(今回R2ボタン
yuto17320508 20:b84beed117ef 173 R_close = 128;
yuto17320508 18:2579c275ef57 174 } else {
yuto17320508 20:b84beed117ef 175 R_close = 0;
yuto17320508 18:2579c275ef57 176 }
yuto17320508 16:b232fd9ee9c2 177
yuto17320508 16:b232fd9ee9c2 178
yuto17320508 16:b232fd9ee9c2 179
yuto17320508 20:b84beed117ef 180 send = L_up+L_down+L_open+L_close+R_up+R_down+R_open+R_close;
yuto17320508 18:2579c275ef57 181
yuto17320508 18:2579c275ef57 182 cs = 0;
yuto17320508 18:2579c275ef57 183 spi. write(send);
yuto17320508 18:2579c275ef57 184 cs = 1;
yuto17320508 19:a3f57c9833b6 185 // printf("%d\r\n",send);
yuto17320508 19:a3f57c9833b6 186
yuto17320508 18:2579c275ef57 187
yuto17320508 18:2579c275ef57 188
yuto17320508 19:a3f57c9833b6 189 //オムニホイールのプログラム
yuto17320508 20:b84beed117ef 190 if(LSX>=bound_m && LSX<=bound_p && LSY>=bound_m && LSY<=bound_p) {
yuto17320508 19:a3f57c9833b6 191 M1=0;
yuto17320508 19:a3f57c9833b6 192 M2=0;
yuto17320508 19:a3f57c9833b6 193 M3=0;
yuto17320508 20:b84beed117ef 194 if(RSX>=bound_p && RSX<=255) {
yuto17320508 19:a3f57c9833b6 195 M1=power_f*roll_spd;
yuto17320508 19:a3f57c9833b6 196 M2=power_l*roll_spd;
yuto17320508 19:a3f57c9833b6 197 M3=power_r*roll_spd;
yuto17320508 20:b84beed117ef 198 } else if(RSX>=0 && RSX<=bound_m) {
yuto17320508 19:a3f57c9833b6 199 M1=-1.0*power_f*roll_spd;
yuto17320508 19:a3f57c9833b6 200 M2=-1.0*power_l*roll_spd;
yuto17320508 19:a3f57c9833b6 201 M3=-1.0*power_r*roll_spd;
yuto17320508 19:a3f57c9833b6 202 }
yuto17320508 19:a3f57c9833b6 203
yuto17320508 18:2579c275ef57 204 motor_act();
yuto17320508 16:b232fd9ee9c2 205
yuto17320508 19:a3f57c9833b6 206 sita=0;
yuto17320508 19:a3f57c9833b6 207 } else {
yuto17320508 20:b84beed117ef 208 if(LSX>=center && LSX<=255 && LSY>=center && LSY<=255) { //第四象限
yuto17320508 20:b84beed117ef 209 sita = -1.0*(atan2((double)LSY-center,(double)LSX-center))*180/Pi;
yuto17320508 19:a3f57c9833b6 210 sita_2=90-sita;
yuto17320508 21:d9f5b74d5034 211 M1=sin((sita_2-(fai+0))*Pi/180)*power_f;
yuto17320508 21:d9f5b74d5034 212 M2=sin((sita_2-(fai+240))*Pi/180)*power_l;
yuto17320508 21:d9f5b74d5034 213 M3=sin((sita_2-(fai+120))*Pi/180)*power_r;
yuto17320508 19:a3f57c9833b6 214 motor_act();
yuto17320508 20:b84beed117ef 215 } else if(LSX>=0 && LSX<=center && LSY>=center && LSY<=255) { //第三象限
yuto17320508 20:b84beed117ef 216 sita = -1.0*(atan2((double)LSY-center,(double)LSX-center))*180/Pi;
yuto17320508 19:a3f57c9833b6 217 sita_2=90-sita;
yuto17320508 21:d9f5b74d5034 218 M1=sin((sita_2-(fai+0))*Pi/180)*power_f;
yuto17320508 21:d9f5b74d5034 219 M2=sin((sita_2-(fai+240))*Pi/180)*power_l;
yuto17320508 21:d9f5b74d5034 220 M3=sin((sita_2-(fai+120))*Pi/180)*power_r;
yuto17320508 19:a3f57c9833b6 221 motor_act();
yuto17320508 20:b84beed117ef 222 } else if(LSX>=0 && LSX<=center && LSY>=0 && LSY<=center) { //第二象限
yuto17320508 20:b84beed117ef 223 sita = -1.0*(atan2((double)LSY-center,(double)LSX-center))*180/Pi;
yuto17320508 19:a3f57c9833b6 224 sita_2=90-sita;
yuto17320508 21:d9f5b74d5034 225 M1=sin((sita_2-(fai+0))*Pi/180)*power_f;
yuto17320508 21:d9f5b74d5034 226 M2=sin((sita_2-(fai+240))*Pi/180)*power_l;
yuto17320508 21:d9f5b74d5034 227 M3=sin((sita_2-(fai+120))*Pi/180)*power_r;
yuto17320508 19:a3f57c9833b6 228 motor_act();
yuto17320508 20:b84beed117ef 229 } else if(LSX>=center && LSX<=255 && LSY>=0 && LSY<=center) { //第一象限
yuto17320508 20:b84beed117ef 230 sita = -1.0*(atan2((double)LSY-center,(double)LSX-center))*180/Pi;
yuto17320508 19:a3f57c9833b6 231 sita_2=90-sita;
yuto17320508 21:d9f5b74d5034 232 M1=sin((sita_2-(fai+0))*Pi/180)*power_f;
yuto17320508 21:d9f5b74d5034 233 M2=sin((sita_2-(fai+240))*Pi/180)*power_l;
yuto17320508 21:d9f5b74d5034 234 M3=sin((sita_2-(fai+120))*Pi/180)*power_r;
yuto17320508 19:a3f57c9833b6 235 motor_act();
yuto17320508 20:b84beed117ef 236 }
yuto17320508 19:a3f57c9833b6 237 //真っすぐだけのプログラム(いらない）
yuto17320508 20:b84beed117ef 238
yuto17320508 19:a3f57c9833b6 239 /*else if(LSX==255) {
yuto17320508 19:a3f57c9833b6 240 sita = 0;
yuto17320508 19:a3f57c9833b6 241 sita_2=90-sita;
yuto17320508 19:a3f57c9833b6 242 M1=sin((sita_2-fai)*Pi/180)*power_f;
yuto17320508 19:a3f57c9833b6 243 M2=sin((sita_2-fai+240)*Pi/180)*power_l;
yuto17320508 19:a3f57c9833b6 244 M3=sin((sita_2-fai+120)*Pi/180)*power_r;
yuto17320508 19:a3f57c9833b6 245 motor_act();
yuto17320508 18:2579c275ef57 246
yuto17320508 19:a3f57c9833b6 247 } else if(LSY==255) {
yuto17320508 19:a3f57c9833b6 248 sita = -90;
yuto17320508 19:a3f57c9833b6 249 sita_2=90-sita;
yuto17320508 19:a3f57c9833b6 250 M1=sin((sita_2-fai)*Pi/180)*power_f;
yuto17320508 19:a3f57c9833b6 251 M2=sin((sita_2-fai+240)*Pi/180)*power_l;
yuto17320508 19:a3f57c9833b6 252 M3=sin((sita_2-fai+120)*Pi/180)*power_r;
yuto17320508 19:a3f57c9833b6 253 motor_act();
yuto17320508 19:a3f57c9833b6 254
yuto17320508 19:a3f57c9833b6 255 } else if(LSX==0) {
yuto17320508 19:a3f57c9833b6 256 sita = 180;
yuto17320508 19:a3f57c9833b6 257 sita_2=90-sita;
yuto17320508 19:a3f57c9833b6 258 M1=sin((sita_2-fai)*Pi/180)*power_f;
yuto17320508 19:a3f57c9833b6 259 M2=sin((sita_2-fai+240)*Pi/180)*power_l;
yuto17320508 19:a3f57c9833b6 260 M3=sin((sita_2-fai+120)*Pi/180)*power_r;
yuto17320508 19:a3f57c9833b6 261 motor_act();
yuto17320508 19:a3f57c9833b6 262
yuto17320508 19:a3f57c9833b6 263 } else if(LSY==0) {
yuto17320508 19:a3f57c9833b6 264 sita = 90;
yuto17320508 19:a3f57c9833b6 265 sita_2=90-sita;
yuto17320508 19:a3f57c9833b6 266 M1=sin((sita_2-fai)*Pi/180)*power_f;
yuto17320508 19:a3f57c9833b6 267 M2=sin((sita_2-fai+240)*Pi/180)*power_l;
yuto17320508 19:a3f57c9833b6 268 M3=sin((sita_2-fai+120)*Pi/180)*power_r;
yuto17320508 19:a3f57c9833b6 269 motor_act();
yuto17320508 19:a3f57c9833b6 270 } */
yuto17320508 16:b232fd9ee9c2 271
yuto17320508 16:b232fd9ee9c2 272 }
yuto17320508 18:2579c275ef57 273
yuto17320508 18:2579c275ef57 274
yuto17320508 19:a3f57c9833b6 275
yuto17320508 19:a3f57c9833b6 276
yuto17320508 20:b84beed117ef 277 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 278
yuto17320508 18:2579c275ef57 279
yuto17320508 18:2579c275ef57 280
hirokimineshita 0:736c76a75def 281 }