主導機 mbed用のプログラムです 改良しました
Dependencies: mbed
Fork of F3RC_syudou_master_3 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
- Child:
- 23:33a40d8c0535
a
Who changed what in which revision?
User | Revision | Line number | New 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 | l↙ ↖f |
yuto17320508 | 21:d9f5b74d5034 | 21 | |
yuto17320508 | 21:d9f5b74d5034 | 22 | → |
yuto17320508 | 21:d9f5b74d5034 | 23 | r */ |
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 | } |