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

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

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

User.cpp@20:18da3cef9b39, 2018-09-14 (annotated)

Committer:
yuki0701
Date:
Fri Sep 14 07:04:12 2018 +0000
Revision:
20:18da3cef9b39
Parent:
19:509531c29e65 
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 20:18da3cef9b39 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 20:18da3cef9b39 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 17:95cb86ce56a9 153 motor(0,0,GetMotorOut(1),0,0,-GetMotorOut(2)); //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 17:95cb86ce56a9 178 motor(0,0,0,GetMotorOut(1),-GetMotorOut(2),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 17:95cb86ce56a9 330 }
yuki0701 17:95cb86ce56a9 331
yuki0701 16:55c180a4338c 332 }
yuki0701 17:95cb86ce56a9 333
yuki0701 17:95cb86ce56a9 334
yuki0701 16:55c180a4338c 335 ////////////////////////////ここからヌクレオのプログラム/////////////////////////////
yuki0701 16:55c180a4338c 336
yuki0701 16:55c180a4338c 337 if((ButtonState >> BUTTONTRIANGEL)&1 == 1) { //servo1
yuki0701 16:55c180a4338c 338 wait(0.1);
yuki0701 16:55c180a4338c 339 if((ButtonState >> BUTTONTRIANGEL)&1 == 1) {
yuki0701 16:55c180a4338c 340 data1=1;
yuki0701 20:18da3cef9b39 341 printf("5");
yuki0701 16:55c180a4338c 342 }
yuki0701 16:55c180a4338c 343 }
yuki0701 16:55c180a4338c 344
yuki0701 16:55c180a4338c 345
yuki0701 16:55c180a4338c 346 if((ButtonState >> BUTTONCIRCLE)&1 == 1) { //servo2
yuki0701 16:55c180a4338c 347 wait(0.1);
yuki0701 16:55c180a4338c 348 if((ButtonState >> BUTTONCIRCLE)&1 == 1) {
yuki0701 16:55c180a4338c 349 data1=2;
yuki0701 20:18da3cef9b39 350 printf("2");
yuki0701 16:55c180a4338c 351 }
yuki0701 16:55c180a4338c 352 }
yuki0701 16:55c180a4338c 353
yuki0701 16:55c180a4338c 354
yuki0701 16:55c180a4338c 355 if((ButtonState >> BUTTONCROSS)&1 == 1) { //servo3
yuki0701 16:55c180a4338c 356 wait(0.1);
yuki0701 16:55c180a4338c 357 if((ButtonState >> BUTTONCROSS)&1 == 1) {
yuki0701 16:55c180a4338c 358 data1=3;
yuki0701 20:18da3cef9b39 359 printf("3");
yuki0701 16:55c180a4338c 360 }
yuki0701 16:55c180a4338c 361 }
yuki0701 16:55c180a4338c 362
yuki0701 16:55c180a4338c 363
yuki0701 16:55c180a4338c 364 if((ButtonState >> BUTTONSQUARE)&1 == 1) { //servo4
yuki0701 16:55c180a4338c 365 wait(0.1);
yuki0701 16:55c180a4338c 366 if((ButtonState >> BUTTONSQUARE)&1 == 1) {
yuki0701 16:55c180a4338c 367 data1=4;
yuki0701 20:18da3cef9b39 368 printf("4");
yuki0701 16:55c180a4338c 369 }
yuki0701 16:55c180a4338c 370 }
yuki0701 16:55c180a4338c 371
yuki0701 16:55c180a4338c 372
yuki0701 16:55c180a4338c 373 if((ButtonState >> BUTTONLEFT)&1 == 1) { //エアシリンダー
yuki0701 16:55c180a4338c 374 wait(0.1);
yuki0701 16:55c180a4338c 375 if((ButtonState >> BUTTONLEFT)&1 == 1) {
yuki0701 16:55c180a4338c 376 data1=5;
yuki0701 20:18da3cef9b39 377 printf("5");
yuki0701 20:18da3cef9b39 378
yuki0701 16:55c180a4338c 379 }
yuki0701 16:55c180a4338c 380 }
yuki0701 16:55c180a4338c 381
yuki0701 16:55c180a4338c 382 ////////////////////////ここからジャイロの角度指定プログラム//////////////////////////
yuki0701 16:55c180a4338c 383
yuki0701 18:b24438ec4fd2 384 /*if(receive_data >= 0) { //反時計回りに0~359°となるよう修正
yuki0701 16:55c180a4338c 385 a = receive_data / 360;
yuki0701 16:55c180a4338c 386 now_angle = receive_data - (360 * a); //現在の角度
yuki0701 16:55c180a4338c 387 } else {
yuki0701 16:55c180a4338c 388 a = receive_data / 360;
yuki0701 17:95cb86ce56a9 389 now_angle = 360 + receive_data - (360 * a);
yuki0701 18:b24438ec4fd2 390 }*/
yuki0701 16:55c180a4338c 391
yuki0701 16:55c180a4338c 392
yuki0701 16:55c180a4338c 393 /*true_data = receive_data - 65536;
yuki0701 16:55c180a4338c 394 } else {
yuki0701 16:55c180a4338c 395 true_data = receive_data;
yuki0701 16:55c180a4338c 396 }*/
yuki0701 16:55c180a4338c 397
yuki0701 16:55c180a4338c 398
yuki0701 16:55c180a4338c 399 //now_angle = true_data; //現在の角度
yuki0701 16:55c180a4338c 400 //out = 0.01 * (target_angle - now_angle); //P(目標の角度 - 現在の角度) Pは自分で決めた定数
yuki0701 16:55c180a4338c 401
yuki0701 16:55c180a4338c 402 if((ButtonState >> BUTTONRIGHT)&1 == 1) {
yuki0701 16:55c180a4338c 403 wait(0.1);
yuki0701 16:55c180a4338c 404 if((ButtonState >> BUTTONRIGHT)&1 == 1) {
yuki0701 18:b24438ec4fd2 405 target_angle = 270;
yuki0701 16:55c180a4338c 406 out = 0.01 * (target_angle - now_angle);
yuki0701 16:55c180a4338c 407
yuki0701 16:55c180a4338c 408 //printf("%f\r\n",now_angle);
yuki0701 16:55c180a4338c 409 //printf("%f\r\n",out);
yuki0701 18:b24438ec4fd2 410 if(now_angle >= 269 && now_angle <= 271) {
yuki0701 17:95cb86ce56a9 411 motor(0,0,0,0,0,0);
yuki0701 17:95cb86ce56a9 412
yuki0701 16:55c180a4338c 413 } else if(out > 1.8 || out <= -0.3) { //0～89°と270～359°のときは時計回りに回転
yuki0701 16:55c180a4338c 414 motor(0.3,0,0.3,0,0.3,0);
yuki0701 16:55c180a4338c 415 //printf("cw 0.3\r\n");
yuki0701 16:55c180a4338c 416
yuki0701 16:55c180a4338c 417 } else if(out > -0.3 && out <= 0) {
yuki0701 16:55c180a4338c 418 motor(-out,0,-out,0,-out,0);
yuki0701 16:55c180a4338c 419 //printf("cw -out\r\n");
yuki0701 16:55c180a4338c 420
yuki0701 16:55c180a4338c 421 } else if(out > 0 && out <= 0.3) { //90～269°のときは反時計回りに回転
yuki0701 16:55c180a4338c 422 motor(0,out,0,out,0,out);
yuki0701 16:55c180a4338c 423 //printf("ccw out\r\n");
yuki0701 16:55c180a4338c 424
yuki0701 16:55c180a4338c 425 } else if(out <= 1.8 && out > 0.3) {
yuki0701 16:55c180a4338c 426 motor(0,0.3,0,0.3,0,0.3);
yuki0701 16:55c180a4338c 427 //printf("ccw 0.3\r\n");
yuki0701 16:55c180a4338c 428 }
yuki0701 16:55c180a4338c 429 }
yuki0701 16:55c180a4338c 430 }
yuki0701 16:55c180a4338c 431
yuki0701 16:55c180a4338c 432 if((ButtonState >> BUTTONDOWN)&1 == 1) {
yuki0701 16:55c180a4338c 433 wait(0.1);
yuki0701 16:55c180a4338c 434 if((ButtonState >> BUTTONDOWN)&1 == 1) {
yuki0701 18:b24438ec4fd2 435 target_angle = 180;
yuki0701 16:55c180a4338c 436 out = 0.01 * (target_angle - now_angle);
yuki0701 16:55c180a4338c 437
yuki0701 18:b24438ec4fd2 438 if(now_angle >= 179 && now_angle <= 181) {
yuki0701 16:55c180a4338c 439 motor(0,0,0,0,0,0);
yuki0701 17:95cb86ce56a9 440
yuki0701 16:55c180a4338c 441 } else if(out > 0.3) { //0～178°のときは反時計回りに回転
yuki0701 16:55c180a4338c 442 motor(0,0.3,0,0.3,0,0.3);
yuki0701 16:55c180a4338c 443
yuki0701 16:55c180a4338c 444 } else if(out <= 0.3 && out > 0) {
yuki0701 16:55c180a4338c 445 motor(0,out,0,out,0,out);
yuki0701 16:55c180a4338c 446
yuki0701 16:55c180a4338c 447 } else if(out <= 0 && out > -0.3) { //180～359°のときは時計回りに回転
yuki0701 16:55c180a4338c 448 motor(-out,0,-out,0,-out,0);
yuki0701 16:55c180a4338c 449
yuki0701 16:55c180a4338c 450 } else if(out <= -0.3) {
yuki0701 16:55c180a4338c 451 motor(0.3,0,0.3,0,0.3,0);
yuki0701 16:55c180a4338c 452 }
yuki0701 16:55c180a4338c 453 }
yuki0701 16:55c180a4338c 454 }
yuki0701 17:95cb86ce56a9 455
yuki0701 16:55c180a4338c 456 if((ButtonState >> BUTTONUP)&1 == 1) {
yuki0701 16:55c180a4338c 457 wait(0.1);
yuki0701 16:55c180a4338c 458 if((ButtonState >> BUTTONUP)&1 == 1) {
yuki0701 16:55c180a4338c 459 if(0 <= now_angle && now_angle < 180) {
yuki0701 16:55c180a4338c 460 target_angle = 0;
yuki0701 16:55c180a4338c 461 out = -0.01 * (target_angle - now_angle);
yuki0701 16:55c180a4338c 462
yuki0701 16:55c180a4338c 463 if(now_angle <= 1) {
yuki0701 16:55c180a4338c 464 motor(0,0,0,0,0,0);
yuki0701 16:55c180a4338c 465
yuki0701 16:55c180a4338c 466 } else if(out > 0.3) { //0～179°のときは時計回りに回転
yuki0701 16:55c180a4338c 467 motor(0.3,0,0.3,0,0.3,0);
yuki0701 16:55c180a4338c 468
yuki0701 16:55c180a4338c 469 } else if(out <= 0.3 && out > 0) {
yuki0701 16:55c180a4338c 470 motor(out,0,out,0,out,0);
yuki0701 16:55c180a4338c 471 }
yuki0701 16:55c180a4338c 472 }
yuki0701 16:55c180a4338c 473
yuki0701 16:55c180a4338c 474 if(180 <= now_angle && now_angle< 360) {
yuki0701 16:55c180a4338c 475 target_angle = 360;
yuki0701 16:55c180a4338c 476 out = 0.01 * (target_angle - now_angle);
yuki0701 16:55c180a4338c 477
yuki0701 16:55c180a4338c 478 if(now_angle >= 359) {
yuki0701 16:55c180a4338c 479 motor(0,0,0,0,0,0);
yuki0701 16:55c180a4338c 480
yuki0701 16:55c180a4338c 481 } else if(out > 0.3) { //180～359°のときは反時計回りに回転
yuki0701 16:55c180a4338c 482 motor(0,0.3,0,0.3,0,0.3);
yuki0701 16:55c180a4338c 483
yuki0701 16:55c180a4338c 484 } else if(out <= 0.3 && out > 0) {
yuki0701 16:55c180a4338c 485 motor(0,out,0,out,0,out);
yuki0701 16:55c180a4338c 486 }
yuki0701 16:55c180a4338c 487 }
yuki0701 16:55c180a4338c 488 }
yuki0701 16:55c180a4338c 489 }
yuki0701 17:95cb86ce56a9 490
yuki0701 17:95cb86ce56a9 491
yuki0701 17:95cb86ce56a9 492
yuki0701 17:95cb86ce56a9 493 //printf("%f\t",receive_data);
yuki0701 17:95cb86ce56a9 494
yuki0701 19:509531c29e65 495 //printf("%f\n\r",now_angle);
yuki0701 16:55c180a4338c 496
yuki0701 16:55c180a4338c 497 cs=0;
yuki0701 18:b24438ec4fd2 498 now_angle = spi.write(data1)*0.01;
yuki0701 16:55c180a4338c 499
yuki0701 16:55c180a4338c 500 cs=1;
yuki0701 16:55c180a4338c 501 data1=0;
yuki0701 16:55c180a4338c 502
yuki0701 16:55c180a4338c 503
yuki0701 16:55c180a4338c 504
hirokimineshita 0:736c76a75def 505 }