通信変えたやつです
Dependencies: mbed
Fork of F3RC_syudou_master_3 by
User.cpp@26:91dd637de4d4, 2017-09-14 (annotated)
- Committer:
- yuto17320508
- Date:
- Thu Sep 14 01:20:12 2017 +0000
- Revision:
- 26:91dd637de4d4
- Parent:
- 25:d5588a50f069
a
Who changed what in which revision?
User | Revision | Line number | New 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 | l↙ ↖f |
yuto17320508 | 21:d9f5b74d5034 | 22 | |
yuto17320508 | 21:d9f5b74d5034 | 23 | → |
yuto17320508 | 21:d9f5b74d5034 | 24 | r */ |
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 | 26:91dd637de4d4 | 34 | double power_f=0.32; |
yuto17320508 | 26:91dd637de4d4 | 35 | double power_l=0.32; |
yuto17320508 | 26:91dd637de4d4 | 36 | double power_r=0.32; |
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 | 26:91dd637de4d4 | 42 | double accel=1.3; |
yuto17320508 | 20:b84beed117ef | 43 | //ジョイスティックの中心座標 |
yuto17320508 | 20:b84beed117ef | 44 | double center=127; |
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 | 26:91dd637de4d4 | 51 | //回転の比 |
yuto17320508 | 26:91dd637de4d4 | 52 | //移動中の回転 |
yuto17320508 | 26:91dd637de4d4 | 53 | double roll_spd=0.4; |
yuto17320508 | 26:91dd637de4d4 | 54 | //横移動中の回転 |
yuto17320508 | 26:91dd637de4d4 | 55 | double roll_spd2=0.1; |
yuto17320508 | 26:91dd637de4d4 | 56 | //横移動のスピード |
yuto17320508 | 26:91dd637de4d4 | 57 | double yoko_spd=0.9; |
yuto17320508 | 20:b84beed117ef | 58 | |
yuto17320508 | 22:e88dd3acec2b | 59 | //モーターの動作 |
yuto17320508 | 18:2579c275ef57 | 60 | void motor_act() |
yuto17320508 | 18:2579c275ef57 | 61 | { |
yuto17320508 | 26:91dd637de4d4 | 62 | //絶対値をつける関数 |
yuto17320508 | 18:2579c275ef57 | 63 | if(M1 >=0) { |
yuto17320508 | 18:2579c275ef57 | 64 | motor_f_1=M1; |
yuto17320508 | 18:2579c275ef57 | 65 | motor_f_2=0; |
yuto17320508 | 18:2579c275ef57 | 66 | } else { |
yuto17320508 | 18:2579c275ef57 | 67 | motor_f_1=0; |
yuto17320508 | 18:2579c275ef57 | 68 | motor_f_2=-M1; |
yuto17320508 | 18:2579c275ef57 | 69 | } |
yuto17320508 | 18:2579c275ef57 | 70 | if(M2 >=0) { |
yuto17320508 | 18:2579c275ef57 | 71 | motor_l_1=M2; |
yuto17320508 | 18:2579c275ef57 | 72 | motor_l_2=0; |
yuto17320508 | 18:2579c275ef57 | 73 | } else { |
yuto17320508 | 18:2579c275ef57 | 74 | motor_l_1=0; |
yuto17320508 | 18:2579c275ef57 | 75 | motor_l_2=-M2; |
yuto17320508 | 18:2579c275ef57 | 76 | } |
yuto17320508 | 18:2579c275ef57 | 77 | if(M3 >=0) { |
yuto17320508 | 18:2579c275ef57 | 78 | motor_r_1=M3; |
yuto17320508 | 18:2579c275ef57 | 79 | motor_r_2=0; |
yuto17320508 | 18:2579c275ef57 | 80 | } else { |
yuto17320508 | 18:2579c275ef57 | 81 | motor_r_1=0; |
yuto17320508 | 18:2579c275ef57 | 82 | motor_r_2=-M3; |
yuto17320508 | 18:2579c275ef57 | 83 | } |
yuto17320508 | 18:2579c275ef57 | 84 | } |
yuto17320508 | 22:e88dd3acec2b | 85 | //ジョイスティック入力値の偏角 |
yuto17320508 | 18:2579c275ef57 | 86 | double sita; |
yuto17320508 | 22:e88dd3acec2b | 87 | //関数代入用の角度調整 |
yuto17320508 | 18:2579c275ef57 | 88 | double sita_2; |
yuto17320508 | 18:2579c275ef57 | 89 | |
yuto17320508 | 16:b232fd9ee9c2 | 90 | void UserLoopSetting() |
yuto17320508 | 16:b232fd9ee9c2 | 91 | { |
yuto17320508 | 23:33a40d8c0535 | 92 | motor_f_1.period_us(100); |
yuto17320508 | 23:33a40d8c0535 | 93 | motor_f_2.period_us(100); |
yuto17320508 | 23:33a40d8c0535 | 94 | motor_l_1.period_us(100); |
yuto17320508 | 23:33a40d8c0535 | 95 | motor_l_2.period_us(100); |
yuto17320508 | 23:33a40d8c0535 | 96 | motor_r_1.period_us(100); |
yuto17320508 | 23:33a40d8c0535 | 97 | motor_r_2.period_us(100); |
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 | 26:91dd637de4d4 | 123 | //BUS用プログラム |
yuto17320508 | 25:d5588a50f069 | 124 | int flag=0; |
yuto17320508 | 25:d5588a50f069 | 125 | num=0; |
yuto17320508 | 26:91dd637de4d4 | 126 | if((ButtonState >> BUTTONTRIANGEL)&1 == 1) {//対応するボタンを書く |
yuto17320508 | 24:3610bcb8e275 | 127 | num = 1; |
yuto17320508 | 25:d5588a50f069 | 128 | flag=flag+1; |
yuto17320508 | 25:d5588a50f069 | 129 | } |
yuto17320508 | 26:91dd637de4d4 | 130 | if((ButtonState >> BUTTONCROSS)&1 == 1) { //対応するボタンを書く |
yuto17320508 | 24:3610bcb8e275 | 131 | num = 2; |
yuto17320508 | 25:d5588a50f069 | 132 | flag=flag+1; |
yuto17320508 | 25:d5588a50f069 | 133 | } |
yuto17320508 | 26:91dd637de4d4 | 134 | if((ButtonState >> BUTTONR1)&1 == 1) { //対応するボタンを書く |
yuto17320508 | 24:3610bcb8e275 | 135 | num = 3; |
yuto17320508 | 25:d5588a50f069 | 136 | flag=flag+1; |
yuto17320508 | 25:d5588a50f069 | 137 | } |
yuto17320508 | 26:91dd637de4d4 | 138 | if((ButtonState >> BUTTONR2)&1 == 1) { //対応するボタンを書く |
yuto17320508 | 24:3610bcb8e275 | 139 | num = 4; |
yuto17320508 | 25:d5588a50f069 | 140 | flag=flag+1; |
yuto17320508 | 25:d5588a50f069 | 141 | } |
yuto17320508 | 26:91dd637de4d4 | 142 | if((ButtonState >> BUTTONUP)&1 == 1) { //対応するボタンを書く |
yuto17320508 | 24:3610bcb8e275 | 143 | num = 5; |
yuto17320508 | 25:d5588a50f069 | 144 | flag=flag+1; |
yuto17320508 | 25:d5588a50f069 | 145 | } |
yuto17320508 | 26:91dd637de4d4 | 146 | if((ButtonState >> BUTTONDOWN)&1 == 1) { //対応するボタンを書く |
yuto17320508 | 24:3610bcb8e275 | 147 | num = 6; |
yuto17320508 | 25:d5588a50f069 | 148 | flag=flag+1; |
yuto17320508 | 25:d5588a50f069 | 149 | } |
yuto17320508 | 26:91dd637de4d4 | 150 | if((ButtonState >> BUTTONL1)&1 == 1) { //対応するボタンを書く |
yuto17320508 | 24:3610bcb8e275 | 151 | num = 7; |
yuto17320508 | 25:d5588a50f069 | 152 | flag=flag+1; |
yuto17320508 | 25:d5588a50f069 | 153 | } |
yuto17320508 | 26:91dd637de4d4 | 154 | if((ButtonState >> BUTTONL2)&1 == 1) { //対応するボタンを書く |
yuto17320508 | 24:3610bcb8e275 | 155 | num = 8; |
yuto17320508 | 25:d5588a50f069 | 156 | flag=flag+1; |
yuto17320508 | 25:d5588a50f069 | 157 | } |
yuto17320508 | 25:d5588a50f069 | 158 | if(flag >=2) { |
yuto17320508 | 24:3610bcb8e275 | 159 | num=0; |
yuto17320508 | 25:d5588a50f069 | 160 | } |
yuto17320508 | 24:3610bcb8e275 | 161 | out=num; |
yuto17320508 | 24:3610bcb8e275 | 162 | printf("%d\r\n",num); |
yuto17320508 | 22:e88dd3acec2b | 163 | //オムニホイールのプログラム |
yuto17320508 | 26:91dd637de4d4 | 164 | |
yuto17320508 | 26:91dd637de4d4 | 165 | if((ButtonState >> BUTTONRANALOG)&1 == 1) {//押し込みで速度変化 |
yuto17320508 | 26:91dd637de4d4 | 166 | roll_spd=0.8; |
yuto17320508 | 26:91dd637de4d4 | 167 | } else { |
yuto17320508 | 26:91dd637de4d4 | 168 | roll_spd=0.4; |
yuto17320508 | 26:91dd637de4d4 | 169 | } |
yuto17320508 | 20:b84beed117ef | 170 | if(LSX>=bound_m && LSX<=bound_p && LSY>=bound_m && LSY<=bound_p) { |
yuto17320508 | 19:a3f57c9833b6 | 171 | M1=0; |
yuto17320508 | 19:a3f57c9833b6 | 172 | M2=0; |
yuto17320508 | 19:a3f57c9833b6 | 173 | M3=0; |
yuto17320508 | 26:91dd637de4d4 | 174 | if(RSX>=bound_p && RSX<=255) {//右スティック右 |
yuto17320508 | 19:a3f57c9833b6 | 175 | M1=power_f*roll_spd; |
yuto17320508 | 19:a3f57c9833b6 | 176 | M2=power_l*roll_spd; |
yuto17320508 | 19:a3f57c9833b6 | 177 | M3=power_r*roll_spd; |
yuto17320508 | 26:91dd637de4d4 | 178 | } else if(RSX>=0 && RSX<=bound_m) {//右スティック左 |
yuto17320508 | 19:a3f57c9833b6 | 179 | M1=-1.0*power_f*roll_spd; |
yuto17320508 | 19:a3f57c9833b6 | 180 | M2=-1.0*power_l*roll_spd; |
yuto17320508 | 19:a3f57c9833b6 | 181 | M3=-1.0*power_r*roll_spd; |
yuto17320508 | 19:a3f57c9833b6 | 182 | } |
yuto17320508 | 26:91dd637de4d4 | 183 | sita=0; |
yuto17320508 | 16:b232fd9ee9c2 | 184 | |
yuto17320508 | 26:91dd637de4d4 | 185 | } else {//左スティック全方向 |
yuto17320508 | 24:3610bcb8e275 | 186 | sita = -1.0*(atan2((double)LSY-center,(double)LSX-center))*180/Pi; |
yuto17320508 | 24:3610bcb8e275 | 187 | sita_2=90-sita; |
yuto17320508 | 24:3610bcb8e275 | 188 | M1=sin((sita_2-(fai+0))*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 | 26:91dd637de4d4 | 191 | if(RSX>=bound_p && RSX<=255) {//進行中の右スティック右 |
yuto17320508 | 26:91dd637de4d4 | 192 | M1=M1+power_f*roll_spd; |
yuto17320508 | 26:91dd637de4d4 | 193 | M2=M2+power_l*roll_spd; |
yuto17320508 | 26:91dd637de4d4 | 194 | M3=M3+power_r*roll_spd; |
yuto17320508 | 26:91dd637de4d4 | 195 | } else if(RSX>=0 && RSX<=bound_m) {//進行中の右スティック左 |
yuto17320508 | 26:91dd637de4d4 | 196 | M1=M1+-1.0*power_f*roll_spd; |
yuto17320508 | 26:91dd637de4d4 | 197 | M2=M2+-1.0*power_l*roll_spd; |
yuto17320508 | 26:91dd637de4d4 | 198 | M3=M3+-1.0*power_r*roll_spd; |
yuto17320508 | 26:91dd637de4d4 | 199 | } |
yuto17320508 | 24:3610bcb8e275 | 200 | } |
yuto17320508 | 26:91dd637de4d4 | 201 | //真横移動 |
yuto17320508 | 26:91dd637de4d4 | 202 | if((ButtonState >> BUTTONLEFT)&1 == 1) { //十字キー左 |
yuto17320508 | 26:91dd637de4d4 | 203 | sita = 180; |
yuto17320508 | 26:91dd637de4d4 | 204 | sita_2=90-sita; |
yuto17320508 | 26:91dd637de4d4 | 205 | M1=sin((sita_2-(fai+0))*Pi/180)*power_f*yoko_spd; |
yuto17320508 | 26:91dd637de4d4 | 206 | M2=sin((sita_2-(fai+240))*Pi/180)*power_l*yoko_spd; |
yuto17320508 | 26:91dd637de4d4 | 207 | M3=sin((sita_2-(fai+120))*Pi/180)*power_r*yoko_spd; |
yuto17320508 | 26:91dd637de4d4 | 208 | if(RSX>=bound_p && RSX<=255) {//横移動中の右スティック右 |
yuto17320508 | 26:91dd637de4d4 | 209 | M1=M1+power_f*roll_spd2; |
yuto17320508 | 26:91dd637de4d4 | 210 | M2=M2+power_l*roll_spd2; |
yuto17320508 | 26:91dd637de4d4 | 211 | M3=M3+power_r*roll_spd2; |
yuto17320508 | 26:91dd637de4d4 | 212 | } else if(RSX>=0 && RSX<=bound_m) {//横移動中の右スティック左 |
yuto17320508 | 26:91dd637de4d4 | 213 | M1=M1+-1.0*power_f*roll_spd2; |
yuto17320508 | 26:91dd637de4d4 | 214 | M2=M2+-1.0*power_l*roll_spd2; |
yuto17320508 | 26:91dd637de4d4 | 215 | M3=M3+-1.0*power_r*roll_spd2; |
yuto17320508 | 26:91dd637de4d4 | 216 | } |
yuto17320508 | 26:91dd637de4d4 | 217 | } |
yuto17320508 | 26:91dd637de4d4 | 218 | if((ButtonState >> BUTTONRIGHT)&1 == 1) { //十字キー右 |
yuto17320508 | 26:91dd637de4d4 | 219 | sita = 0; |
yuto17320508 | 26:91dd637de4d4 | 220 | sita_2=90-sita; |
yuto17320508 | 26:91dd637de4d4 | 221 | M1=sin((sita_2-(fai+0))*Pi/180)*power_f*yoko_spd; |
yuto17320508 | 26:91dd637de4d4 | 222 | M2=sin((sita_2-(fai+240))*Pi/180)*power_l*yoko_spd; |
yuto17320508 | 26:91dd637de4d4 | 223 | M3=sin((sita_2-(fai+120))*Pi/180)*power_r*yoko_spd; |
yuto17320508 | 26:91dd637de4d4 | 224 | if(RSX>=bound_p && RSX<=255) {//横移動中の右スティック右 |
yuto17320508 | 26:91dd637de4d4 | 225 | M1=M1+power_f*roll_spd2; |
yuto17320508 | 26:91dd637de4d4 | 226 | M2=M2+power_l*roll_spd2; |
yuto17320508 | 26:91dd637de4d4 | 227 | M3=M3+power_r*roll_spd2; |
yuto17320508 | 26:91dd637de4d4 | 228 | } else if(RSX>=0 && RSX<=bound_m) {//横移動中の右スティック左 |
yuto17320508 | 26:91dd637de4d4 | 229 | M1=M1+-1.0*power_f*roll_spd2; |
yuto17320508 | 26:91dd637de4d4 | 230 | M2=M2+-1.0*power_l*roll_spd2; |
yuto17320508 | 26:91dd637de4d4 | 231 | M3=M3+-1.0*power_r*roll_spd2; |
yuto17320508 | 26:91dd637de4d4 | 232 | } |
yuto17320508 | 26:91dd637de4d4 | 233 | } |
yuto17320508 | 26:91dd637de4d4 | 234 | motor_act(); |
yuto17320508 | 16:b232fd9ee9c2 | 235 | |
yuto17320508 | 24:3610bcb8e275 | 236 | |
yuto17320508 | 26:91dd637de4d4 | 237 | printf("motor_f:%.4f\t\motor_l:%.4f\t\motor_r:%.4f\t\sita:%f\r\n",M1,M2,M3,sita); |
yuto17320508 | 19:a3f57c9833b6 | 238 | |
yuto17320508 | 26:91dd637de4d4 | 239 | } |