春ロボ1班(元F3RC4班+) / Mbed 2 deprecated F3RC-mbed-new

仮

Dependencies:   mbed move3wheel

Fork of PS3_BlueUSB_user_ver2 by ROBOSTEP_SHARE

User.cpp@17:95cb86ce56a9, 2018-09-05 (annotated)

Committer:
yuki0701
Date:
Wed Sep 05 11:42:29 2018 +0000
Revision:
17:95cb86ce56a9
Parent:
16:55c180a4338c 
??;

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
hirokimineshita 0:736c76a75def 7 #include "mbed.h"
yuki0701 16:55c180a4338c 8 #include "math.h"
yuki0701 16:55c180a4338c 9 #include "move3wheel.h"
yuki0701 16:55c180a4338c 10
hirokimineshita 0:736c76a75def 11
hirokimineshita 0:736c76a75def 12 int RSX,RSY,LSX,LSY,BSU,BSL;
yuki0701 16:55c180a4338c 13 /*DigitalOut led1(LED1);
yuki0701 16:55c180a4338c 14 DigitalOut led2(LED2);
hirokimineshita 0:736c76a75def 15 DigitalOut led3(LED3);
yuki0701 16:55c180a4338c 16 DigitalOut led4(LED4);*/
yuki0701 16:55c180a4338c 17
yuki0701 16:55c180a4338c 18 SPI spi(p5,p6,p7);
yuki0701 16:55c180a4338c 19 DigitalOut cs(p8);
yuki0701 16:55c180a4338c 20 int data1 = 0;
yuki0701 16:55c180a4338c 21 int a; //aは定数
yuki0701 16:55c180a4338c 22 double receive_data/*,true_data*/;
yuki0701 16:55c180a4338c 23 double now_angle,target_angle,out;
yuki0701 17:95cb86ce56a9 24 int count=0;
baba2357 11:86d717718dbf 25
yuki0701 16:55c180a4338c 26 PwmOut motor0CW(p21); //0番motor 外から見て時計回り
yuki0701 16:55c180a4338c 27 PwmOut motor0CCW(p22); //0番motor 反時計回り
yuki0701 16:55c180a4338c 28 PwmOut motor1CW(p23); //1番motor
yuki0701 16:55c180a4338c 29 PwmOut motor1CCW(p24); //1番motor
yuki0701 16:55c180a4338c 30 PwmOut motor2CW(p26); //2番motor
yuki0701 16:55c180a4338c 31 PwmOut motor2CCW(p25); //2番motor
yuki0701 16:55c180a4338c 32
yuki0701 16:55c180a4338c 33
yuki0701 16:55c180a4338c 34 void motor(double cw0,double ccw0,double cw1,double ccw1,double cw2,double ccw2)
yuki0701 16:55c180a4338c 35 {
yuki0701 16:55c180a4338c 36 motor0CW = cw0;
yuki0701 16:55c180a4338c 37 motor0CCW = ccw0;
yuki0701 16:55c180a4338c 38 motor1CW = cw1;
yuki0701 16:55c180a4338c 39 motor1CCW = ccw1;
yuki0701 16:55c180a4338c 40 motor2CW = cw2;
yuki0701 16:55c180a4338c 41 motor2CCW = ccw2;
hirokimineshita 0:736c76a75def 42 }
hirokimineshita 0:736c76a75def 43
yuki0701 16:55c180a4338c 44
yuki0701 16:55c180a4338c 45
yuki0701 16:55c180a4338c 46 void UserLoopSetting()
yuki0701 16:55c180a4338c 47 {
yuki0701 16:55c180a4338c 48 spi.format(16,3);
yuki0701 16:55c180a4338c 49 spi.frequency(1000000);
yuki0701 16:55c180a4338c 50
yuki0701 16:55c180a4338c 51 motor0CW.period_us(50);
yuki0701 16:55c180a4338c 52 motor0CCW.period_us(50);
yuki0701 16:55c180a4338c 53 motor1CW.period_us(50);
yuki0701 16:55c180a4338c 54 motor1CCW.period_us(50);
yuki0701 16:55c180a4338c 55 motor2CW.period_us(50);
yuki0701 16:55c180a4338c 56 motor2CCW.period_us(50);
yuki0701 16:55c180a4338c 57 }
yuki0701 16:55c180a4338c 58
yuki0701 16:55c180a4338c 59 void UserLoop(char n,const u8* data)
yuki0701 16:55c180a4338c 60 {
hirokimineshita 0:736c76a75def 61 u16 ButtonState;
yuki0701 16:55c180a4338c 62 if(n==0) { //有線Ps3USB.cpp
hirokimineshita 0:736c76a75def 63 RSX = ((ps3report*)data)->RightStickX;
hirokimineshita 0:736c76a75def 64 RSY = ((ps3report*)data)->RightStickY;
hirokimineshita 0:736c76a75def 65 LSX = ((ps3report*)data)->LeftStickX;
hirokimineshita 0:736c76a75def 66 LSY = ((ps3report*)data)->LeftStickY;
hirokimineshita 0:736c76a75def 67 BSU = (u8)(((ps3report*)data)->ButtonState & 0x00ff);
hirokimineshita 0:736c76a75def 68 BSL = (u8)(((ps3report*)data)->ButtonState >> 8);
hirokimineshita 0:736c76a75def 69 //ボタンの処理
hirokimineshita 0:736c76a75def 70 ButtonState = ((ps3report*)data)->ButtonState;
yuki0701 16:55c180a4338c 71 } else {//無線TestShell.cpp
hirokimineshita 0:736c76a75def 72 RSX = ((ps3report*)(data + 1))->RightStickX;
hirokimineshita 0:736c76a75def 73 RSY = ((ps3report*)(data + 1))->RightStickY;
hirokimineshita 0:736c76a75def 74 LSX = ((ps3report*)(data + 1))->LeftStickX;
hirokimineshita 0:736c76a75def 75 LSY = ((ps3report*)(data + 1))->LeftStickY;
hirokimineshita 0:736c76a75def 76 BSU = (u8)(((ps3report*)(data + 1))->ButtonState & 0x00ff);
hirokimineshita 0:736c76a75def 77 BSL = (u8)(((ps3report*)(data + 1))->ButtonState >> 8);
hirokimineshita 0:736c76a75def 78 //ボタンの処理
hirokimineshita 0:736c76a75def 79 ButtonState = ((ps3report*)(data + 1))->ButtonState;
hirokimineshita 0:736c76a75def 80 }
yuki0701 16:55c180a4338c 81
yuki0701 16:55c180a4338c 82 ///////////////////////////ここからmotorのプログラム///////////////////////////////
yuki0701 16:55c180a4338c 83
yuki0701 16:55c180a4338c 84 /*int a = (ButtonState >> BUTTONRIGHT)&1;
yuki0701 16:55c180a4338c 85 if(a == 0)led4=0;
yuki0701 16:55c180a4338c 86 else led4=1;*/
yuki0701 16:55c180a4338c 87
yuki0701 16:55c180a4338c 88 //printf("RSX = %d, RSY = %d\r\n", RSX, RSY);
yuki0701 16:55c180a4338c 89 if(RSX >=100 && RSX <150 && RSY >=100 && RSY <150
yuki0701 17:95cb86ce56a9 90 && (((ButtonState >> BUTTONRIGHT)&1) == 0) && (((ButtonState >> BUTTONDOWN)&1) == 0) && (((ButtonState >> BUTTONUP)&1) == 0)) { //ニュートラルポジション
yuki0701 16:55c180a4338c 91 motor(0,0,0,0,0,0);
yuki0701 16:55c180a4338c 92 /*led1 = 1;
yuki0701 16:55c180a4338c 93 led2 = 0;
yuki0701 16:55c180a4338c 94 led3 = 0;
yuki0701 16:55c180a4338c 95 led4 = 0;*/
yuki0701 16:55c180a4338c 96
yuki0701 16:55c180a4338c 97 if(LSX >=100 && LSX <150 ) { //左スティックニュートラルポジション
yuki0701 16:55c180a4338c 98 motor(0,0,0,0,0,0);
yuki0701 16:55c180a4338c 99
yuki0701 16:55c180a4338c 100 } else if(LSX >=150 ) { //右回転
yuki0701 17:95cb86ce56a9 101 if((ButtonState >> BUTTONR1)&1 == 1) {
yuki0701 17:95cb86ce56a9 102 motor(0,0.13,0,0.13,0,0.13);
yuki0701 17:95cb86ce56a9 103 } else {
yuki0701 17:95cb86ce56a9 104 motor(0,0.3,0,0.3,0,0.3);
yuki0701 17:95cb86ce56a9 105 }
yuki0701 16:55c180a4338c 106
yuki0701 16:55c180a4338c 107 } else if(LSX <100 ) { //左回転
yuki0701 17:95cb86ce56a9 108 if((ButtonState >> BUTTONR1)&1 == 1) {
yuki0701 17:95cb86ce56a9 109 motor(0.13,0,0.13,0,0.13,0);
yuki0701 17:95cb86ce56a9 110 } else {
yuki0701 17:95cb86ce56a9 111 motor(0.3,0,0.3,0,0.3,0);
yuki0701 17:95cb86ce56a9 112 }
yuki0701 16:55c180a4338c 113
yuki0701 16:55c180a4338c 114 } else {
yuki0701 16:55c180a4338c 115 motor(0,0,0,0,0,0);
yuki0701 16:55c180a4338c 116 }
yuki0701 17:95cb86ce56a9 117
yuki0701 17:95cb86ce56a9 118
yuki0701 17:95cb86ce56a9 119
yuki0701 17:95cb86ce56a9 120 } else if(RSX >=100 && RSX <150 && RSY <100) { // ↑ 移動
yuki0701 17:95cb86ce56a9 121
yuki0701 17:95cb86ce56a9 122 if((ButtonState >> BUTTONR1)&1 == 1) {
yuki0701 17:95cb86ce56a9 123 CalMotorOut(0,0.13,0);
yuki0701 17:95cb86ce56a9 124
yuki0701 17:95cb86ce56a9 125 } else {
yuki0701 17:95cb86ce56a9 126 CalMotorOut(0,0.3,0); //走らせたい速さを入れる(X方向,y方向,回転) 回転方向は上から見て反時計回りが正
yuki0701 17:95cb86ce56a9 127 }
yuki0701 17:95cb86ce56a9 128
yuki0701 17:95cb86ce56a9 129 if(now_angle > 135 && now_angle < 225) {
yuki0701 17:95cb86ce56a9 130 motor(0,0,0,GetMotorOut(1),-GetMotorOut(2),0);
yuki0701 17:95cb86ce56a9 131
yuki0701 17:95cb86ce56a9 132 } else if(now_angle > 225 && now_angle < 315) {
yuki0701 17:95cb86ce56a9 133 if((ButtonState >> BUTTONR1)&1 == 1) {
yuki0701 17:95cb86ce56a9 134 CalMotorOut(0.13,0,0);
yuki0701 17:95cb86ce56a9 135
yuki0701 17:95cb86ce56a9 136 } else {
yuki0701 17:95cb86ce56a9 137 CalMotorOut(0.3,0,0);
yuki0701 17:95cb86ce56a9 138 }
yuki0701 17:95cb86ce56a9 139 motor(0,GetMotorOut(0),GetMotorOut(1),0,GetMotorOut(2),0);
yuki0701 17:95cb86ce56a9 140
yuki0701 17:95cb86ce56a9 141 } else {
yuki0701 17:95cb86ce56a9 142 motor(0,0,GetMotorOut(1),0,0,-GetMotorOut(2)); //1番motorはCW方向、2番motorはCCW方向に回す
yuki0701 17:95cb86ce56a9 143
yuki0701 17:95cb86ce56a9 144 }
yuki0701 17:95cb86ce56a9 145
yuki0701 17:95cb86ce56a9 146 } else if(RSX >=100 && RSX <150 && RSY >=150) { // ↓ 移動
yuki0701 17:95cb86ce56a9 147 if((ButtonState >> BUTTONR1)&1 == 1) {
yuki0701 17:95cb86ce56a9 148 CalMotorOut(0,0.13,0);
yuki0701 17:95cb86ce56a9 149
yuki0701 17:95cb86ce56a9 150 } else {
yuki0701 17:95cb86ce56a9 151 CalMotorOut(0,0.3,0);
yuki0701 17:95cb86ce56a9 152 }
yuki0701 17:95cb86ce56a9 153
yuki0701 17:95cb86ce56a9 154 if(now_angle > 135 && now_angle < 225) {
yuki0701 17:95cb86ce56a9 155 motor(0,0,GetMotorOut(1),0,0,-GetMotorOut(2));
yuki0701 17:95cb86ce56a9 156
yuki0701 17:95cb86ce56a9 157 } else if(now_angle > 225 && now_angle < 315) {
yuki0701 17:95cb86ce56a9 158 if((ButtonState >> BUTTONR1)&1 == 1) {
yuki0701 17:95cb86ce56a9 159 CalMotorOut(0.13,0,0);
yuki0701 17:95cb86ce56a9 160
yuki0701 17:95cb86ce56a9 161 } else {
yuki0701 17:95cb86ce56a9 162 CalMotorOut(0.3,0,0);
yuki0701 17:95cb86ce56a9 163 }
yuki0701 17:95cb86ce56a9 164 motor(GetMotorOut(0),0,0,GetMotorOut(1),0,GetMotorOut(2));
yuki0701 17:95cb86ce56a9 165
yuki0701 17:95cb86ce56a9 166 } else {
yuki0701 17:95cb86ce56a9 167 motor(0,0,0,GetMotorOut(1),-GetMotorOut(2),0); //前進と逆の方向に回す
yuki0701 17:95cb86ce56a9 168
yuki0701 17:95cb86ce56a9 169 }
yuki0701 17:95cb86ce56a9 170
yuki0701 17:95cb86ce56a9 171 } else if(RSX >=150 && RSY >=100 && RSY <150) { // → 移動
yuki0701 17:95cb86ce56a9 172 if((ButtonState >> BUTTONR1)&1 == 1) {
yuki0701 17:95cb86ce56a9 173 CalMotorOut(0.13,0,0);
yuki0701 17:95cb86ce56a9 174
yuki0701 17:95cb86ce56a9 175 } else {
yuki0701 17:95cb86ce56a9 176 CalMotorOut(0.3,0,0);
yuki0701 17:95cb86ce56a9 177 }
yuki0701 17:95cb86ce56a9 178
yuki0701 17:95cb86ce56a9 179 if(now_angle > 135 && now_angle < 225) {
yuki0701 17:95cb86ce56a9 180 motor(0,GetMotorOut(0),GetMotorOut(1),0,GetMotorOut(2),0);
yuki0701 17:95cb86ce56a9 181
yuki0701 17:95cb86ce56a9 182 } else if(now_angle > 225 && now_angle < 315) {
yuki0701 17:95cb86ce56a9 183 if((ButtonState >> BUTTONR1)&1 == 1) {
yuki0701 17:95cb86ce56a9 184 CalMotorOut(0,0.13,0);
yuki0701 17:95cb86ce56a9 185
yuki0701 17:95cb86ce56a9 186 } else {
yuki0701 17:95cb86ce56a9 187 CalMotorOut(0,0.3,0);
yuki0701 17:95cb86ce56a9 188 }
yuki0701 17:95cb86ce56a9 189 motor(0,0,GetMotorOut(1),0,0,-GetMotorOut(2));
yuki0701 17:95cb86ce56a9 190
yuki0701 17:95cb86ce56a9 191 } else {
yuki0701 17:95cb86ce56a9 192 motor(GetMotorOut(0),0,0,GetMotorOut(1),0,GetMotorOut(2));
yuki0701 17:95cb86ce56a9 193
yuki0701 17:95cb86ce56a9 194 }
yuki0701 17:95cb86ce56a9 195
yuki0701 17:95cb86ce56a9 196 } else if(RSX <100 && RSY >=100 && RSY <150) { // ← 移動
yuki0701 17:95cb86ce56a9 197 if((ButtonState >> BUTTONR1)&1 == 1) {
yuki0701 17:95cb86ce56a9 198 CalMotorOut(0.13,0,0);
yuki0701 17:95cb86ce56a9 199
yuki0701 17:95cb86ce56a9 200 } else {
yuki0701 17:95cb86ce56a9 201 CalMotorOut(0.3,0,0);
yuki0701 17:95cb86ce56a9 202 }
yuki0701 17:95cb86ce56a9 203
yuki0701 17:95cb86ce56a9 204 if(now_angle > 135 && now_angle < 225) {
yuki0701 17:95cb86ce56a9 205 motor(GetMotorOut(0),0,0,GetMotorOut(1),0,GetMotorOut(2));
yuki0701 17:95cb86ce56a9 206
yuki0701 17:95cb86ce56a9 207 } else if(now_angle > 225 && now_angle < 315) {
yuki0701 17:95cb86ce56a9 208 if((ButtonState >> BUTTONR1)&1 == 1) {
yuki0701 17:95cb86ce56a9 209 CalMotorOut(0,0.13,0);
yuki0701 17:95cb86ce56a9 210
yuki0701 17:95cb86ce56a9 211 } else {
yuki0701 17:95cb86ce56a9 212 CalMotorOut(0,0.3,0);
yuki0701 17:95cb86ce56a9 213 }
yuki0701 17:95cb86ce56a9 214 motor(0,0,0,GetMotorOut(1),-GetMotorOut(2),0);
yuki0701 17:95cb86ce56a9 215
yuki0701 17:95cb86ce56a9 216 } else {
yuki0701 17:95cb86ce56a9 217 motor(0,GetMotorOut(0),GetMotorOut(1),0,GetMotorOut(2),0);
yuki0701 17:95cb86ce56a9 218
yuki0701 17:95cb86ce56a9 219 }
yuki0701 17:95cb86ce56a9 220
yuki0701 17:95cb86ce56a9 221 } else if(RSX < 100 && RSY <100) { // 左上移動
yuki0701 17:95cb86ce56a9 222 if((ButtonState >> BUTTONR1)&1 == 1) {
yuki0701 17:95cb86ce56a9 223 CalMotorOut(0.1,0.1,0);
yuki0701 17:95cb86ce56a9 224
yuki0701 17:95cb86ce56a9 225 } else {
yuki0701 17:95cb86ce56a9 226 CalMotorOut(0.2,0.2,0);
yuki0701 17:95cb86ce56a9 227 }
yuki0701 17:95cb86ce56a9 228
yuki0701 17:95cb86ce56a9 229 if(now_angle > 135 && now_angle < 225) {
yuki0701 17:95cb86ce56a9 230 motor(GetMotorOut(0),0,0,GetMotorOut(1),-GetMotorOut(2),0);
yuki0701 17:95cb86ce56a9 231
yuki0701 17:95cb86ce56a9 232 } else if(now_angle > 225 && now_angle < 315) {
yuki0701 17:95cb86ce56a9 233 if((ButtonState >> BUTTONR1)&1 == 1) {
yuki0701 17:95cb86ce56a9 234 CalMotorOut(0.1,-0.1,0);
yuki0701 17:95cb86ce56a9 235
yuki0701 17:95cb86ce56a9 236 } else {
yuki0701 17:95cb86ce56a9 237 CalMotorOut(0.2,-0.2,0);
yuki0701 17:95cb86ce56a9 238 }
yuki0701 17:95cb86ce56a9 239 motor(0,GetMotorOut(0),0,-GetMotorOut(1),GetMotorOut(2),0);
yuki0701 17:95cb86ce56a9 240
yuki0701 17:95cb86ce56a9 241 } else {
yuki0701 17:95cb86ce56a9 242 motor(0,GetMotorOut(0),GetMotorOut(1),0,0,-GetMotorOut(2));
yuki0701 17:95cb86ce56a9 243
yuki0701 17:95cb86ce56a9 244 }
yuki0701 17:95cb86ce56a9 245
yuki0701 17:95cb86ce56a9 246 } else if(RSX >= 150 && RSY <100) { // 右上移動
yuki0701 17:95cb86ce56a9 247 if((ButtonState >> BUTTONR1)&1 == 1) {
yuki0701 17:95cb86ce56a9 248 CalMotorOut(-0.1,0.1,0);
yuki0701 17:95cb86ce56a9 249
yuki0701 17:95cb86ce56a9 250 } else {
yuki0701 17:95cb86ce56a9 251 CalMotorOut(-0.2,0.2,0);
yuki0701 17:95cb86ce56a9 252 }
yuki0701 17:95cb86ce56a9 253
yuki0701 17:95cb86ce56a9 254 if(now_angle > 135 && now_angle < 225) {
yuki0701 17:95cb86ce56a9 255 motor(0,-GetMotorOut(0),0,GetMotorOut(1),-GetMotorOut(2),0);
yuki0701 17:95cb86ce56a9 256
yuki0701 17:95cb86ce56a9 257 } else if(now_angle > 225 && now_angle < 315) {
yuki0701 17:95cb86ce56a9 258 if((ButtonState >> BUTTONR1)&1 == 1) {
yuki0701 17:95cb86ce56a9 259 CalMotorOut(0.1,0.1,0);
yuki0701 17:95cb86ce56a9 260
yuki0701 17:95cb86ce56a9 261 } else {
yuki0701 17:95cb86ce56a9 262 CalMotorOut(0.2,0.2,0);
yuki0701 17:95cb86ce56a9 263 }
yuki0701 17:95cb86ce56a9 264 motor(0,GetMotorOut(0),GetMotorOut(1),0,0,-GetMotorOut(2));
yuki0701 17:95cb86ce56a9 265
yuki0701 17:95cb86ce56a9 266 } else {
yuki0701 17:95cb86ce56a9 267 motor(-GetMotorOut(0),0,GetMotorOut(1),0,0,-GetMotorOut(2));
yuki0701 17:95cb86ce56a9 268
yuki0701 17:95cb86ce56a9 269 }
yuki0701 17:95cb86ce56a9 270
yuki0701 17:95cb86ce56a9 271 } else if(RSX <100 && RSY >=150) { // 左下移動
yuki0701 17:95cb86ce56a9 272 if((ButtonState >> BUTTONR1)&1 == 1) {
yuki0701 17:95cb86ce56a9 273 CalMotorOut(0.1,-0.1,0);
yuki0701 17:95cb86ce56a9 274
yuki0701 17:95cb86ce56a9 275 } else {
yuki0701 17:95cb86ce56a9 276 CalMotorOut(0.2,-0.2,0);
yuki0701 17:95cb86ce56a9 277 }
yuki0701 17:95cb86ce56a9 278
yuki0701 17:95cb86ce56a9 279 if(now_angle > 135 && now_angle < 225) {
yuki0701 17:95cb86ce56a9 280 motor(GetMotorOut(0),0,-GetMotorOut(1),0,0,GetMotorOut(2));
yuki0701 17:95cb86ce56a9 281
yuki0701 17:95cb86ce56a9 282 } else if(now_angle > 225 && now_angle < 315) {
yuki0701 17:95cb86ce56a9 283 if((ButtonState >> BUTTONR1)&1 == 1) {
yuki0701 17:95cb86ce56a9 284 CalMotorOut(-0.1,-0.1,0);
yuki0701 17:95cb86ce56a9 285
yuki0701 17:95cb86ce56a9 286 } else {
yuki0701 17:95cb86ce56a9 287 CalMotorOut(-0.2,-0.2,0);
yuki0701 17:95cb86ce56a9 288 }
yuki0701 17:95cb86ce56a9 289 motor(-GetMotorOut(0),0,0,-GetMotorOut(1),GetMotorOut(2),0);
yuki0701 17:95cb86ce56a9 290
yuki0701 17:95cb86ce56a9 291 } else {
yuki0701 17:95cb86ce56a9 292 motor(0,GetMotorOut(0),0,-GetMotorOut(1),GetMotorOut(2),0);
yuki0701 17:95cb86ce56a9 293
yuki0701 17:95cb86ce56a9 294 }
yuki0701 17:95cb86ce56a9 295
yuki0701 17:95cb86ce56a9 296 } else if(RSX >= 150 && RSY >=150) { // 右下移動
yuki0701 17:95cb86ce56a9 297 if((ButtonState >> BUTTONR1)&1 == 1) {
yuki0701 17:95cb86ce56a9 298 CalMotorOut(-0.1,-0.1,0);
yuki0701 17:95cb86ce56a9 299
yuki0701 17:95cb86ce56a9 300 } else {
yuki0701 17:95cb86ce56a9 301 CalMotorOut(-0.2,-0.2,0);
yuki0701 17:95cb86ce56a9 302 }
yuki0701 17:95cb86ce56a9 303
yuki0701 17:95cb86ce56a9 304 if(now_angle > 135 && now_angle < 225) {
yuki0701 17:95cb86ce56a9 305 motor(0,-GetMotorOut(0),-GetMotorOut(1),0,0,GetMotorOut(2));
yuki0701 17:95cb86ce56a9 306
yuki0701 17:95cb86ce56a9 307 } else if(now_angle > 225 && now_angle < 315) {
yuki0701 17:95cb86ce56a9 308 if((ButtonState >> BUTTONR1)&1 == 1) {
yuki0701 17:95cb86ce56a9 309 CalMotorOut(-0.1,-0.1,0);
yuki0701 17:95cb86ce56a9 310
yuki0701 17:95cb86ce56a9 311 } else {
yuki0701 17:95cb86ce56a9 312 CalMotorOut(-0.2,0.2,0);
yuki0701 17:95cb86ce56a9 313 }
yuki0701 17:95cb86ce56a9 314 motor(-GetMotorOut(0),0,GetMotorOut(1),0,0,-GetMotorOut(2));
yuki0701 17:95cb86ce56a9 315
yuki0701 17:95cb86ce56a9 316 } else {
yuki0701 17:95cb86ce56a9 317 motor(-GetMotorOut(0),0,0,-GetMotorOut(1),GetMotorOut(2),0);
yuki0701 17:95cb86ce56a9 318
yuki0701 17:95cb86ce56a9 319 }
yuki0701 17:95cb86ce56a9 320
yuki0701 16:55c180a4338c 321 }
yuki0701 17:95cb86ce56a9 322
yuki0701 17:95cb86ce56a9 323
yuki0701 16:55c180a4338c 324 ////////////////////////////ここからヌクレオのプログラム/////////////////////////////
yuki0701 16:55c180a4338c 325
yuki0701 16:55c180a4338c 326 if((ButtonState >> BUTTONTRIANGEL)&1 == 1) { //servo1
yuki0701 16:55c180a4338c 327 wait(0.1);
yuki0701 16:55c180a4338c 328 if((ButtonState >> BUTTONTRIANGEL)&1 == 1) {
yuki0701 16:55c180a4338c 329 data1=1;
yuki0701 16:55c180a4338c 330
yuki0701 16:55c180a4338c 331 }
yuki0701 16:55c180a4338c 332 }
yuki0701 16:55c180a4338c 333
yuki0701 16:55c180a4338c 334
yuki0701 16:55c180a4338c 335 if((ButtonState >> BUTTONCIRCLE)&1 == 1) { //servo2
yuki0701 16:55c180a4338c 336 wait(0.1);
yuki0701 16:55c180a4338c 337 if((ButtonState >> BUTTONCIRCLE)&1 == 1) {
yuki0701 16:55c180a4338c 338 data1=2;
yuki0701 16:55c180a4338c 339 }
yuki0701 16:55c180a4338c 340 }
yuki0701 16:55c180a4338c 341
yuki0701 16:55c180a4338c 342
yuki0701 16:55c180a4338c 343 if((ButtonState >> BUTTONCROSS)&1 == 1) { //servo3
yuki0701 16:55c180a4338c 344 wait(0.1);
yuki0701 16:55c180a4338c 345 if((ButtonState >> BUTTONCROSS)&1 == 1) {
yuki0701 16:55c180a4338c 346 data1=3;
yuki0701 16:55c180a4338c 347 }
yuki0701 16:55c180a4338c 348 }
yuki0701 16:55c180a4338c 349
yuki0701 16:55c180a4338c 350
yuki0701 16:55c180a4338c 351 if((ButtonState >> BUTTONSQUARE)&1 == 1) { //servo4
yuki0701 16:55c180a4338c 352 wait(0.1);
yuki0701 16:55c180a4338c 353 if((ButtonState >> BUTTONSQUARE)&1 == 1) {
yuki0701 16:55c180a4338c 354 data1=4;
yuki0701 16:55c180a4338c 355 }
yuki0701 16:55c180a4338c 356 }
yuki0701 16:55c180a4338c 357
yuki0701 16:55c180a4338c 358
yuki0701 16:55c180a4338c 359 if((ButtonState >> BUTTONLEFT)&1 == 1) { //エアシリンダー
yuki0701 16:55c180a4338c 360 wait(0.1);
yuki0701 16:55c180a4338c 361 if((ButtonState >> BUTTONLEFT)&1 == 1) {
yuki0701 16:55c180a4338c 362 data1=5;
yuki0701 16:55c180a4338c 363 }
yuki0701 16:55c180a4338c 364 }
yuki0701 16:55c180a4338c 365
yuki0701 16:55c180a4338c 366 ////////////////////////ここからジャイロの角度指定プログラム//////////////////////////
yuki0701 16:55c180a4338c 367
yuki0701 16:55c180a4338c 368 if(receive_data >= 0) { //反時計回りに0~359°となるよう修正
yuki0701 16:55c180a4338c 369 a = receive_data / 360;
yuki0701 16:55c180a4338c 370 now_angle = receive_data - (360 * a); //現在の角度
yuki0701 16:55c180a4338c 371 } else {
yuki0701 16:55c180a4338c 372 a = receive_data / 360;
yuki0701 17:95cb86ce56a9 373 now_angle = 360 + receive_data - (360 * a);
yuki0701 16:55c180a4338c 374 }
yuki0701 16:55c180a4338c 375
yuki0701 16:55c180a4338c 376
yuki0701 16:55c180a4338c 377 /*true_data = receive_data - 65536;
yuki0701 16:55c180a4338c 378 } else {
yuki0701 16:55c180a4338c 379 true_data = receive_data;
yuki0701 16:55c180a4338c 380 }*/
yuki0701 16:55c180a4338c 381
yuki0701 16:55c180a4338c 382
yuki0701 16:55c180a4338c 383 //now_angle = true_data; //現在の角度
yuki0701 16:55c180a4338c 384 //out = 0.01 * (target_angle - now_angle); //P(目標の角度 - 現在の角度) Pは自分で決めた定数
yuki0701 16:55c180a4338c 385
yuki0701 16:55c180a4338c 386 if((ButtonState >> BUTTONRIGHT)&1 == 1) {
yuki0701 16:55c180a4338c 387 wait(0.1);
yuki0701 16:55c180a4338c 388 if((ButtonState >> BUTTONRIGHT)&1 == 1) {
yuki0701 17:95cb86ce56a9 389 target_angle = 285;
yuki0701 16:55c180a4338c 390 out = 0.01 * (target_angle - now_angle);
yuki0701 16:55c180a4338c 391
yuki0701 16:55c180a4338c 392 //printf("%f\r\n",now_angle);
yuki0701 16:55c180a4338c 393 //printf("%f\r\n",out);
yuki0701 17:95cb86ce56a9 394 if(now_angle >= 284 && now_angle <= 286) {
yuki0701 17:95cb86ce56a9 395 motor(0,0,0,0,0,0);
yuki0701 17:95cb86ce56a9 396
yuki0701 16:55c180a4338c 397 } else if(out > 1.8 || out <= -0.3) { //0～89°と270～359°のときは時計回りに回転
yuki0701 16:55c180a4338c 398 motor(0.3,0,0.3,0,0.3,0);
yuki0701 16:55c180a4338c 399 //printf("cw 0.3\r\n");
yuki0701 16:55c180a4338c 400
yuki0701 16:55c180a4338c 401 } else if(out > -0.3 && out <= 0) {
yuki0701 16:55c180a4338c 402 motor(-out,0,-out,0,-out,0);
yuki0701 16:55c180a4338c 403 //printf("cw -out\r\n");
yuki0701 16:55c180a4338c 404
yuki0701 16:55c180a4338c 405 } else if(out > 0 && out <= 0.3) { //90～269°のときは反時計回りに回転
yuki0701 16:55c180a4338c 406 motor(0,out,0,out,0,out);
yuki0701 16:55c180a4338c 407 //printf("ccw out\r\n");
yuki0701 16:55c180a4338c 408
yuki0701 16:55c180a4338c 409 } else if(out <= 1.8 && out > 0.3) {
yuki0701 16:55c180a4338c 410 motor(0,0.3,0,0.3,0,0.3);
yuki0701 16:55c180a4338c 411 //printf("ccw 0.3\r\n");
yuki0701 16:55c180a4338c 412 }
yuki0701 16:55c180a4338c 413 }
yuki0701 16:55c180a4338c 414 }
yuki0701 16:55c180a4338c 415
yuki0701 16:55c180a4338c 416 if((ButtonState >> BUTTONDOWN)&1 == 1) {
yuki0701 16:55c180a4338c 417 wait(0.1);
yuki0701 16:55c180a4338c 418 if((ButtonState >> BUTTONDOWN)&1 == 1) {
yuki0701 17:95cb86ce56a9 419 target_angle = 197;
yuki0701 16:55c180a4338c 420 out = 0.01 * (target_angle - now_angle);
yuki0701 16:55c180a4338c 421
yuki0701 17:95cb86ce56a9 422 if(now_angle >= 196 && now_angle <= 198) {
yuki0701 16:55c180a4338c 423 motor(0,0,0,0,0,0);
yuki0701 17:95cb86ce56a9 424
yuki0701 16:55c180a4338c 425 } else if(out > 0.3) { //0～178°のときは反時計回りに回転
yuki0701 16:55c180a4338c 426 motor(0,0.3,0,0.3,0,0.3);
yuki0701 16:55c180a4338c 427
yuki0701 16:55c180a4338c 428 } else if(out <= 0.3 && out > 0) {
yuki0701 16:55c180a4338c 429 motor(0,out,0,out,0,out);
yuki0701 16:55c180a4338c 430
yuki0701 16:55c180a4338c 431 } else if(out <= 0 && out > -0.3) { //180～359°のときは時計回りに回転
yuki0701 16:55c180a4338c 432 motor(-out,0,-out,0,-out,0);
yuki0701 16:55c180a4338c 433
yuki0701 16:55c180a4338c 434 } else if(out <= -0.3) {
yuki0701 16:55c180a4338c 435 motor(0.3,0,0.3,0,0.3,0);
yuki0701 16:55c180a4338c 436 }
yuki0701 16:55c180a4338c 437 }
yuki0701 16:55c180a4338c 438 }
yuki0701 17:95cb86ce56a9 439
yuki0701 16:55c180a4338c 440 if((ButtonState >> BUTTONUP)&1 == 1) {
yuki0701 16:55c180a4338c 441 wait(0.1);
yuki0701 16:55c180a4338c 442 if((ButtonState >> BUTTONUP)&1 == 1) {
yuki0701 16:55c180a4338c 443 if(0 <= now_angle && now_angle < 180) {
yuki0701 16:55c180a4338c 444 target_angle = 0;
yuki0701 16:55c180a4338c 445 out = -0.01 * (target_angle - now_angle);
yuki0701 16:55c180a4338c 446
yuki0701 16:55c180a4338c 447 if(now_angle <= 1) {
yuki0701 16:55c180a4338c 448 motor(0,0,0,0,0,0);
yuki0701 16:55c180a4338c 449
yuki0701 16:55c180a4338c 450 } else if(out > 0.3) { //0～179°のときは時計回りに回転
yuki0701 16:55c180a4338c 451 motor(0.3,0,0.3,0,0.3,0);
yuki0701 16:55c180a4338c 452
yuki0701 16:55c180a4338c 453 } else if(out <= 0.3 && out > 0) {
yuki0701 16:55c180a4338c 454 motor(out,0,out,0,out,0);
yuki0701 16:55c180a4338c 455 }
yuki0701 16:55c180a4338c 456 }
yuki0701 16:55c180a4338c 457
yuki0701 16:55c180a4338c 458 if(180 <= now_angle && now_angle< 360) {
yuki0701 16:55c180a4338c 459 target_angle = 360;
yuki0701 16:55c180a4338c 460 out = 0.01 * (target_angle - now_angle);
yuki0701 16:55c180a4338c 461
yuki0701 16:55c180a4338c 462 if(now_angle >= 359) {
yuki0701 16:55c180a4338c 463 motor(0,0,0,0,0,0);
yuki0701 16:55c180a4338c 464
yuki0701 16:55c180a4338c 465 } else if(out > 0.3) { //180～359°のときは反時計回りに回転
yuki0701 16:55c180a4338c 466 motor(0,0.3,0,0.3,0,0.3);
yuki0701 16:55c180a4338c 467
yuki0701 16:55c180a4338c 468 } else if(out <= 0.3 && out > 0) {
yuki0701 16:55c180a4338c 469 motor(0,out,0,out,0,out);
yuki0701 16:55c180a4338c 470 }
yuki0701 16:55c180a4338c 471 }
yuki0701 16:55c180a4338c 472 }
yuki0701 16:55c180a4338c 473 }
yuki0701 17:95cb86ce56a9 474
yuki0701 17:95cb86ce56a9 475
yuki0701 17:95cb86ce56a9 476
yuki0701 17:95cb86ce56a9 477 //printf("%f\t",receive_data);
yuki0701 17:95cb86ce56a9 478
yuki0701 17:95cb86ce56a9 479 printf("%f\n\r",now_angle);
yuki0701 16:55c180a4338c 480
yuki0701 16:55c180a4338c 481 cs=0;
yuki0701 17:95cb86ce56a9 482 receive_data = spi.write(data1)*0.01;
yuki0701 16:55c180a4338c 483
yuki0701 16:55c180a4338c 484 cs=1;
yuki0701 16:55c180a4338c 485 data1=0;
yuki0701 16:55c180a4338c 486
yuki0701 16:55c180a4338c 487
yuki0701 16:55c180a4338c 488
hirokimineshita 0:736c76a75def 489 }