yuto kawamura / Mbed 2 deprecated F3RC_syudou_master

主導機　mbed用のプログラムです

Dependencies:   mbed

Fork of F3RC_syudou_master by F3RC1班

User.cpp@19:a3f57c9833b6, 2017-08-23 (annotated)

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