F3RC1班 / Mbed 2 deprecated F3RC_syudou_BUS_master

通信変えたやつです

Dependencies:   mbed

Fork of F3RC_syudou_master_3 by F3RC1班

User.cpp@25:d5588a50f069, 2017-08-25 (annotated)

Committer:
yuto17320508
Date:
Fri Aug 25 12:23:18 2017 +0000
Revision:
25:d5588a50f069
Parent:
24:3610bcb8e275
Child:
26:91dd637de4d4
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 25:d5588a50f069 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 25:d5588a50f069 123 int flag=0;
yuto17320508 25:d5588a50f069 124 num=0;
yuto17320508 16:b232fd9ee9c2 125
yuto17320508 18:2579c275ef57 126
yuto17320508 18:2579c275ef57 127 if((ButtonState >> BUTTONUP)&1 == 1) {//対応するボタンを書く(今回上ボタン
yuto17320508 24:3610bcb8e275 128 num = 1;
yuto17320508 25:d5588a50f069 129 flag=flag+1;
yuto17320508 25:d5588a50f069 130 }
yuto17320508 25:d5588a50f069 131 if((ButtonState >> BUTTONDOWN)&1 == 1) { //対応するボタンを書く(今回下ボタン
yuto17320508 24:3610bcb8e275 132 num = 2;
yuto17320508 25:d5588a50f069 133 flag=flag+1;
yuto17320508 25:d5588a50f069 134 }
yuto17320508 25:d5588a50f069 135 if((ButtonState >> BUTTONL1)&1 == 1) { //対応するボタンを書く(今回L1ボタン
yuto17320508 24:3610bcb8e275 136 num = 3;
yuto17320508 25:d5588a50f069 137 flag=flag+1;
yuto17320508 25:d5588a50f069 138 }
yuto17320508 25:d5588a50f069 139 if((ButtonState >> BUTTONL2)&1 == 1) { //対応するボタンを書く(今回L2ボタン
yuto17320508 24:3610bcb8e275 140 num = 4;
yuto17320508 25:d5588a50f069 141 flag=flag+1;
yuto17320508 25:d5588a50f069 142 }
yuto17320508 25:d5588a50f069 143 if((ButtonState >> BUTTONTRIANGEL)&1 == 1) { //対応するボタンを書く(今回△ボタン
yuto17320508 24:3610bcb8e275 144 num = 5;
yuto17320508 25:d5588a50f069 145 flag=flag+1;
yuto17320508 25:d5588a50f069 146 }
yuto17320508 25:d5588a50f069 147 if((ButtonState >> BUTTONCROSS)&1 == 1) { //対応するボタンを書く(今回×ボタン
yuto17320508 24:3610bcb8e275 148 num = 6;
yuto17320508 25:d5588a50f069 149 flag=flag+1;
yuto17320508 25:d5588a50f069 150 }
yuto17320508 25:d5588a50f069 151 if((ButtonState >> BUTTONR1)&1 == 1) { //対応するボタンを書く(今回R1ボタン
yuto17320508 24:3610bcb8e275 152 num = 7;
yuto17320508 25:d5588a50f069 153 flag=flag+1;
yuto17320508 25:d5588a50f069 154 }
yuto17320508 25:d5588a50f069 155 if((ButtonState >> BUTTONR2)&1 == 1) { //対応するボタンを書く(今回R2ボタン
yuto17320508 24:3610bcb8e275 156 num = 8;
yuto17320508 25:d5588a50f069 157 flag=flag+1;
yuto17320508 25:d5588a50f069 158 }
yuto17320508 25:d5588a50f069 159 if(flag >=2) {
yuto17320508 24:3610bcb8e275 160 num=0;
yuto17320508 25:d5588a50f069 161 }
yuto17320508 24:3610bcb8e275 162 out=num;
yuto17320508 24:3610bcb8e275 163 printf("%d\r\n",num);
yuto17320508 22:e88dd3acec2b 164 //オムニホイールのプログラム
yuto17320508 20:b84beed117ef 165 if(LSX>=bound_m && LSX<=bound_p && LSY>=bound_m && LSY<=bound_p) {
yuto17320508 19:a3f57c9833b6 166 M1=0;
yuto17320508 19:a3f57c9833b6 167 M2=0;
yuto17320508 19:a3f57c9833b6 168 M3=0;
yuto17320508 20:b84beed117ef 169 if(RSX>=bound_p && RSX<=255) {
yuto17320508 19:a3f57c9833b6 170 M1=power_f*roll_spd;
yuto17320508 19:a3f57c9833b6 171 M2=power_l*roll_spd;
yuto17320508 19:a3f57c9833b6 172 M3=power_r*roll_spd;
yuto17320508 20:b84beed117ef 173 } else if(RSX>=0 && RSX<=bound_m) {
yuto17320508 19:a3f57c9833b6 174 M1=-1.0*power_f*roll_spd;
yuto17320508 19:a3f57c9833b6 175 M2=-1.0*power_l*roll_spd;
yuto17320508 19:a3f57c9833b6 176 M3=-1.0*power_r*roll_spd;
yuto17320508 19:a3f57c9833b6 177 }
yuto17320508 19:a3f57c9833b6 178
yuto17320508 18:2579c275ef57 179 motor_act();
yuto17320508 16:b232fd9ee9c2 180
yuto17320508 19:a3f57c9833b6 181 sita=0;
yuto17320508 19:a3f57c9833b6 182 } else {
yuto17320508 24:3610bcb8e275 183 sita = -1.0*(atan2((double)LSY-center,(double)LSX-center))*180/Pi;
yuto17320508 24:3610bcb8e275 184 sita_2=90-sita;
yuto17320508 24:3610bcb8e275 185 M1=sin((sita_2-(fai+0))*Pi/180)*power_f;
yuto17320508 24:3610bcb8e275 186 M2=sin((sita_2-(fai+240))*Pi/180)*power_l;
yuto17320508 24:3610bcb8e275 187 M3=sin((sita_2-(fai+120))*Pi/180)*power_r;
yuto17320508 24:3610bcb8e275 188 motor_act();
yuto17320508 24:3610bcb8e275 189 }
yuto17320508 19:a3f57c9833b6 190 //真っすぐだけのプログラム(いらない）
yuto17320508 20:b84beed117ef 191
yuto17320508 24:3610bcb8e275 192 /*else if(LSX==255) {
yuto17320508 24:3610bcb8e275 193 sita = 0;
yuto17320508 24:3610bcb8e275 194 sita_2=90-sita;
yuto17320508 24:3610bcb8e275 195 M1=sin((sita_2-fai)*Pi/180)*power_f;
yuto17320508 24:3610bcb8e275 196 M2=sin((sita_2-fai+240)*Pi/180)*power_l;
yuto17320508 24:3610bcb8e275 197 M3=sin((sita_2-fai+120)*Pi/180)*power_r;
yuto17320508 24:3610bcb8e275 198 motor_act();
yuto17320508 18:2579c275ef57 199
yuto17320508 24:3610bcb8e275 200 } else if(LSY==255) {
yuto17320508 24:3610bcb8e275 201 sita = -90;
yuto17320508 24:3610bcb8e275 202 sita_2=90-sita;
yuto17320508 24:3610bcb8e275 203 M1=sin((sita_2-fai)*Pi/180)*power_f;
yuto17320508 24:3610bcb8e275 204 M2=sin((sita_2-fai+240)*Pi/180)*power_l;
yuto17320508 24:3610bcb8e275 205 M3=sin((sita_2-fai+120)*Pi/180)*power_r;
yuto17320508 24:3610bcb8e275 206 motor_act();
yuto17320508 19:a3f57c9833b6 207
yuto17320508 24:3610bcb8e275 208 } else if(LSX==0) {
yuto17320508 24:3610bcb8e275 209 sita = 180;
yuto17320508 24:3610bcb8e275 210 sita_2=90-sita;
yuto17320508 24:3610bcb8e275 211 M1=sin((sita_2-fai)*Pi/180)*power_f;
yuto17320508 24:3610bcb8e275 212 M2=sin((sita_2-fai+240)*Pi/180)*power_l;
yuto17320508 24:3610bcb8e275 213 M3=sin((sita_2-fai+120)*Pi/180)*power_r;
yuto17320508 24:3610bcb8e275 214 motor_act();
yuto17320508 19:a3f57c9833b6 215
yuto17320508 24:3610bcb8e275 216 } else if(LSY==0) {
yuto17320508 24:3610bcb8e275 217 sita = 90;
yuto17320508 24:3610bcb8e275 218 sita_2=90-sita;
yuto17320508 24:3610bcb8e275 219 M1=sin((sita_2-fai)*Pi/180)*power_f;
yuto17320508 24:3610bcb8e275 220 M2=sin((sita_2-fai+240)*Pi/180)*power_l;
yuto17320508 24:3610bcb8e275 221 M3=sin((sita_2-fai+120)*Pi/180)*power_r;
yuto17320508 24:3610bcb8e275 222 motor_act();
yuto17320508 24:3610bcb8e275 223 } */
yuto17320508 16:b232fd9ee9c2 224
yuto17320508 24:3610bcb8e275 225
yuto17320508 18:2579c275ef57 226
yuto17320508 18:2579c275ef57 227
yuto17320508 19:a3f57c9833b6 228
yuto17320508 19:a3f57c9833b6 229
yuto17320508 20:b84beed117ef 230 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 231
yuto17320508 18:2579c275ef57 232
yuto17320508 18:2579c275ef57 233
hirokimineshita 0:736c76a75def 234 }