仮
Dependencies: mbed move3wheel
Fork of F3RC-mbed-new by
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?
User | Revision | Line number | New contents of line |
---|---|---|---|
hirokimineshita | 0:736c76a75def | 1 | #include "Utils.h" |
hirokimineshita | 0:736c76a75def | 2 | #include "USBHost.h" |
hirokimineshita | 0:736c76a75def | 3 | #include "hci.h" |
hirokimineshita | 0:736c76a75def | 4 | #include "ps3.h" |
hirokimineshita | 0:736c76a75def | 5 | #include "User.h" |
hirokimineshita | 0:736c76a75def | 6 | |
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 | } |