F3RC1班 / Mbed 2 deprecated F3RC_syudou_BUS_master

通信変えたやつです

Dependencies:   mbed

Fork of F3RC_syudou_master_3 by F3RC1班

User.cpp@20:b84beed117ef, 2017-08-23 (annotated)

Committer:
yuto17320508
Date:
Wed Aug 23 13:20:31 2017 +0000
Revision:
20:b84beed117ef
Parent:
19:a3f57c9833b6
Child:
21:d9f5b74d5034
aa

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