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

さいしんばん

Dependencies:   mbed move3wheel

Fork of F3RC-mbed-new4 by 春ロボ1班(元F3RC4班+)

User.cpp@16:55c180a4338c, 2018-09-04 (annotated)

Committer:
yuki0701
Date:
Tue Sep 04 12:10:13 2018 +0000
Revision:
16:55c180a4338c
Parent:
15:c5258a4dee75
Child:
17:95cb86ce56a9
?

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