F3RC1班 / Mbed 2 deprecated F3RC_syudou_master_3

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

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?

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 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 }