robostep8th / Mbed 2 deprecated F3RC-mbed-new

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed move3wheel

User.cpp@21:0d063cc8b15f, 2018-11-01 (annotated)

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