yuto kawamura / Mbed 2 deprecated F3RC_syudou_master

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

Dependencies:   mbed

Fork of F3RC_syudou_master by F3RC1班

User.cpp@18:2579c275ef57, 2017-08-23 (annotated)

Committer:
yuto17320508
Date:
Wed Aug 23 03:31:34 2017 +0000
Revision:
18:2579c275ef57
Parent:
17:c5c41fcf316e
Child:
19:a3f57c9833b6
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 18:2579c275ef57 31
yuto17320508 18:2579c275ef57 32
yuto17320508 18:2579c275ef57 33 double power_f=1;
yuto17320508 18:2579c275ef57 34 double power_l=1;
yuto17320508 18:2579c275ef57 35 double power_r=1;
yuto17320508 18:2579c275ef57 36
yuto17320508 18:2579c275ef57 37 double M1;
yuto17320508 18:2579c275ef57 38 double M2;
yuto17320508 18:2579c275ef57 39 double M3;
yuto17320508 18:2579c275ef57 40
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 18:2579c275ef57 173 printf("%d\r\n",send);
yuto17320508 18:2579c275ef57 174
yuto17320508 18:2579c275ef57 175
yuto17320508 18:2579c275ef57 176 if(LSX>=150 && LSX<=255 && LSY>=150 && LSY<=255) { //第一象限
yuto17320508 18:2579c275ef57 177 sita = -1.0*(atan2((double)LSY-128,(double)LSX-128))*180/Pi;
yuto17320508 18:2579c275ef57 178 sita_2=90-sita;
yuto17320508 18:2579c275ef57 179 M1=sin((sita_2-fai)*Pi/180)*power_f;
yuto17320508 18:2579c275ef57 180 M2=sin((sita_2-fai+240)*Pi/180)*power_l;
yuto17320508 18:2579c275ef57 181 M3=sin((sita_2-fai+120)*Pi/180)*power_r;
yuto17320508 18:2579c275ef57 182 motor_act();
yuto17320508 18:2579c275ef57 183 } else if(LSX>=0 && LSX<=80 && LSY>=150 && LSY<=255) { //第二象限
yuto17320508 18:2579c275ef57 184 sita = -1.0*(atan2((double)LSY-128,(double)LSX-128))*180/Pi;
yuto17320508 18:2579c275ef57 185 sita_2=90-sita;
yuto17320508 18:2579c275ef57 186 M1=sin((sita_2-fai)*Pi/180)*power_f;
yuto17320508 18:2579c275ef57 187 M2=sin((sita_2-fai+240)*Pi/180)*power_l;
yuto17320508 18:2579c275ef57 188 M3=sin((sita_2-fai+120)*Pi/180)*power_r;
yuto17320508 18:2579c275ef57 189 motor_act();
yuto17320508 18:2579c275ef57 190 } else if(LSX>=0 && LSX<=80 && LSY>=0 && LSY<=80) { //第三象限
yuto17320508 18:2579c275ef57 191 sita = -1.0*(atan2((double)LSY-128,(double)LSX-128))*180/Pi;
yuto17320508 18:2579c275ef57 192 sita_2=90-sita;
yuto17320508 18:2579c275ef57 193 M1=sin((sita_2-fai)*Pi/180)*power_f;
yuto17320508 18:2579c275ef57 194 M2=sin((sita_2-fai+240)*Pi/180)*power_l;
yuto17320508 18:2579c275ef57 195 M3=sin((sita_2-fai+120)*Pi/180)*power_r;
yuto17320508 18:2579c275ef57 196 motor_act();
yuto17320508 18:2579c275ef57 197 } else if(LSX>=150 && LSX<=255 && LSY>=0 && LSY<=80) { //第四象限
yuto17320508 18:2579c275ef57 198 sita = -1.0*(atan2((double)LSY-128,(double)LSX-128))*180/Pi;
yuto17320508 18:2579c275ef57 199 sita_2=90-sita;
yuto17320508 18:2579c275ef57 200 M1=sin((sita_2-fai)*Pi/180)*power_f;
yuto17320508 18:2579c275ef57 201 M2=sin((sita_2-fai+240)*Pi/180)*power_l;
yuto17320508 18:2579c275ef57 202 M3=sin((sita_2-fai+120)*Pi/180)*power_r;
yuto17320508 18:2579c275ef57 203 motor_act();
yuto17320508 16:b232fd9ee9c2 204
yuto17320508 18:2579c275ef57 205 } else if(LSX==255) {
yuto17320508 18:2579c275ef57 206 sita = 0;
yuto17320508 18:2579c275ef57 207 sita_2=90-sita;
yuto17320508 18:2579c275ef57 208 M1=sin((sita_2-fai)*Pi/180)*power_f;
yuto17320508 18:2579c275ef57 209 M2=sin((sita_2-fai+240)*Pi/180)*power_l;
yuto17320508 18:2579c275ef57 210 M3=sin((sita_2-fai+120)*Pi/180)*power_r;
yuto17320508 18:2579c275ef57 211 motor_act();
yuto17320508 18:2579c275ef57 212
yuto17320508 18:2579c275ef57 213 } else if(LSY==255) {
yuto17320508 18:2579c275ef57 214 sita = 90;
yuto17320508 18:2579c275ef57 215 sita_2=90-sita;
yuto17320508 18:2579c275ef57 216 M1=sin((sita_2-fai)*Pi/180)*power_f;
yuto17320508 18:2579c275ef57 217 M2=sin((sita_2-fai+240)*Pi/180)*power_l;
yuto17320508 18:2579c275ef57 218 M3=sin((sita_2-fai+120)*Pi/180)*power_r;
yuto17320508 18:2579c275ef57 219 motor_act();
yuto17320508 18:2579c275ef57 220
yuto17320508 18:2579c275ef57 221 } else if(LSX==0) {
yuto17320508 18:2579c275ef57 222 sita = 180;
yuto17320508 18:2579c275ef57 223 sita_2=90-sita;
yuto17320508 18:2579c275ef57 224 M1=sin((sita_2-fai)*Pi/180)*power_f;
yuto17320508 18:2579c275ef57 225 M2=sin((sita_2-fai+240)*Pi/180)*power_l;
yuto17320508 18:2579c275ef57 226 M3=sin((sita_2-fai+120)*Pi/180)*power_r;
yuto17320508 18:2579c275ef57 227 motor_act();
yuto17320508 18:2579c275ef57 228
yuto17320508 18:2579c275ef57 229 } else if(LSY==0) {
yuto17320508 18:2579c275ef57 230 sita = 270;
yuto17320508 18:2579c275ef57 231 sita_2=90-sita;
yuto17320508 18:2579c275ef57 232 M1=sin((sita_2-fai)*Pi/180)*power_f;
yuto17320508 18:2579c275ef57 233 M2=sin((sita_2-fai+240)*Pi/180)*power_l;
yuto17320508 18:2579c275ef57 234 M3=sin((sita_2-fai+120)*Pi/180)*power_r;
yuto17320508 18:2579c275ef57 235 motor_act();
yuto17320508 18:2579c275ef57 236 } else {
yuto17320508 18:2579c275ef57 237 motor_f_1=0;
yuto17320508 18:2579c275ef57 238 motor_f_2=0;
yuto17320508 18:2579c275ef57 239 motor_l_1=0;
yuto17320508 18:2579c275ef57 240 motor_l_2=0;
yuto17320508 18:2579c275ef57 241 motor_r_1=0;
yuto17320508 18:2579c275ef57 242 motor_r_2=0;
yuto17320508 16:b232fd9ee9c2 243
yuto17320508 16:b232fd9ee9c2 244 }
yuto17320508 18:2579c275ef57 245
yuto17320508 18:2579c275ef57 246
yuto17320508 18:2579c275ef57 247 printf("motor_f_1:%f\motor_l_1:%f\motor_r_1:%f\sita:%f\r\n",M1,M2,M3,sita);
yuto17320508 18:2579c275ef57 248
yuto17320508 18:2579c275ef57 249
yuto17320508 18:2579c275ef57 250
hirokimineshita 0:736c76a75def 251 }