日記 / Mbed 2 deprecated F3RC_syudou_master_3

主導機　mbed用のプログラムです　改良しました

Dependencies:   mbed

Fork of F3RC_syudou_master by 日記

User.cpp@22:e88dd3acec2b, 2017-08-23 (annotated)

Committer:
yuto17320508
Date:
Wed Aug 23 16:41:56 2017 +0000
Revision:
22:e88dd3acec2b
Parent:
21:d9f5b74d5034 
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 22:e88dd3acec2b 53 //モーターの動作
yuto17320508 18:2579c275ef57 54 void motor_act()
yuto17320508 18:2579c275ef57 55 {
yuto17320508 18:2579c275ef57 56 if(M1 >=0) {
yuto17320508 18:2579c275ef57 57 motor_f_1=M1;
yuto17320508 18:2579c275ef57 58 motor_f_2=0;
yuto17320508 18:2579c275ef57 59 } else {
yuto17320508 18:2579c275ef57 60 motor_f_1=0;
yuto17320508 18:2579c275ef57 61 motor_f_2=-M1;
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 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 22:e88dd3acec2b 79 //ジョイスティック入力値の偏角
yuto17320508 18:2579c275ef57 80 double sita;
yuto17320508 22:e88dd3acec2b 81 //関数代入用の角度調整
yuto17320508 18:2579c275ef57 82 double sita_2;
yuto17320508 18:2579c275ef57 83
yuto17320508 16:b232fd9ee9c2 84
yuto17320508 16:b232fd9ee9c2 85 void UserLoopSetting()
yuto17320508 16:b232fd9ee9c2 86 {
yuto17320508 17:c5c41fcf316e 87 spi.format(8,3);
yuto17320508 16:b232fd9ee9c2 88 spi.frequency(1000000);
yuto17320508 18:2579c275ef57 89 motor_f_1.period_us(50);
yuto17320508 18:2579c275ef57 90 motor_f_2.period_us(50);
yuto17320508 18:2579c275ef57 91 motor_l_1.period_us(50);
yuto17320508 18:2579c275ef57 92 motor_l_2.period_us(50);
yuto17320508 18:2579c275ef57 93 motor_r_1.period_us(50);
yuto17320508 18:2579c275ef57 94 motor_r_2.period_us(50);
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 20:b84beed117ef 129 L_up = 1;
yuto17320508 18:2579c275ef57 130 } else {
yuto17320508 20:b84beed117ef 131 L_up = 0;
yuto17320508 18:2579c275ef57 132 }
yuto17320508 16:b232fd9ee9c2 133
yuto17320508 18:2579c275ef57 134 if((ButtonState >> BUTTONDOWN)&1 == 1) {//対応するボタンを書く(今回下ボタン
yuto17320508 20:b84beed117ef 135 L_down = 2;
yuto17320508 18:2579c275ef57 136 } else {
yuto17320508 20:b84beed117ef 137 L_down = 0;
yuto17320508 18:2579c275ef57 138 }
yuto17320508 16:b232fd9ee9c2 139
yuto17320508 18:2579c275ef57 140 if((ButtonState >> BUTTONL1)&1 == 1) {//対応するボタンを書く(今回L1ボタン
yuto17320508 20:b84beed117ef 141 L_open = 4;
yuto17320508 18:2579c275ef57 142 } else {
yuto17320508 20:b84beed117ef 143 L_open = 0;
yuto17320508 18:2579c275ef57 144 }
yuto17320508 16:b232fd9ee9c2 145
yuto17320508 18:2579c275ef57 146 if((ButtonState >> BUTTONL2)&1 == 1) {//対応するボタンを書く(今回L2ボタン
yuto17320508 20:b84beed117ef 147 L_close = 8;
yuto17320508 18:2579c275ef57 148 } else {
yuto17320508 20:b84beed117ef 149 L_close = 0;
yuto17320508 18:2579c275ef57 150 }
yuto17320508 16:b232fd9ee9c2 151
yuto17320508 18:2579c275ef57 152 if((ButtonState >> BUTTONTRIANGEL)&1 == 1) {//対応するボタンを書く(今回△ボタン
yuto17320508 20:b84beed117ef 153 R_up = 16;
yuto17320508 18:2579c275ef57 154 } else {
yuto17320508 20:b84beed117ef 155 R_up = 0;
yuto17320508 18:2579c275ef57 156 }
yuto17320508 16:b232fd9ee9c2 157
yuto17320508 18:2579c275ef57 158 if((ButtonState >> BUTTONCROSS)&1 == 1) {//対応するボタンを書く(今回×ボタン
yuto17320508 20:b84beed117ef 159 R_down = 32;
yuto17320508 18:2579c275ef57 160 } else {
yuto17320508 20:b84beed117ef 161 R_down = 0;
yuto17320508 18:2579c275ef57 162 }
yuto17320508 16:b232fd9ee9c2 163
yuto17320508 18:2579c275ef57 164 if((ButtonState >> BUTTONR1)&1 == 1) {//対応するボタンを書く(今回R1ボタン
yuto17320508 20:b84beed117ef 165 R_open = 64;
yuto17320508 18:2579c275ef57 166 } else {
yuto17320508 20:b84beed117ef 167 R_open = 0;
yuto17320508 18:2579c275ef57 168 }
yuto17320508 16:b232fd9ee9c2 169
yuto17320508 18:2579c275ef57 170 if((ButtonState >> BUTTONR2)&1 == 1) {//対応するボタンを書く(今回R2ボタン
yuto17320508 20:b84beed117ef 171 R_close = 128;
yuto17320508 18:2579c275ef57 172 } else {
yuto17320508 20:b84beed117ef 173 R_close = 0;
yuto17320508 18:2579c275ef57 174 }
yuto17320508 16:b232fd9ee9c2 175
yuto17320508 16:b232fd9ee9c2 176
yuto17320508 16:b232fd9ee9c2 177
yuto17320508 20:b84beed117ef 178 send = L_up+L_down+L_open+L_close+R_up+R_down+R_open+R_close;
yuto17320508 18:2579c275ef57 179
yuto17320508 18:2579c275ef57 180 cs = 0;
yuto17320508 18:2579c275ef57 181 spi. write(send);
yuto17320508 18:2579c275ef57 182 cs = 1;
yuto17320508 22:e88dd3acec2b 183 // printf("%d\r\n",send
yuto17320508 22:e88dd3acec2b 184 //オムニホイールのプログラム
yuto17320508 20:b84beed117ef 185 if(LSX>=bound_m && LSX<=bound_p && LSY>=bound_m && LSY<=bound_p) {
yuto17320508 19:a3f57c9833b6 186 M1=0;
yuto17320508 19:a3f57c9833b6 187 M2=0;
yuto17320508 19:a3f57c9833b6 188 M3=0;
yuto17320508 20:b84beed117ef 189 if(RSX>=bound_p && RSX<=255) {
yuto17320508 19:a3f57c9833b6 190 M1=power_f*roll_spd;
yuto17320508 19:a3f57c9833b6 191 M2=power_l*roll_spd;
yuto17320508 19:a3f57c9833b6 192 M3=power_r*roll_spd;
yuto17320508 20:b84beed117ef 193 } else if(RSX>=0 && RSX<=bound_m) {
yuto17320508 19:a3f57c9833b6 194 M1=-1.0*power_f*roll_spd;
yuto17320508 19:a3f57c9833b6 195 M2=-1.0*power_l*roll_spd;
yuto17320508 19:a3f57c9833b6 196 M3=-1.0*power_r*roll_spd;
yuto17320508 19:a3f57c9833b6 197 }
yuto17320508 19:a3f57c9833b6 198
yuto17320508 18:2579c275ef57 199 motor_act();
yuto17320508 16:b232fd9ee9c2 200
yuto17320508 19:a3f57c9833b6 201 sita=0;
yuto17320508 19:a3f57c9833b6 202 } else {
yuto17320508 20:b84beed117ef 203 if(LSX>=center && LSX<=255 && LSY>=center && LSY<=255) { //第四象限
yuto17320508 20:b84beed117ef 204 sita = -1.0*(atan2((double)LSY-center,(double)LSX-center))*180/Pi;
yuto17320508 19:a3f57c9833b6 205 sita_2=90-sita;
yuto17320508 21:d9f5b74d5034 206 M1=sin((sita_2-(fai+0))*Pi/180)*power_f;
yuto17320508 21:d9f5b74d5034 207 M2=sin((sita_2-(fai+240))*Pi/180)*power_l;
yuto17320508 21:d9f5b74d5034 208 M3=sin((sita_2-(fai+120))*Pi/180)*power_r;
yuto17320508 19:a3f57c9833b6 209 motor_act();
yuto17320508 20:b84beed117ef 210 } else if(LSX>=0 && LSX<=center && LSY>=center && LSY<=255) { //第三象限
yuto17320508 20:b84beed117ef 211 sita = -1.0*(atan2((double)LSY-center,(double)LSX-center))*180/Pi;
yuto17320508 19:a3f57c9833b6 212 sita_2=90-sita;
yuto17320508 21:d9f5b74d5034 213 M1=sin((sita_2-(fai+0))*Pi/180)*power_f;
yuto17320508 21:d9f5b74d5034 214 M2=sin((sita_2-(fai+240))*Pi/180)*power_l;
yuto17320508 21:d9f5b74d5034 215 M3=sin((sita_2-(fai+120))*Pi/180)*power_r;
yuto17320508 19:a3f57c9833b6 216 motor_act();
yuto17320508 20:b84beed117ef 217 } else if(LSX>=0 && LSX<=center && LSY>=0 && LSY<=center) { //第二象限
yuto17320508 20:b84beed117ef 218 sita = -1.0*(atan2((double)LSY-center,(double)LSX-center))*180/Pi;
yuto17320508 19:a3f57c9833b6 219 sita_2=90-sita;
yuto17320508 21:d9f5b74d5034 220 M1=sin((sita_2-(fai+0))*Pi/180)*power_f;
yuto17320508 21:d9f5b74d5034 221 M2=sin((sita_2-(fai+240))*Pi/180)*power_l;
yuto17320508 21:d9f5b74d5034 222 M3=sin((sita_2-(fai+120))*Pi/180)*power_r;
yuto17320508 19:a3f57c9833b6 223 motor_act();
yuto17320508 20:b84beed117ef 224 } else if(LSX>=center && LSX<=255 && LSY>=0 && LSY<=center) { //第一象限
yuto17320508 20:b84beed117ef 225 sita = -1.0*(atan2((double)LSY-center,(double)LSX-center))*180/Pi;
yuto17320508 19:a3f57c9833b6 226 sita_2=90-sita;
yuto17320508 21:d9f5b74d5034 227 M1=sin((sita_2-(fai+0))*Pi/180)*power_f;
yuto17320508 21:d9f5b74d5034 228 M2=sin((sita_2-(fai+240))*Pi/180)*power_l;
yuto17320508 21:d9f5b74d5034 229 M3=sin((sita_2-(fai+120))*Pi/180)*power_r;
yuto17320508 19:a3f57c9833b6 230 motor_act();
yuto17320508 20:b84beed117ef 231 }
yuto17320508 19:a3f57c9833b6 232 //真っすぐだけのプログラム(いらない）
yuto17320508 20:b84beed117ef 233
yuto17320508 19:a3f57c9833b6 234 /*else if(LSX==255) {
yuto17320508 19:a3f57c9833b6 235 sita = 0;
yuto17320508 19:a3f57c9833b6 236 sita_2=90-sita;
yuto17320508 19:a3f57c9833b6 237 M1=sin((sita_2-fai)*Pi/180)*power_f;
yuto17320508 19:a3f57c9833b6 238 M2=sin((sita_2-fai+240)*Pi/180)*power_l;
yuto17320508 19:a3f57c9833b6 239 M3=sin((sita_2-fai+120)*Pi/180)*power_r;
yuto17320508 19:a3f57c9833b6 240 motor_act();
yuto17320508 18:2579c275ef57 241
yuto17320508 19:a3f57c9833b6 242 } else if(LSY==255) {
yuto17320508 19:a3f57c9833b6 243 sita = -90;
yuto17320508 19:a3f57c9833b6 244 sita_2=90-sita;
yuto17320508 19:a3f57c9833b6 245 M1=sin((sita_2-fai)*Pi/180)*power_f;
yuto17320508 19:a3f57c9833b6 246 M2=sin((sita_2-fai+240)*Pi/180)*power_l;
yuto17320508 19:a3f57c9833b6 247 M3=sin((sita_2-fai+120)*Pi/180)*power_r;
yuto17320508 19:a3f57c9833b6 248 motor_act();
yuto17320508 19:a3f57c9833b6 249
yuto17320508 19:a3f57c9833b6 250 } else if(LSX==0) {
yuto17320508 19:a3f57c9833b6 251 sita = 180;
yuto17320508 19:a3f57c9833b6 252 sita_2=90-sita;
yuto17320508 19:a3f57c9833b6 253 M1=sin((sita_2-fai)*Pi/180)*power_f;
yuto17320508 19:a3f57c9833b6 254 M2=sin((sita_2-fai+240)*Pi/180)*power_l;
yuto17320508 19:a3f57c9833b6 255 M3=sin((sita_2-fai+120)*Pi/180)*power_r;
yuto17320508 19:a3f57c9833b6 256 motor_act();
yuto17320508 19:a3f57c9833b6 257
yuto17320508 19:a3f57c9833b6 258 } else if(LSY==0) {
yuto17320508 19:a3f57c9833b6 259 sita = 90;
yuto17320508 19:a3f57c9833b6 260 sita_2=90-sita;
yuto17320508 19:a3f57c9833b6 261 M1=sin((sita_2-fai)*Pi/180)*power_f;
yuto17320508 19:a3f57c9833b6 262 M2=sin((sita_2-fai+240)*Pi/180)*power_l;
yuto17320508 19:a3f57c9833b6 263 M3=sin((sita_2-fai+120)*Pi/180)*power_r;
yuto17320508 19:a3f57c9833b6 264 motor_act();
yuto17320508 19:a3f57c9833b6 265 } */
yuto17320508 16:b232fd9ee9c2 266
yuto17320508 16:b232fd9ee9c2 267 }
yuto17320508 18:2579c275ef57 268
yuto17320508 18:2579c275ef57 269
yuto17320508 19:a3f57c9833b6 270
yuto17320508 19:a3f57c9833b6 271
yuto17320508 20:b84beed117ef 272 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 273
yuto17320508 18:2579c275ef57 274
yuto17320508 18:2579c275ef57 275
hirokimineshita 0:736c76a75def 276 }