2019NHK_teamA / Mbed 2 deprecated NHK2019robokon_10_5

NHK2019ROBOCON team A manual robot program 10/4 version (ロボコン前日ピットにて)

Dependencies:   mbed 2019ROBOKONmanualProgram

main.cpp@1:99294241f2ba, 2019-09-05 (annotated)

Committer:
shina
Date:
Thu Sep 05 09:40:50 2019 +0000
Revision:
1:99294241f2ba
Parent:
0:c7e17c2fd542
Child:
2:830db42bae7e
NHK2019manualprogram

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shina 0:c7e17c2fd542 1 /////////////////////////////////////
shina 0:c7e17c2fd542 2 /*NHK2019*/
shina 1:99294241f2ba 3 //Aチーム手動機プログラム
shina 0:c7e17c2fd542 4 /*
shina 0:c7e17c2fd542 5 アドレス
shina 0:c7e17c2fd542 6 0x10:右前
shina 1:99294241f2ba 7 0x12:左後ろ
shina 0:c7e17c2fd542 8 0x14:右後ろ
shina 1:99294241f2ba 9 0x16:左前
shina 0:c7e17c2fd542 10 0x18:サーボ制御回路
shina 0:c7e17c2fd542 11 0x20:右ラック
shina 0:c7e17c2fd542 12 0x22:左ラック
shina 0:c7e17c2fd542 13 0x24:回収機構
shina 0:c7e17c2fd542 14 0x26:右ファン
shina 0:c7e17c2fd542 15 0x28:左ファン
shina 1:99294241f2ba 16 0x30:吐き出し機構
shina 0:c7e17c2fd542 17 */
shina 0:c7e17c2fd542 18 /////////////////////////////////////
shina 0:c7e17c2fd542 19
shina 0:c7e17c2fd542 20 //宣言
shina 0:c7e17c2fd542 21 #include "mbed.h"
shina 0:c7e17c2fd542 22 #include "PS3.h"
shina 0:c7e17c2fd542 23 I2C i2c(D14,D15);
shina 0:c7e17c2fd542 24 Serial pc(USBTX,USBRX);
shina 0:c7e17c2fd542 25 Serial slave(PC_6,PC_7);
shina 0:c7e17c2fd542 26 PS3 ps3(D8,D2);
shina 0:c7e17c2fd542 27 DigitalOut led(D5);//電源確認
shina 0:c7e17c2fd542 28 DigitalOut tsushin(D6);//通信確認
shina 0:c7e17c2fd542 29 DigitalOut data_check(D7);
shina 0:c7e17c2fd542 30 DigitalOut pwm(D4);
shina 0:c7e17c2fd542 31 DigitalOut stop(D10);
shina 0:c7e17c2fd542 32 DigitalOut led1(LED1);
shina 0:c7e17c2fd542 33
shina 1:99294241f2ba 34
shina 0:c7e17c2fd542 35 //変数
shina 0:c7e17c2fd542 36 char data1;//右上
shina 0:c7e17c2fd542 37 char data2;//左上
shina 0:c7e17c2fd542 38 char data3;//右下
shina 0:c7e17c2fd542 39 char data4;//左下
shina 0:c7e17c2fd542 40 char data_servo;
shina 0:c7e17c2fd542 41 char data5;//ラック直動右
shina 0:c7e17c2fd542 42 char data6;//ラック直動左
shina 0:c7e17c2fd542 43 char data7;//回収機構
shina 0:c7e17c2fd542 44 char data8;//右ファン
shina 0:c7e17c2fd542 45 char data9;//左ファン
shina 1:99294241f2ba 46 char data10;//パトランプ
shina 1:99294241f2ba 47 char data11;//吐き出し機構
shina 0:c7e17c2fd542 48 char get_data_rs232=0x80;
shina 0:c7e17c2fd542 49 int Ry;
shina 0:c7e17c2fd542 50 int Rx;
shina 1:99294241f2ba 51 int Ly;
shina 0:c7e17c2fd542 52 int right1;
shina 1:99294241f2ba 53 int right2;
shina 0:c7e17c2fd542 54 int left1;
shina 0:c7e17c2fd542 55 int left2;
shina 0:c7e17c2fd542 56 int select;
shina 0:c7e17c2fd542 57 int start;
shina 0:c7e17c2fd542 58 int circle;
shina 0:c7e17c2fd542 59 int cross;
shina 1:99294241f2ba 60 int triangle;
shina 1:99294241f2ba 61 int square;
shina 0:c7e17c2fd542 62 int ue1;
shina 0:c7e17c2fd542 63 int shita;
shina 0:c7e17c2fd542 64 int migi1;
shina 0:c7e17c2fd542 65 int hidari1;
shina 0:c7e17c2fd542 66 int tushin_check;
shina 0:c7e17c2fd542 67 int old_select=0;
shina 0:c7e17c2fd542 68 int i=1;
shina 0:c7e17c2fd542 69 int old_start=0;
shina 0:c7e17c2fd542 70 int j=1;
shina 0:c7e17c2fd542 71 int old_circle=0;
shina 0:c7e17c2fd542 72 int k=1;
shina 1:99294241f2ba 73 int old_square=0;
shina 1:99294241f2ba 74 int old_migi1=0;
shina 1:99294241f2ba 75 int old_hidari1=0;
shina 1:99294241f2ba 76 int n=1;
shina 1:99294241f2ba 77 int old_right2=0;
shina 1:99294241f2ba 78 int o=1;
shina 1:99294241f2ba 79 int old_right1=0;
shina 1:99294241f2ba 80 int old_left1=0;
shina 1:99294241f2ba 81 int p=2;
shina 1:99294241f2ba 82 int q=0;
shina 1:99294241f2ba 83
shina 1:99294241f2ba 84
shina 0:c7e17c2fd542 85
shina 0:c7e17c2fd542 86
shina 0:c7e17c2fd542 87 //関数プロトタイプ宣言
shina 1:99294241f2ba 88 void initialization();
shina 0:c7e17c2fd542 89 void get_data();
shina 0:c7e17c2fd542 90 void change_data();
shina 0:c7e17c2fd542 91 void change_pwm();
shina 0:c7e17c2fd542 92 void change_servo();
shina 0:c7e17c2fd542 93 void change_rack();
shina 0:c7e17c2fd542 94 void change_fan();
shina 0:c7e17c2fd542 95 void send_data(char address,char data);
shina 0:c7e17c2fd542 96 void emergency();
shina 1:99294241f2ba 97 void change_rack_2();
shina 1:99294241f2ba 98 void sequence_kaisyu();
shina 1:99294241f2ba 99 //void kaisyu();
shina 1:99294241f2ba 100 void sequence_hakidashi();
shina 1:99294241f2ba 101 //void hakidashi();
shina 1:99294241f2ba 102
shina 1:99294241f2ba 103
shina 0:c7e17c2fd542 104
shina 0:c7e17c2fd542 105 //メイン関数
shina 0:c7e17c2fd542 106 int main(){
shina 0:c7e17c2fd542 107 led=1;
shina 1:99294241f2ba 108 initialization();
shina 0:c7e17c2fd542 109 while(true){
shina 0:c7e17c2fd542 110 emergency();
shina 0:c7e17c2fd542 111 get_data();
shina 0:c7e17c2fd542 112 change_pwm();
shina 0:c7e17c2fd542 113 change_servo();
shina 0:c7e17c2fd542 114 change_data();
shina 0:c7e17c2fd542 115 change_rack();
shina 1:99294241f2ba 116 change_rack_2();
shina 1:99294241f2ba 117 sequence_kaisyu();
shina 1:99294241f2ba 118 //kaisyu();
shina 1:99294241f2ba 119 sequence_hakidashi();
shina 1:99294241f2ba 120 //hakidashi();
shina 0:c7e17c2fd542 121 change_fan();
shina 0:c7e17c2fd542 122 send_data(0x10,data1);
shina 0:c7e17c2fd542 123 send_data(0x12,data2);
shina 0:c7e17c2fd542 124 send_data(0x14,data3);
shina 0:c7e17c2fd542 125 send_data(0x16,data4);
shina 0:c7e17c2fd542 126 send_data(0x18,data_servo);
shina 0:c7e17c2fd542 127 send_data(0x20,data5);
shina 0:c7e17c2fd542 128 send_data(0x22,data6);
shina 1:99294241f2ba 129 //send_data(0x24,data7);
shina 0:c7e17c2fd542 130 send_data(0x26,data8);
shina 0:c7e17c2fd542 131 send_data(0x28,data9);
shina 1:99294241f2ba 132 send_data(0x40,data10);
shina 1:99294241f2ba 133 //send_data(0x30,data11);
shina 1:99294241f2ba 134
shina 1:99294241f2ba 135
shina 1:99294241f2ba 136
shina 1:99294241f2ba 137
shina 0:c7e17c2fd542 138 }
shina 0:c7e17c2fd542 139 }
shina 0:c7e17c2fd542 140
shina 1:99294241f2ba 141
shina 1:99294241f2ba 142 //初期化
shina 1:99294241f2ba 143 void initialization(){
shina 1:99294241f2ba 144 data1=0x80;
shina 1:99294241f2ba 145 data2=0x80;
shina 1:99294241f2ba 146 data3=0x80;
shina 1:99294241f2ba 147 data4=0x80;
shina 1:99294241f2ba 148 data5=0x80;
shina 1:99294241f2ba 149 data6=0x80;
shina 1:99294241f2ba 150 data7=0x10;
shina 1:99294241f2ba 151 data8=0x80;
shina 1:99294241f2ba 152 data9=0x80;
shina 1:99294241f2ba 153 data10=0x80;
shina 1:99294241f2ba 154 data11=0x80;
shina 1:99294241f2ba 155
shina 1:99294241f2ba 156 send_data(0x10,data1);
shina 1:99294241f2ba 157 send_data(0x12,data2);
shina 1:99294241f2ba 158 send_data(0x14,data3);
shina 1:99294241f2ba 159 send_data(0x16,data4);
shina 1:99294241f2ba 160 send_data(0x20,data5);
shina 1:99294241f2ba 161 send_data(0x22,data6);
shina 1:99294241f2ba 162 send_data(0x24,data7);
shina 1:99294241f2ba 163 send_data(0x26,data8);
shina 1:99294241f2ba 164 send_data(0x28,data9);
shina 1:99294241f2ba 165 send_data(0x40,data10);
shina 1:99294241f2ba 166 send_data(0x30,data11);
shina 1:99294241f2ba 167
shina 1:99294241f2ba 168
shina 1:99294241f2ba 169 }
shina 1:99294241f2ba 170
shina 1:99294241f2ba 171
shina 0:c7e17c2fd542 172 //データ読み込み
shina 0:c7e17c2fd542 173 void get_data(){
shina 0:c7e17c2fd542 174 Ry=ps3.getRightJoystickYaxis();
shina 0:c7e17c2fd542 175 Rx=ps3.getRightJoystickXaxis();
shina 1:99294241f2ba 176 Ly=ps3.getLeftJoystickYaxis();
shina 0:c7e17c2fd542 177 circle=ps3.getButtonState(maru);
shina 0:c7e17c2fd542 178 cross=ps3.getButtonState(batu);
shina 1:99294241f2ba 179 triangle=ps3.getButtonState(sankaku);
shina 1:99294241f2ba 180 square=ps3.getButtonState(sikaku);
shina 0:c7e17c2fd542 181 left1=ps3.getButtonState(L1);
shina 0:c7e17c2fd542 182 left2=ps3.getButtonState(L2);
shina 0:c7e17c2fd542 183 right1=ps3.getButtonState(R1);
shina 1:99294241f2ba 184 right2=ps3.getButtonState(R2);
shina 0:c7e17c2fd542 185 select=ps3.getSELECTState();
shina 0:c7e17c2fd542 186 start=ps3.getSTARTState();
shina 0:c7e17c2fd542 187 ue1=ps3.getButtonState(ue);
shina 0:c7e17c2fd542 188 shita=ps3.getButtonState(sita);
shina 0:c7e17c2fd542 189 hidari1=ps3.getButtonState(hidari);
shina 0:c7e17c2fd542 190 migi1=ps3.getButtonState(migi);
shina 1:99294241f2ba 191 get_data_rs232=slave.getc();
shina 1:99294241f2ba 192
shina 1:99294241f2ba 193
shina 1:99294241f2ba 194 if(get_data_rs232==0x20||get_data_rs232==0x22||get_data_rs232==0x24||get_data_rs232==0x32||get_data_rs232==0x34){
shina 1:99294241f2ba 195 led1=1;
shina 1:99294241f2ba 196 }else{
shina 1:99294241f2ba 197 led1=0;
shina 1:99294241f2ba 198 }
shina 1:99294241f2ba 199 pc.printf("%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n",Ry,Rx,Ly,left1,left2,right1,right2,select,start,ue1,shita,triangle,square,migi1,hidari1,tushin_check,i);
shina 1:99294241f2ba 200 if(Ry==0&&Rx==0&&Ly==0&&left1==0&&right1==0&&right2==0&&left2==0&&select==0&&start==0&&shita==0&&triangle==0&&ue1==0&&migi1==0&&hidari1==0&&square==0&&circle==0&&cross==0){
shina 0:c7e17c2fd542 201 data_check=0;
shina 0:c7e17c2fd542 202 }else{
shina 0:c7e17c2fd542 203 data_check=1;
shina 0:c7e17c2fd542 204 }
shina 0:c7e17c2fd542 205
shina 0:c7e17c2fd542 206 }
shina 0:c7e17c2fd542 207
shina 0:c7e17c2fd542 208 //緊急停止
shina 0:c7e17c2fd542 209 void emergency(){
shina 0:c7e17c2fd542 210 if(start!=old_start){
shina 0:c7e17c2fd542 211 old_start=start;
shina 0:c7e17c2fd542 212 if(start==1){
shina 0:c7e17c2fd542 213 if(j==1){
shina 0:c7e17c2fd542 214 //緊急停止
shina 0:c7e17c2fd542 215 led=0;
shina 0:c7e17c2fd542 216 stop=1;
shina 0:c7e17c2fd542 217 j=0;
shina 0:c7e17c2fd542 218 }else if(j==0){
shina 0:c7e17c2fd542 219 //緊急停止解除
shina 0:c7e17c2fd542 220 led=1;
shina 0:c7e17c2fd542 221 stop=0;
shina 0:c7e17c2fd542 222 j=1;
shina 0:c7e17c2fd542 223 }
shina 0:c7e17c2fd542 224 }
shina 0:c7e17c2fd542 225 }
shina 0:c7e17c2fd542 226 }
shina 0:c7e17c2fd542 227
shina 0:c7e17c2fd542 228
shina 0:c7e17c2fd542 229 //データ変化(メカナム)
shina 0:c7e17c2fd542 230 void change_data(){
shina 1:99294241f2ba 231
shina 1:99294241f2ba 232 if(Ry!=0||Rx!=0){
shina 1:99294241f2ba 233 square=0;
shina 1:99294241f2ba 234 }
shina 1:99294241f2ba 235
shina 0:c7e17c2fd542 236 if(Ry>30&&i==0){
shina 1:99294241f2ba 237 if(right1==1){
shina 1:99294241f2ba 238 data1=0xcf;
shina 1:99294241f2ba 239 data2=0x00;
shina 1:99294241f2ba 240 data3=0xcf;
shina 1:99294241f2ba 241 data4=0x00;
shina 1:99294241f2ba 242 }else if(left1==1){
shina 1:99294241f2ba 243 data1=0x00;
shina 1:99294241f2ba 244 data2=0xcf;
shina 1:99294241f2ba 245 data3=0x00;
shina 1:99294241f2ba 246 data4=0xcf;
shina 1:99294241f2ba 247 }else{
shina 1:99294241f2ba 248 data1=0x00;
shina 1:99294241f2ba 249 data2=0x00;
shina 1:99294241f2ba 250 data3=0x00;
shina 1:99294241f2ba 251 data4=0x00;
shina 1:99294241f2ba 252 }
shina 0:c7e17c2fd542 253 }else if(Ry<-30&&i==0){
shina 0:c7e17c2fd542 254 data1=0xff;
shina 0:c7e17c2fd542 255 data2=0xff;
shina 0:c7e17c2fd542 256 data3=0xff;
shina 0:c7e17c2fd542 257 data4=0xff;
shina 1:99294241f2ba 258 }else if(Rx>30&&i==0){
shina 1:99294241f2ba 259 if(right1==1){
shina 1:99294241f2ba 260 data1=0xff;
shina 1:99294241f2ba 261 data2=0xdf;
shina 1:99294241f2ba 262 data3=0xcf;
shina 1:99294241f2ba 263 data4=0x00;
shina 1:99294241f2ba 264 }else if(left1==1){
shina 1:99294241f2ba 265 data1=0xdf;
shina 1:99294241f2ba 266 data2=0xff;
shina 1:99294241f2ba 267 data3=0x00;
shina 1:99294241f2ba 268 data4=0xcf;
shina 1:99294241f2ba 269 }else{
shina 1:99294241f2ba 270 data1=0xff;
shina 1:99294241f2ba 271 data2=0xff;
shina 1:99294241f2ba 272 data3=0x00;
shina 1:99294241f2ba 273 data4=0x00;
shina 1:99294241f2ba 274 }
shina 1:99294241f2ba 275 }else if(Rx<-30&&i==0){
shina 1:99294241f2ba 276 if(right1==1){
shina 1:99294241f2ba 277 data1=0xcf;
shina 1:99294241f2ba 278 data2=0x00;
shina 1:99294241f2ba 279 data3=0xff;
shina 1:99294241f2ba 280 data4=0xdf;
shina 1:99294241f2ba 281 }else if(left1==1){
shina 1:99294241f2ba 282 data1=0x00;
shina 1:99294241f2ba 283 data2=0xcf;
shina 1:99294241f2ba 284 data3=0xdf;
shina 1:99294241f2ba 285 data4=0xff;
shina 1:99294241f2ba 286 }else{
shina 1:99294241f2ba 287 data1=0x00;
shina 1:99294241f2ba 288 data2=0x00;
shina 1:99294241f2ba 289 data3=0xff;
shina 1:99294241f2ba 290 data4=0xff;
shina 1:99294241f2ba 291 }
shina 1:99294241f2ba 292 }else if(right1==1&&i==0){
shina 1:99294241f2ba 293 data1=0xff;
shina 1:99294241f2ba 294 data2=0x00;
shina 1:99294241f2ba 295 data3=0xff;
shina 1:99294241f2ba 296 data4=0x00;
shina 1:99294241f2ba 297 }else if(left1==1&&i==0){
shina 1:99294241f2ba 298 data1=0x00;
shina 1:99294241f2ba 299 data2=0xff;
shina 1:99294241f2ba 300 data3=0x00;
shina 1:99294241f2ba 301 data4=0xff;
shina 0:c7e17c2fd542 302 }else if(Ry>30&&i==1){
shina 1:99294241f2ba 303 if(right1==1){
shina 1:99294241f2ba 304 data1=0x4f;
shina 1:99294241f2ba 305 data2=0x3f;
shina 1:99294241f2ba 306 data3=0x4f;
shina 1:99294241f2ba 307 data4=0x3f;
shina 1:99294241f2ba 308 }else if(left1==1){
shina 1:99294241f2ba 309 data1=0x3f;
shina 1:99294241f2ba 310 data2=0x4f;
shina 1:99294241f2ba 311 data3=0x3f;
shina 1:99294241f2ba 312 data4=0x4f;
shina 1:99294241f2ba 313 }else{
shina 1:99294241f2ba 314 data1=0x3f;
shina 1:99294241f2ba 315 data2=0x3f;
shina 1:99294241f2ba 316 data3=0x3f;
shina 1:99294241f2ba 317 data4=0x3f;
shina 1:99294241f2ba 318 }
shina 1:99294241f2ba 319 }else if(Ry<-30&&i==1){
shina 0:c7e17c2fd542 320 data1=0xbf;
shina 1:99294241f2ba 321 data2=0xbf;
shina 0:c7e17c2fd542 322 data3=0xbf;
shina 0:c7e17c2fd542 323 data4=0xbf;
shina 0:c7e17c2fd542 324 }else if(Rx>30&&i==1){
shina 1:99294241f2ba 325 if(right1==1){
shina 1:99294241f2ba 326 data1=0xbf;
shina 1:99294241f2ba 327 data2=0x5f;
shina 1:99294241f2ba 328 data3=0x4f;
shina 1:99294241f2ba 329 data4=0x3f;
shina 1:99294241f2ba 330 }else if(left1==1){
shina 1:99294241f2ba 331 data1=0x5f;
shina 1:99294241f2ba 332 data2=0xbf;
shina 1:99294241f2ba 333 data3=0x3f;
shina 1:99294241f2ba 334 data4=0x4f;
shina 1:99294241f2ba 335 }else{
shina 0:c7e17c2fd542 336 data1=0xbf;
shina 0:c7e17c2fd542 337 data2=0xbf;
shina 0:c7e17c2fd542 338 data3=0x3f;
shina 0:c7e17c2fd542 339 data4=0x3f;
shina 1:99294241f2ba 340 }
shina 0:c7e17c2fd542 341 }else if(Rx<-30&&i==1){
shina 1:99294241f2ba 342 if(right1==1){
shina 1:99294241f2ba 343 data1=0x4f;
shina 1:99294241f2ba 344 data2=0x3f;
shina 1:99294241f2ba 345 data3=0xbf;
shina 1:99294241f2ba 346 data4=0x5f;
shina 1:99294241f2ba 347 }else if(left1==1){
shina 1:99294241f2ba 348 data1=0x3f;
shina 1:99294241f2ba 349 data2=0x4f;
shina 1:99294241f2ba 350 data3=0x5f;
shina 1:99294241f2ba 351 data4=0xbf;
shina 1:99294241f2ba 352 }else{
shina 0:c7e17c2fd542 353 data1=0x3f;
shina 0:c7e17c2fd542 354 data2=0x3f;
shina 0:c7e17c2fd542 355 data3=0xbf;
shina 0:c7e17c2fd542 356 data4=0xbf;
shina 1:99294241f2ba 357 }
shina 0:c7e17c2fd542 358 }else if(right1==1&&i==1){
shina 1:99294241f2ba 359 data1=0xbf;
shina 0:c7e17c2fd542 360 data2=0x3f;
shina 1:99294241f2ba 361 data3=0xbf;
shina 0:c7e17c2fd542 362 data4=0x3f;
shina 0:c7e17c2fd542 363 }else if(left1==1&&i==1){
shina 1:99294241f2ba 364 data1=0x3f;
shina 0:c7e17c2fd542 365 data2=0xbf;
shina 1:99294241f2ba 366 data3=0x3f;
shina 0:c7e17c2fd542 367 data4=0xbf;
shina 0:c7e17c2fd542 368 }else{
shina 0:c7e17c2fd542 369 data1=0x80;
shina 0:c7e17c2fd542 370 data2=0x80;
shina 0:c7e17c2fd542 371 data3=0x80;
shina 0:c7e17c2fd542 372 data4=0x80;
shina 0:c7e17c2fd542 373 }
shina 0:c7e17c2fd542 374 }
shina 0:c7e17c2fd542 375
shina 1:99294241f2ba 376
shina 0:c7e17c2fd542 377 //pwm変化
shina 0:c7e17c2fd542 378 void change_pwm(){
shina 0:c7e17c2fd542 379 if(select!=old_select){
shina 0:c7e17c2fd542 380 old_select=select;
shina 0:c7e17c2fd542 381 if(select==1){
shina 0:c7e17c2fd542 382 if(i==1){
shina 0:c7e17c2fd542 383 pwm=0;
shina 0:c7e17c2fd542 384 i=0;
shina 0:c7e17c2fd542 385 }else if(i==0){
shina 0:c7e17c2fd542 386 pwm=1;
shina 0:c7e17c2fd542 387 i=1;
shina 0:c7e17c2fd542 388 }
shina 0:c7e17c2fd542 389 }
shina 0:c7e17c2fd542 390 }
shina 0:c7e17c2fd542 391 }
shina 0:c7e17c2fd542 392
shina 0:c7e17c2fd542 393 //サーボモーター
shina 0:c7e17c2fd542 394 void change_servo(){
shina 1:99294241f2ba 395 if(right2!=old_right2){
shina 1:99294241f2ba 396 old_right2=right2;
shina 1:99294241f2ba 397 //ハンガー機構
shina 1:99294241f2ba 398 if(right2==1){
shina 1:99294241f2ba 399 if(o==1){
shina 1:99294241f2ba 400 data_servo=0x01;
shina 1:99294241f2ba 401 //send_data(0x18,data_servo);
shina 1:99294241f2ba 402 o=2;
shina 1:99294241f2ba 403 }else if(o==2){
shina 1:99294241f2ba 404 data_servo=0x02;
shina 1:99294241f2ba 405 //send_data(0x18,data_servo);
shina 1:99294241f2ba 406 o=3;
shina 1:99294241f2ba 407 }
shina 1:99294241f2ba 408 //data_servo=0x01;
shina 1:99294241f2ba 409 }
shina 1:99294241f2ba 410 }else if(circle!=old_circle){
shina 1:99294241f2ba 411 old_circle=circle;
shina 1:99294241f2ba 412 //バスタオル挟む機構
shina 0:c7e17c2fd542 413 if(circle==1){
shina 0:c7e17c2fd542 414 if(k==1){
shina 1:99294241f2ba 415 data_servo=0x03;
shina 1:99294241f2ba 416 //send_data(0x18,data_servo);
shina 0:c7e17c2fd542 417 k=0;
shina 0:c7e17c2fd542 418 }else if(k==0){
shina 1:99294241f2ba 419 data_servo=0x04;
shina 1:99294241f2ba 420 //send_data(0x18,data_servo);
shina 1:99294241f2ba 421 k=1;
shina 0:c7e17c2fd542 422 }
shina 0:c7e17c2fd542 423 }
shina 1:99294241f2ba 424 }/*else if(hidari1!=old_hidari1){
shina 1:99294241f2ba 425 old_hidari1=hidari1;
shina 1:99294241f2ba 426
shina 1:99294241f2ba 427 if(hidari1==1&&select==0){
shina 1:99294241f2ba 428 if(n==1){
shina 1:99294241f2ba 429 data_servo=0x05;
shina 1:99294241f2ba 430 n=0;
shina 1:99294241f2ba 431 }else if(n==0){
shina 1:99294241f2ba 432 data_servo=0x06;
shina 1:99294241f2ba 433 n=1;
shina 0:c7e17c2fd542 434 }
shina 0:c7e17c2fd542 435 }
shina 1:99294241f2ba 436 }*/
shina 0:c7e17c2fd542 437 }
shina 0:c7e17c2fd542 438
shina 0:c7e17c2fd542 439 //ラック
shina 0:c7e17c2fd542 440 void change_rack(){
shina 1:99294241f2ba 441
shina 1:99294241f2ba 442
shina 1:99294241f2ba 443 //右
shina 1:99294241f2ba 444 if(triangle==1&&cross==0&&ue1==0&&shita==0&&(-32<Ly<32)){
shina 1:99294241f2ba 445 if(get_data_rs232!=0x20&&get_data_rs232!=0x22){
shina 1:99294241f2ba 446 data5=0x00;
shina 1:99294241f2ba 447 }else if(get_data_rs232==0x20||get_data_rs232==0x22){
shina 1:99294241f2ba 448 data5=0x10;
shina 1:99294241f2ba 449 }
shina 1:99294241f2ba 450 }else if(triangle==0&&cross==1&&ue1==0&&shita==0&&(-32<Ly<32)){
shina 1:99294241f2ba 451 if(get_data_rs232!=0x26&&get_data_rs232!=0x28){
shina 1:99294241f2ba 452 data5=0xff;
shina 1:99294241f2ba 453 }else if(get_data_rs232==0x26||get_data_rs232==0x28){
shina 1:99294241f2ba 454 data5=0x10;
shina 1:99294241f2ba 455 }
shina 1:99294241f2ba 456 //左
shina 1:99294241f2ba 457 }else if(triangle==0&&cross==0&&ue1==1&&shita==0&&(-32<Ly<32)){
shina 1:99294241f2ba 458 if(get_data_rs232!=0x20&&get_data_rs232!=0x24){
shina 1:99294241f2ba 459 data6=0xff;
shina 1:99294241f2ba 460 }else if(get_data_rs232==0x20||get_data_rs232==0x24){
shina 1:99294241f2ba 461 data6=0x10;
shina 1:99294241f2ba 462 }
shina 1:99294241f2ba 463 }else if(triangle==0&&cross==0&&ue1==0&&shita==1&&(-32<Ly<32)){
shina 1:99294241f2ba 464 if(get_data_rs232!=0x26&&get_data_rs232!=0x30){
shina 1:99294241f2ba 465 data6=0x00;
shina 1:99294241f2ba 466 }else if(get_data_rs232==0x26||get_data_rs232==0x30){
shina 1:99294241f2ba 467 data6=0x10;
shina 1:99294241f2ba 468 }
shina 1:99294241f2ba 469 }else if(triangle==0&&cross==0&&ue1==0&&shita==0&&(-32<Ly<32)){
shina 1:99294241f2ba 470 data5=0x80;
shina 1:99294241f2ba 471 data6=0x80;
shina 1:99294241f2ba 472
shina 1:99294241f2ba 473 }
shina 1:99294241f2ba 474
shina 1:99294241f2ba 475 }
shina 1:99294241f2ba 476
shina 1:99294241f2ba 477 //ラック左右
shina 1:99294241f2ba 478 void change_rack_2(){
shina 1:99294241f2ba 479 if(triangle==0&&cross==0&&ue1==0&&shita==0&&Ly>32){
shina 1:99294241f2ba 480 if(get_data_rs232==0x20){
shina 1:99294241f2ba 481 data5=0x10;
shina 1:99294241f2ba 482 data6=0x10;
shina 1:99294241f2ba 483 }else if(get_data_rs232==0x22){
shina 1:99294241f2ba 484 data5=0x10;
shina 0:c7e17c2fd542 485 data6=0xff;
shina 1:99294241f2ba 486 }else if(get_data_rs232==0x24){
shina 1:99294241f2ba 487 data5=0x00;
shina 1:99294241f2ba 488 data6=0x10;
shina 1:99294241f2ba 489 }else{
shina 1:99294241f2ba 490 data5=0x00;
shina 1:99294241f2ba 491 data6=0xff;
shina 1:99294241f2ba 492 }
shina 1:99294241f2ba 493 }else if(triangle==0&&cross==0&&ue1==0&&shita==0&&Ly<-32){
shina 1:99294241f2ba 494 if(get_data_rs232==0x26){
shina 1:99294241f2ba 495 data5=0x10;
shina 1:99294241f2ba 496 data6=0x10;
shina 1:99294241f2ba 497 }else if(get_data_rs232==0x28){
shina 1:99294241f2ba 498 data5=0x10;
shina 1:99294241f2ba 499 data6=0x00;
shina 1:99294241f2ba 500 }else if(get_data_rs232==0x30){
shina 1:99294241f2ba 501 data5=0xff;
shina 1:99294241f2ba 502 data6=0x10;
shina 1:99294241f2ba 503 }else{
shina 1:99294241f2ba 504 data5=0xff;
shina 1:99294241f2ba 505 data6=0x00;
shina 1:99294241f2ba 506 }
shina 1:99294241f2ba 507 }else if(triangle==0&&cross==0&&ue1==0&&shita==0&&(-32<Ly<32)){
shina 1:99294241f2ba 508 data5=0x80;
shina 1:99294241f2ba 509 data6=0x80;
shina 0:c7e17c2fd542 510 }
shina 1:99294241f2ba 511 }
shina 0:c7e17c2fd542 512
shina 0:c7e17c2fd542 513 //回収機構
shina 1:99294241f2ba 514
shina 1:99294241f2ba 515 void sequence_kaisyu(){
shina 1:99294241f2ba 516 if(square!=old_square){
shina 1:99294241f2ba 517 old_square=square;
shina 1:99294241f2ba 518 if(square==1){
shina 1:99294241f2ba 519
shina 1:99294241f2ba 520 data_servo=0x07;
shina 1:99294241f2ba 521 send_data(0x18,data_servo);
shina 1:99294241f2ba 522 wait(1.5);
shina 1:99294241f2ba 523 data7=0x00;
shina 1:99294241f2ba 524 while(get_data_rs232!=0x32){
shina 1:99294241f2ba 525 send_data(0x24,data7);
shina 1:99294241f2ba 526 get_data_rs232=slave.getc();
shina 1:99294241f2ba 527 }
shina 1:99294241f2ba 528 data7=0x10;
shina 1:99294241f2ba 529 send_data(0x24,data7);
shina 1:99294241f2ba 530 data_servo=0x08;
shina 1:99294241f2ba 531 send_data(0x18,data_servo);
shina 1:99294241f2ba 532 wait(2);
shina 1:99294241f2ba 533 data7=0xff;
shina 1:99294241f2ba 534 while(get_data_rs232!=0x34){
shina 1:99294241f2ba 535 send_data(0x24,data7);
shina 1:99294241f2ba 536 get_data_rs232=slave.getc();
shina 1:99294241f2ba 537 }
shina 1:99294241f2ba 538 data7=0x10;
shina 1:99294241f2ba 539 send_data(0x24,data7);
shina 1:99294241f2ba 540 data_servo=0x07;
shina 1:99294241f2ba 541 send_data(0x18,data_servo);
shina 1:99294241f2ba 542
shina 1:99294241f2ba 543 }
shina 1:99294241f2ba 544 }
shina 1:99294241f2ba 545 }
shina 1:99294241f2ba 546
shina 1:99294241f2ba 547
shina 1:99294241f2ba 548
shina 1:99294241f2ba 549 /*void kaisyu(){
shina 1:99294241f2ba 550 if(square!=old_square){
shina 1:99294241f2ba 551 if(square==1){
shina 1:99294241f2ba 552 p=1;
shina 1:99294241f2ba 553 data_servo=0x07;
shina 1:99294241f2ba 554 send_data(0x18,data_servo);
shina 1:99294241f2ba 555 wait(1.5);
shina 1:99294241f2ba 556 }
shina 1:99294241f2ba 557 }
shina 1:99294241f2ba 558
shina 1:99294241f2ba 559 //get_data_rs232=slave.getc();
shina 1:99294241f2ba 560
shina 1:99294241f2ba 561 if(p==1&&get_data_rs232_2!=0x32){
shina 1:99294241f2ba 562 data7=0x00;
shina 1:99294241f2ba 563 }else if(p==1&&get_data_rs232_2==0x32){
shina 1:99294241f2ba 564 data7=0x10;
shina 1:99294241f2ba 565 p=0;
shina 1:99294241f2ba 566 data_servo=0x08;
shina 1:99294241f2ba 567 send_data(0x18,data_servo);
shina 1:99294241f2ba 568 wait(1.5);
shina 1:99294241f2ba 569 }else if(p==0&&get_data_rs232!=0x34){
shina 1:99294241f2ba 570 data7=0xff;
shina 1:99294241f2ba 571 }else if(p==0&&get_data_rs232==0x34){
shina 1:99294241f2ba 572 data7=0x10;
shina 1:99294241f2ba 573 p=2;
shina 1:99294241f2ba 574 }
shina 1:99294241f2ba 575
shina 1:99294241f2ba 576 }
shina 1:99294241f2ba 577 */
shina 1:99294241f2ba 578
shina 1:99294241f2ba 579
shina 1:99294241f2ba 580 void sequence_hakidashi(){
shina 1:99294241f2ba 581 if(migi1!=old_migi1){
shina 1:99294241f2ba 582 old_migi1=migi1;
shina 1:99294241f2ba 583 if(migi1==1){
shina 1:99294241f2ba 584 /* data_servo=0x07;
shina 1:99294241f2ba 585 send_data(0x18,data_servo);
shina 1:99294241f2ba 586 wait(1.5);
shina 1:99294241f2ba 587 data11=0xff;
shina 1:99294241f2ba 588 while(get_data_rs232!=0x36){
shina 1:99294241f2ba 589 send_data(0x30,data11);
shina 1:99294241f2ba 590 get_data_rs232=slave.getc();
shina 1:99294241f2ba 591 }
shina 1:99294241f2ba 592 data11=0x10;
shina 1:99294241f2ba 593 send_data(0x30,data11);*/
shina 1:99294241f2ba 594 data11=0xff;
shina 1:99294241f2ba 595 send_data(0x30,data11);
shina 1:99294241f2ba 596 }
shina 1:99294241f2ba 597 }else if(hidari1!=old_hidari1){
shina 1:99294241f2ba 598 if(hidari1==1){
shina 1:99294241f2ba 599 data11=0x00;
shina 1:99294241f2ba 600 send_data(0x30,data11);
shina 1:99294241f2ba 601 }
shina 1:99294241f2ba 602 } /* else if(hidari1==1){
shina 1:99294241f2ba 603 if(get_data_rs232!=0x38){
shina 1:99294241f2ba 604 data11=0x38;
shina 1:99294241f2ba 605 send_data(0x30,data11);
shina 1:99294241f2ba 606 }else if(get_data_rs232==0x38){
shina 1:99294241f2ba 607 data11=0x10;
shina 1:99294241f2ba 608 send_data(0x30,data11;
shina 1:99294241f2ba 609 }
shina 1:99294241f2ba 610 }*/ /*else{
shina 1:99294241f2ba 611 data11=0x80;
shina 1:99294241f2ba 612 send_data(0x30,data11);
shina 1:99294241f2ba 613 }*/
shina 1:99294241f2ba 614
shina 1:99294241f2ba 615 if(get_data_rs232==0x38&&data11==0x00){
shina 1:99294241f2ba 616 data11=0x10;
shina 1:99294241f2ba 617 send_data(0x30,data11);
shina 1:99294241f2ba 618 } else if(get_data_rs232==0x36&&data11==0xff){
shina 1:99294241f2ba 619 data11=0x10;
shina 1:99294241f2ba 620 send_data(0x30,data11);
shina 1:99294241f2ba 621
shina 1:99294241f2ba 622 }
shina 1:99294241f2ba 623 }
shina 1:99294241f2ba 624
shina 1:99294241f2ba 625
shina 1:99294241f2ba 626 /*
shina 1:99294241f2ba 627 void hakidashi(){
shina 0:c7e17c2fd542 628 if(migi1==1&&hidari1==0){
shina 1:99294241f2ba 629 data_servo=0x07;
shina 1:99294241f2ba 630 if(get_data_rs232==0x36){
shina 1:99294241f2ba 631 data11=0x10;
shina 1:99294241f2ba 632 }else if(get_data_rs232!=0x38){
shina 1:99294241f2ba 633 data11=0xff;
shina 1:99294241f2ba 634 }
shina 1:99294241f2ba 635 }else if(migi1==0&&hidari1==1){
shina 1:99294241f2ba 636 data_servo=0x08;
shina 1:99294241f2ba 637 if(get_data_rs232==0x38){
shina 1:99294241f2ba 638 data11=0x10;
shina 1:99294241f2ba 639 }else if(get_data_rs232!=0x38){
shina 1:99294241f2ba 640 data11=0x00;
shina 1:99294241f2ba 641 }
shina 1:99294241f2ba 642 }else{
shina 1:99294241f2ba 643 data11=0x80;
shina 1:99294241f2ba 644 }
shina 1:99294241f2ba 645 }
shina 1:99294241f2ba 646
shina 1:99294241f2ba 647 */
shina 1:99294241f2ba 648
shina 0:c7e17c2fd542 649 //ファン
shina 0:c7e17c2fd542 650 void change_fan(){
shina 0:c7e17c2fd542 651 if(left2==1){
shina 0:c7e17c2fd542 652 data8=0xff;
shina 0:c7e17c2fd542 653 data9=0xff;
shina 1:99294241f2ba 654 data10=0xff;
shina 0:c7e17c2fd542 655 }else if(left2==0){
shina 0:c7e17c2fd542 656 data8=0x00;
shina 0:c7e17c2fd542 657 data9=0x00;
shina 1:99294241f2ba 658 data10=0x00;
shina 0:c7e17c2fd542 659 }
shina 0:c7e17c2fd542 660 }
shina 0:c7e17c2fd542 661
shina 1:99294241f2ba 662
shina 1:99294241f2ba 663
shina 0:c7e17c2fd542 664
shina 0:c7e17c2fd542 665 //i2c
shina 0:c7e17c2fd542 666 void send_data(char address,char data){
shina 0:c7e17c2fd542 667 tsushin=1;
shina 0:c7e17c2fd542 668 tushin_check=0;
shina 0:c7e17c2fd542 669 i2c.frequency(100000);
shina 0:c7e17c2fd542 670 i2c.start();
shina 0:c7e17c2fd542 671 i2c.write(address);
shina 0:c7e17c2fd542 672 tushin_check=i2c.write(data);
shina 0:c7e17c2fd542 673 i2c.stop();
shina 0:c7e17c2fd542 674 wait(0.003);
shina 1:99294241f2ba 675 }
shina 1:99294241f2ba 676