2019NHK_teamA / Mbed 2 deprecated NHK2019robokon_11_19

NHK2019 manual program(回収機構ストール問題解決)

Dependencies:   mbed PS3_10_26

main.cpp@2:e462c8257384, 2019-09-05 (annotated)

Committer:
yosino_adati
Date:
Thu Sep 05 10:28:09 2019 +0000
Revision:
2:e462c8257384
Parent:
1:99294241f2ba
Child:
3:8c4e42cba9cb
syusei

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