2019NHK_teamA / Mbed 2 deprecated NHK2019manualver2_10_3

A team manual robot program 10/3 もとどうり

Dependencies:   mbed PS3

main.cpp@2:283e5bb8a712, 2019-10-03 (annotated)

Committer:
shina
Date:
Thu Oct 03 11:58:43 2019 +0000
Revision:
2:283e5bb8a712
Parent:
0:f99e0ea93e31 
manual robot 10/3

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shina 0:f99e0ea93e31 1 /////////////////////////////////////
shina 0:f99e0ea93e31 2 /*NHK2019*/
shina 0:f99e0ea93e31 3 //Aチーム手動機プログラム
shina 0:f99e0ea93e31 4 /*
shina 0:f99e0ea93e31 5 アドレス
shina 0:f99e0ea93e31 6 0x10:右前
shina 2:283e5bb8a712 7 0x12:左後ろ
shina 0:f99e0ea93e31 8 0x14:右後ろ
shina 2:283e5bb8a712 9 0x16:左前
shina 0:f99e0ea93e31 10 0x18:サーボ制御回路
shina 0:f99e0ea93e31 11 0x20:右ラック
shina 2:283e5bb8a712 12 0x22:左ラック.
shina 0:f99e0ea93e31 13 0x24:回収機構
shina 0:f99e0ea93e31 14 0x26:右ファン
shina 0:f99e0ea93e31 15 0x28:左ファン
shina 0:f99e0ea93e31 16 0x30:吐き出し機構
shina 0:f99e0ea93e31 17 */
shina 0:f99e0ea93e31 18 /////////////////////////////////////
shina 0:f99e0ea93e31 19
shina 0:f99e0ea93e31 20 //宣言
shina 0:f99e0ea93e31 21 #include "mbed.h"
shina 0:f99e0ea93e31 22 #include "PS3.h"
shina 0:f99e0ea93e31 23 I2C i2c(D14,D15);
shina 0:f99e0ea93e31 24 Serial pc(USBTX,USBRX);
shina 0:f99e0ea93e31 25 Serial slave(PC_6,PC_7);
shina 0:f99e0ea93e31 26 PS3 ps3(D8,D2);
shina 0:f99e0ea93e31 27 DigitalOut led(D5);//電源確認
shina 0:f99e0ea93e31 28 DigitalOut tsushin(D6);//通信確認
shina 0:f99e0ea93e31 29 DigitalOut data_check(D7);
shina 0:f99e0ea93e31 30 DigitalOut pwm(D4);
shina 0:f99e0ea93e31 31 DigitalOut stop(D10);
shina 0:f99e0ea93e31 32 DigitalOut led1(LED1);
shina 2:283e5bb8a712 33 Timer timer;
shina 2:283e5bb8a712 34 Timer timer2;
shina 0:f99e0ea93e31 35
shina 0:f99e0ea93e31 36 //変数
shina 0:f99e0ea93e31 37 char data1;//右上
shina 0:f99e0ea93e31 38 char data2;//左上
shina 0:f99e0ea93e31 39 char data3;//右下
shina 0:f99e0ea93e31 40 char data4;//左下
shina 0:f99e0ea93e31 41 char data_servo;
shina 0:f99e0ea93e31 42 char data5;//ラック直動右
shina 0:f99e0ea93e31 43 char data6;//ラック直動左
shina 0:f99e0ea93e31 44 char data7;//回収機構
shina 0:f99e0ea93e31 45 char data8;//右ファン
shina 0:f99e0ea93e31 46 char data9;//左ファン
shina 0:f99e0ea93e31 47 char data10;//パトランプ
shina 0:f99e0ea93e31 48 char data11;//吐き出し機構
shina 2:283e5bb8a712 49 char get_data_rs232=0x00;
shina 0:f99e0ea93e31 50 int Ry;
shina 0:f99e0ea93e31 51 int Rx;
shina 0:f99e0ea93e31 52 int Ly;
shina 0:f99e0ea93e31 53 int right1;
shina 0:f99e0ea93e31 54 int right2;
shina 0:f99e0ea93e31 55 int left1;
shina 0:f99e0ea93e31 56 int left2;
shina 0:f99e0ea93e31 57 int select;
shina 0:f99e0ea93e31 58 int start;
shina 0:f99e0ea93e31 59 int circle;
shina 0:f99e0ea93e31 60 int cross;
shina 0:f99e0ea93e31 61 int triangle;
shina 0:f99e0ea93e31 62 int square;
shina 0:f99e0ea93e31 63 int ue1;
shina 0:f99e0ea93e31 64 int shita;
shina 0:f99e0ea93e31 65 int migi1;
shina 0:f99e0ea93e31 66 int hidari1;
shina 0:f99e0ea93e31 67 int tushin_check;
shina 0:f99e0ea93e31 68 int old_select=0;
shina 0:f99e0ea93e31 69 int i=1;
shina 0:f99e0ea93e31 70 int old_start=0;
shina 0:f99e0ea93e31 71 int j=1;
shina 0:f99e0ea93e31 72 int old_circle=0;
shina 0:f99e0ea93e31 73 int k=1;
shina 0:f99e0ea93e31 74 int old_square=0;
shina 0:f99e0ea93e31 75 int old_migi1=0;
shina 0:f99e0ea93e31 76 int old_hidari1=0;
shina 0:f99e0ea93e31 77 int n=1;
shina 0:f99e0ea93e31 78 int old_right2=0;
shina 0:f99e0ea93e31 79 int o=1;
shina 0:f99e0ea93e31 80 int old_right1=0;
shina 0:f99e0ea93e31 81 int old_left1=0;
shina 2:283e5bb8a712 82 int p=5;
shina 2:283e5bb8a712 83 int q=0;
shina 2:283e5bb8a712 84 int old_ue1=0;
shina 2:283e5bb8a712 85 int old_shita=0;
shina 2:283e5bb8a712 86 int old_left2=0;
shina 2:283e5bb8a712 87 int old_triangle=0;
shina 2:283e5bb8a712 88 int old_cross=0;
shina 2:283e5bb8a712 89 int r=6;
shina 2:283e5bb8a712 90 int s=5;
shina 0:f99e0ea93e31 91
shina 0:f99e0ea93e31 92
shina 0:f99e0ea93e31 93 //関数プロトタイプ宣言
shina 2:283e5bb8a712 94 void initialization();
shina 0:f99e0ea93e31 95 void get_data();
shina 0:f99e0ea93e31 96 void change_data();
shina 0:f99e0ea93e31 97 void change_pwm();
shina 0:f99e0ea93e31 98 void change_servo();
shina 2:283e5bb8a712 99 //void change_rack();
shina 0:f99e0ea93e31 100 void change_fan();
shina 0:f99e0ea93e31 101 void send_data(char address,char data);
shina 0:f99e0ea93e31 102 void emergency();
shina 0:f99e0ea93e31 103 void change_rack_2();
shina 0:f99e0ea93e31 104 void sequence_kaisyu();
shina 2:283e5bb8a712 105 //void kaisyu();
shina 2:283e5bb8a712 106 //void sequence_kaisyu_short();
shina 0:f99e0ea93e31 107 //void sequence_hakidashi();
shina 2:283e5bb8a712 108 void hakidashi();
shina 2:283e5bb8a712 109 void mode_change();
shina 2:283e5bb8a712 110
shina 2:283e5bb8a712 111
shina 0:f99e0ea93e31 112
shina 0:f99e0ea93e31 113 //メイン関数
shina 2:283e5bb8a712 114 int main()
shina 2:283e5bb8a712 115 {
shina 0:f99e0ea93e31 116 led=1;
shina 2:283e5bb8a712 117 pwm=1;
shina 2:283e5bb8a712 118 initialization();
shina 2:283e5bb8a712 119 while(true) {
shina 0:f99e0ea93e31 120 emergency();
shina 0:f99e0ea93e31 121 get_data();
shina 2:283e5bb8a712 122 mode_change();
shina 0:f99e0ea93e31 123 change_pwm();
shina 0:f99e0ea93e31 124 change_servo();
shina 0:f99e0ea93e31 125 change_data();
shina 2:283e5bb8a712 126 //change_rack();
shina 0:f99e0ea93e31 127 change_rack_2();
shina 0:f99e0ea93e31 128 sequence_kaisyu();
shina 2:283e5bb8a712 129 //kaisyu();
shina 2:283e5bb8a712 130 //sequence_kaisyu_short();
shina 2:283e5bb8a712 131 //sequence_hakidashi();
shina 2:283e5bb8a712 132 hakidashi();
shina 0:f99e0ea93e31 133 change_fan();
shina 0:f99e0ea93e31 134 send_data(0x10,data1);
shina 0:f99e0ea93e31 135 send_data(0x12,data2);
shina 0:f99e0ea93e31 136 send_data(0x14,data3);
shina 0:f99e0ea93e31 137 send_data(0x16,data4);
shina 0:f99e0ea93e31 138 send_data(0x18,data_servo);
shina 0:f99e0ea93e31 139 send_data(0x20,data5);
shina 0:f99e0ea93e31 140 send_data(0x22,data6);
shina 2:283e5bb8a712 141 //send_data(0x24,data7);
shina 0:f99e0ea93e31 142 send_data(0x26,data8);
shina 0:f99e0ea93e31 143 send_data(0x28,data9);
shina 0:f99e0ea93e31 144 send_data(0x40,data10);
shina 2:283e5bb8a712 145 //send_data(0x30,data11);
shina 2:283e5bb8a712 146
shina 2:283e5bb8a712 147
shina 2:283e5bb8a712 148
shina 2:283e5bb8a712 149
shina 2:283e5bb8a712 150 }
shina 2:283e5bb8a712 151 }
shina 2:283e5bb8a712 152
shina 2:283e5bb8a712 153
shina 2:283e5bb8a712 154 //初期化
shina 2:283e5bb8a712 155 void initialization()
shina 2:283e5bb8a712 156 {
shina 2:283e5bb8a712 157 data1=0x80;
shina 2:283e5bb8a712 158 data2=0x80;
shina 2:283e5bb8a712 159 data3=0x80;
shina 2:283e5bb8a712 160 data4=0x80;
shina 2:283e5bb8a712 161 data5=0x80;
shina 2:283e5bb8a712 162 data6=0x80;
shina 2:283e5bb8a712 163 data7=0x10;
shina 2:283e5bb8a712 164 data8=0x80;
shina 2:283e5bb8a712 165 data9=0x80;
shina 2:283e5bb8a712 166 data10=0x80;
shina 2:283e5bb8a712 167 data11=0x80;
shina 2:283e5bb8a712 168
shina 2:283e5bb8a712 169 send_data(0x10,data1);
shina 2:283e5bb8a712 170 send_data(0x12,data2);
shina 2:283e5bb8a712 171 send_data(0x14,data3);
shina 2:283e5bb8a712 172 send_data(0x16,data4);
shina 2:283e5bb8a712 173 send_data(0x20,data5);
shina 2:283e5bb8a712 174 send_data(0x22,data6);
shina 2:283e5bb8a712 175 send_data(0x24,data7);
shina 2:283e5bb8a712 176 send_data(0x26,data8);
shina 2:283e5bb8a712 177 send_data(0x28,data9);
shina 2:283e5bb8a712 178 send_data(0x40,data10);
shina 2:283e5bb8a712 179 send_data(0x30,data11);
shina 2:283e5bb8a712 180
shina 2:283e5bb8a712 181
shina 2:283e5bb8a712 182 data10=0x3f;
shina 2:283e5bb8a712 183 send_data(0x40,data10);
shina 2:283e5bb8a712 184
shina 2:283e5bb8a712 185 if(q==0){
shina 2:283e5bb8a712 186 data10=0xdf;
shina 2:283e5bb8a712 187 send_data(0x40,data10);
shina 2:283e5bb8a712 188 }else if(q==1){
shina 2:283e5bb8a712 189 data10=0xcf;
shina 2:283e5bb8a712 190 send_data(0x40,data10);
shina 0:f99e0ea93e31 191 }
shina 0:f99e0ea93e31 192
shina 2:283e5bb8a712 193 /*data_servo=0x03;
shina 2:283e5bb8a712 194 send_data(0x18,data_servo);
shina 2:283e5bb8a712 195 data_servo=0x04;
shina 2:283e5bb8a712 196 send_data(0x18,data_servo);*/
shina 2:283e5bb8a712 197 timer2.reset();
shina 2:283e5bb8a712 198
shina 2:283e5bb8a712 199 }
shina 2:283e5bb8a712 200
shina 2:283e5bb8a712 201
shina 0:f99e0ea93e31 202 //データ読み込み
shina 2:283e5bb8a712 203 void get_data()
shina 2:283e5bb8a712 204 {
shina 0:f99e0ea93e31 205 Ry=ps3.getRightJoystickYaxis();
shina 0:f99e0ea93e31 206 Rx=ps3.getRightJoystickXaxis();
shina 0:f99e0ea93e31 207 Ly=ps3.getLeftJoystickYaxis();
shina 0:f99e0ea93e31 208 circle=ps3.getButtonState(maru);
shina 0:f99e0ea93e31 209 cross=ps3.getButtonState(batu);
shina 0:f99e0ea93e31 210 triangle=ps3.getButtonState(sankaku);
shina 0:f99e0ea93e31 211 square=ps3.getButtonState(sikaku);
shina 0:f99e0ea93e31 212 left1=ps3.getButtonState(L1);
shina 0:f99e0ea93e31 213 left2=ps3.getButtonState(L2);
shina 0:f99e0ea93e31 214 right1=ps3.getButtonState(R1);
shina 0:f99e0ea93e31 215 right2=ps3.getButtonState(R2);
shina 0:f99e0ea93e31 216 select=ps3.getSELECTState();
shina 0:f99e0ea93e31 217 start=ps3.getSTARTState();
shina 0:f99e0ea93e31 218 ue1=ps3.getButtonState(ue);
shina 0:f99e0ea93e31 219 shita=ps3.getButtonState(sita);
shina 0:f99e0ea93e31 220 hidari1=ps3.getButtonState(hidari);
shina 0:f99e0ea93e31 221 migi1=ps3.getButtonState(migi);
shina 0:f99e0ea93e31 222 get_data_rs232=slave.getc();
shina 2:283e5bb8a712 223
shina 2:283e5bb8a712 224
shina 2:283e5bb8a712 225
shina 2:283e5bb8a712 226 if(get_data_rs232) {
shina 0:f99e0ea93e31 227 led1=1;
shina 2:283e5bb8a712 228 } else {
shina 2:283e5bb8a712 229 led1=0;
shina 2:283e5bb8a712 230 }
shina 2:283e5bb8a712 231 //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 2:283e5bb8a712 232 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:f99e0ea93e31 233 data_check=0;
shina 2:283e5bb8a712 234 } else {
shina 2:283e5bb8a712 235 data_check=1;
shina 0:f99e0ea93e31 236 }
shina 2:283e5bb8a712 237
shina 2:283e5bb8a712 238 }
shina 2:283e5bb8a712 239
shina 0:f99e0ea93e31 240 //緊急停止
shina 2:283e5bb8a712 241 void emergency()
shina 2:283e5bb8a712 242 {
shina 2:283e5bb8a712 243 if(start!=old_start) {
shina 2:283e5bb8a712 244 old_start=start;
shina 2:283e5bb8a712 245 if(start==1) {
shina 2:283e5bb8a712 246 if(j==1) {
shina 2:283e5bb8a712 247 //緊急停止
shina 2:283e5bb8a712 248 led=0;
shina 2:283e5bb8a712 249 stop=1;
shina 2:283e5bb8a712 250 j=0;
shina 2:283e5bb8a712 251 } else if(j==0) {
shina 2:283e5bb8a712 252 //緊急停止解除
shina 2:283e5bb8a712 253 led=1;
shina 2:283e5bb8a712 254 stop=0;
shina 2:283e5bb8a712 255 j=1;
shina 0:f99e0ea93e31 256 }
shina 0:f99e0ea93e31 257 }
shina 2:283e5bb8a712 258 }
shina 2:283e5bb8a712 259 }
shina 2:283e5bb8a712 260
shina 2:283e5bb8a712 261
shina 2:283e5bb8a712 262 //モードチェンジ
shina 2:283e5bb8a712 263 void mode_change(){
shina 2:283e5bb8a712 264 if(old_select!=select){
shina 2:283e5bb8a712 265 old_select=select;
shina 2:283e5bb8a712 266 if(select==1){
shina 2:283e5bb8a712 267 if(q==0){
shina 2:283e5bb8a712 268 //送風モード
shina 2:283e5bb8a712 269 q=1;
shina 2:283e5bb8a712 270 /* data_servo=0x03;
shina 2:283e5bb8a712 271 send_data(0x18,data_servo);
shina 2:283e5bb8a712 272 data_servo=0x04;
shina 2:283e5bb8a712 273 send_data(0x18,data_servo);*/
shina 2:283e5bb8a712 274 data10=0xcf;
shina 2:283e5bb8a712 275 send_data(0x40,data10);
shina 2:283e5bb8a712 276 }else if(q==1){
shina 2:283e5bb8a712 277 //回収モード
shina 2:283e5bb8a712 278 q=0;
shina 2:283e5bb8a712 279 data10=0xdf;
shina 2:283e5bb8a712 280 send_data(0x40,data10);
shina 2:283e5bb8a712 281 }
shina 2:283e5bb8a712 282 }
shina 2:283e5bb8a712 283 }
shina 0:f99e0ea93e31 284 }
shina 0:f99e0ea93e31 285
shina 0:f99e0ea93e31 286 //データ変化(メカナム)
shina 2:283e5bb8a712 287 void change_data()
shina 2:283e5bb8a712 288 {
shina 2:283e5bb8a712 289
shina 2:283e5bb8a712 290 if(Ry!=0||Rx!=0) {
shina 2:283e5bb8a712 291 square=0;
shina 2:283e5bb8a712 292 migi1=0;
shina 2:283e5bb8a712 293 }
shina 2:283e5bb8a712 294
shina 2:283e5bb8a712 295 if(Ry>30&&i==0) {
shina 2:283e5bb8a712 296 if(right1==1) {
shina 2:283e5bb8a712 297 data1=0xcf;
shina 2:283e5bb8a712 298 data2=0x00;
shina 2:283e5bb8a712 299 data3=0xcf;
shina 2:283e5bb8a712 300 data4=0x00;
shina 2:283e5bb8a712 301 } else if(left1==1) {
shina 2:283e5bb8a712 302 data1=0x00;
shina 2:283e5bb8a712 303 data2=0xcf;
shina 2:283e5bb8a712 304 data3=0x00;
shina 2:283e5bb8a712 305 data4=0xcf;
shina 2:283e5bb8a712 306 } else {
shina 2:283e5bb8a712 307 data1=0x00;
shina 2:283e5bb8a712 308 data2=0x00;
shina 2:283e5bb8a712 309 data3=0x00;
shina 2:283e5bb8a712 310 data4=0x00;
shina 2:283e5bb8a712 311 }
shina 2:283e5bb8a712 312 } else if(Ry<-30&&i==0) {
shina 0:f99e0ea93e31 313 if(right1==1){
shina 2:283e5bb8a712 314 data1=0xdf;
shina 2:283e5bb8a712 315 data2=0xff;
shina 2:283e5bb8a712 316 data3=0xdf;
shina 2:283e5bb8a712 317 data4=0xff;
shina 0:f99e0ea93e31 318 }else if(left1==1){
shina 2:283e5bb8a712 319 data1=0xff;
shina 2:283e5bb8a712 320 data2=0xdf;
shina 2:283e5bb8a712 321 data3=0xff;
shina 2:283e5bb8a712 322 data4=0xdf;
shina 0:f99e0ea93e31 323 }else{
shina 2:283e5bb8a712 324 data1=0xff;
shina 2:283e5bb8a712 325 data2=0xff;
shina 2:283e5bb8a712 326 data3=0xff;
shina 2:283e5bb8a712 327 data4=0xff;
shina 0:f99e0ea93e31 328 }
shina 2:283e5bb8a712 329 } else if(Rx>30&&i==0) {
shina 2:283e5bb8a712 330 if(right1==1) {
shina 2:283e5bb8a712 331 data1=0xff;
shina 2:283e5bb8a712 332 data2=0xdf;
shina 2:283e5bb8a712 333 data3=0xcf;
shina 2:283e5bb8a712 334 data4=0x00;
shina 2:283e5bb8a712 335 } else if(left1==1) {
shina 2:283e5bb8a712 336 data1=0xdf;
shina 2:283e5bb8a712 337 data2=0xff;
shina 2:283e5bb8a712 338 data3=0x00;
shina 2:283e5bb8a712 339 data4=0xcf;
shina 2:283e5bb8a712 340 } else {
shina 2:283e5bb8a712 341 data1=0xff;
shina 2:283e5bb8a712 342 data2=0xff;
shina 2:283e5bb8a712 343 data3=0x00;
shina 2:283e5bb8a712 344 data4=0x00;
shina 2:283e5bb8a712 345 }
shina 2:283e5bb8a712 346 } else if(Rx<-30&&i==0) {
shina 2:283e5bb8a712 347 if(right1==1) {
shina 2:283e5bb8a712 348 data1=0xcf;
shina 2:283e5bb8a712 349 data2=0x00;
shina 2:283e5bb8a712 350 data3=0xff;
shina 2:283e5bb8a712 351 data4=0xdf;
shina 2:283e5bb8a712 352 } else if(left1==1) {
shina 2:283e5bb8a712 353 data1=0x00;
shina 2:283e5bb8a712 354 data2=0xcf;
shina 2:283e5bb8a712 355 data3=0xdf;
shina 2:283e5bb8a712 356 data4=0xff;
shina 2:283e5bb8a712 357 } else {
shina 2:283e5bb8a712 358 data1=0x00;
shina 2:283e5bb8a712 359 data2=0x00;
shina 2:283e5bb8a712 360 data3=0xff;
shina 2:283e5bb8a712 361 data4=0xff;
shina 2:283e5bb8a712 362 }
shina 2:283e5bb8a712 363 } else if(right1==1&&i==0) {
shina 0:f99e0ea93e31 364 data1=0xff;
shina 0:f99e0ea93e31 365 data2=0x00;
shina 0:f99e0ea93e31 366 data3=0xff;
shina 0:f99e0ea93e31 367 data4=0x00;
shina 2:283e5bb8a712 368 } else if(left1==1&&i==0) {
shina 0:f99e0ea93e31 369 data1=0x00;
shina 0:f99e0ea93e31 370 data2=0xff;
shina 0:f99e0ea93e31 371 data3=0x00;
shina 0:f99e0ea93e31 372 data4=0xff;
shina 2:283e5bb8a712 373 } else if(Ry>30&&i==1) {
shina 2:283e5bb8a712 374 if(right1==1) {
shina 2:283e5bb8a712 375 data1=0x4f;
shina 2:283e5bb8a712 376 data2=0x3f;
shina 2:283e5bb8a712 377 data3=0x4f;
shina 2:283e5bb8a712 378 data4=0x3f;
shina 2:283e5bb8a712 379 } else if(left1==1) {
shina 2:283e5bb8a712 380 data1=0x3f;
shina 2:283e5bb8a712 381 data2=0x4f;
shina 2:283e5bb8a712 382 data3=0x3f;
shina 2:283e5bb8a712 383 data4=0x4f;
shina 2:283e5bb8a712 384 } else {
shina 2:283e5bb8a712 385 data1=0x3f;
shina 2:283e5bb8a712 386 data2=0x3f;
shina 2:283e5bb8a712 387 data3=0x3f;
shina 2:283e5bb8a712 388 data4=0x3f;
shina 2:283e5bb8a712 389 }
shina 2:283e5bb8a712 390 } else if(Ry<-30&&i==1) {
shina 2:283e5bb8a712 391 if(right1==1){
shina 2:283e5bb8a712 392 data1=0x5f;
shina 2:283e5bb8a712 393 data2=0xbf;
shina 2:283e5bb8a712 394 data3=0x5f;
shina 2:283e5bb8a712 395 data4=0xbf;
shina 2:283e5bb8a712 396 }else if(left1==1){
shina 2:283e5bb8a712 397 data1=0xbf;
shina 2:283e5bb8a712 398 data2=0x5f;
shina 2:283e5bb8a712 399 data3=0xbf;
shina 2:283e5bb8a712 400 data4=0x5f;
shina 2:283e5bb8a712 401 }else{
shina 2:283e5bb8a712 402 data1=0xbf;
shina 2:283e5bb8a712 403 data2=0xbf;
shina 2:283e5bb8a712 404 data3=0xbf;
shina 2:283e5bb8a712 405 data4=0xbf;
shina 0:f99e0ea93e31 406 }
shina 2:283e5bb8a712 407 } else if(Rx>30&&i==1) {
shina 2:283e5bb8a712 408 if(right1==1) {
shina 2:283e5bb8a712 409 data1=0xbf;
shina 2:283e5bb8a712 410 data2=0x5f;
shina 2:283e5bb8a712 411 data3=0x4f;
shina 2:283e5bb8a712 412 data4=0x3f;
shina 2:283e5bb8a712 413 } else if(left1==1) {
shina 2:283e5bb8a712 414 data1=0x5f;
shina 2:283e5bb8a712 415 data2=0xbf;
shina 2:283e5bb8a712 416 data3=0x3f;
shina 2:283e5bb8a712 417 data4=0x4f;
shina 2:283e5bb8a712 418 } else {
shina 2:283e5bb8a712 419 data1=0xbf;
shina 2:283e5bb8a712 420 data2=0xbf;
shina 2:283e5bb8a712 421 data3=0x3f;
shina 2:283e5bb8a712 422 data4=0x3f;
shina 2:283e5bb8a712 423 }
shina 2:283e5bb8a712 424 } else if(Rx<-30&&i==1) {
shina 2:283e5bb8a712 425 if(right1==1) {
shina 2:283e5bb8a712 426 data1=0x4f;
shina 2:283e5bb8a712 427 data2=0x3f;
shina 2:283e5bb8a712 428 data3=0xbf;
shina 2:283e5bb8a712 429 data4=0x5f;
shina 2:283e5bb8a712 430 } else if(left1==1) {
shina 2:283e5bb8a712 431 data1=0x3f;
shina 2:283e5bb8a712 432 data2=0x4f;
shina 2:283e5bb8a712 433 data3=0x5f;
shina 2:283e5bb8a712 434 data4=0xbf;
shina 2:283e5bb8a712 435 } else {
shina 2:283e5bb8a712 436 data1=0x3f;
shina 2:283e5bb8a712 437 data2=0x3f;
shina 2:283e5bb8a712 438 data3=0xbf;
shina 2:283e5bb8a712 439 data4=0xbf;
shina 2:283e5bb8a712 440 }
shina 2:283e5bb8a712 441 } else if(right1==1&&i==1) {
shina 0:f99e0ea93e31 442 data1=0xbf;
shina 0:f99e0ea93e31 443 data2=0x3f;
shina 0:f99e0ea93e31 444 data3=0xbf;
shina 0:f99e0ea93e31 445 data4=0x3f;
shina 2:283e5bb8a712 446 } else if(left1==1&&i==1) {
shina 0:f99e0ea93e31 447 data1=0x3f;
shina 0:f99e0ea93e31 448 data2=0xbf;
shina 0:f99e0ea93e31 449 data3=0x3f;
shina 0:f99e0ea93e31 450 data4=0xbf;
shina 2:283e5bb8a712 451 } else {
shina 0:f99e0ea93e31 452 data1=0x80;
shina 0:f99e0ea93e31 453 data2=0x80;
shina 0:f99e0ea93e31 454 data3=0x80;
shina 0:f99e0ea93e31 455 data4=0x80;
shina 0:f99e0ea93e31 456 }
shina 2:283e5bb8a712 457 }
shina 0:f99e0ea93e31 458
shina 0:f99e0ea93e31 459
shina 2:283e5bb8a712 460 //pwm変化
shina 2:283e5bb8a712 461 void change_pwm()
shina 2:283e5bb8a712 462 {
shina 2:283e5bb8a712 463 if(cross!=old_cross) {
shina 2:283e5bb8a712 464 old_cross=cross;
shina 2:283e5bb8a712 465 if(cross==1) {
shina 2:283e5bb8a712 466 if(i==1) {
shina 2:283e5bb8a712 467 pwm=0;
shina 2:283e5bb8a712 468 data10=0xbf;
shina 2:283e5bb8a712 469 send_data(0x40,data10);
shina 2:283e5bb8a712 470 i=0;
shina 2:283e5bb8a712 471 } else if(i==0) {
shina 2:283e5bb8a712 472 pwm=1;
shina 2:283e5bb8a712 473 data10=0x3f;
shina 2:283e5bb8a712 474 send_data(0x40,data10);
shina 2:283e5bb8a712 475 i=1;
shina 0:f99e0ea93e31 476 }
shina 0:f99e0ea93e31 477 }
shina 0:f99e0ea93e31 478 }
shina 0:f99e0ea93e31 479 }
shina 0:f99e0ea93e31 480
shina 0:f99e0ea93e31 481 //サーボモーター
shina 2:283e5bb8a712 482 void change_servo()
shina 2:283e5bb8a712 483 {
shina 2:283e5bb8a712 484 if(right2!=old_right2) {
shina 2:283e5bb8a712 485 old_right2=right2;
shina 2:283e5bb8a712 486 //ハンガー機構&バスタオル機構initialize処理
shina 2:283e5bb8a712 487 if(right2==1) {
shina 2:283e5bb8a712 488 if(o==1) {
shina 2:283e5bb8a712 489
shina 2:283e5bb8a712 490 // timer.start();
shina 2:283e5bb8a712 491 // data_servo=0x03;
shina 2:283e5bb8a712 492 // send_data(0x18,data_servo);
shina 2:283e5bb8a712 493 data_servo=0x01;
shina 2:283e5bb8a712 494 //send_data(0x18,data_servo);
shina 2:283e5bb8a712 495 o=2;
shina 2:283e5bb8a712 496 } else if(o==2) {
shina 2:283e5bb8a712 497 data_servo=0x02;
shina 2:283e5bb8a712 498 //send_data(0x18,data_servo);
shina 2:283e5bb8a712 499 o=3;
shina 2:283e5bb8a712 500 }
shina 2:283e5bb8a712 501 //data_servo=0x01;
shina 2:283e5bb8a712 502 }
shina 2:283e5bb8a712 503 } else if(circle!=old_circle) {
shina 2:283e5bb8a712 504 old_circle=circle;
shina 2:283e5bb8a712 505 //バスタオル挟む機構
shina 2:283e5bb8a712 506 if(circle==1) {
shina 2:283e5bb8a712 507 if(k==1) {
shina 2:283e5bb8a712 508 data_servo=0x03;
shina 2:283e5bb8a712 509 //send_data(0x18,data_servo);
shina 2:283e5bb8a712 510 k=0;
shina 2:283e5bb8a712 511 } else if(k==0) {
shina 2:283e5bb8a712 512 data_servo=0x04;
shina 2:283e5bb8a712 513 //send_data(0x18,data_servo);
shina 2:283e5bb8a712 514 k=1;
shina 2:283e5bb8a712 515 }
shina 2:283e5bb8a712 516 }
shina 2:283e5bb8a712 517 }/*else if(hidari1!=old_hidari1){
shina 0:f99e0ea93e31 518 old_hidari1=hidari1;
shina 2:283e5bb8a712 519
shina 0:f99e0ea93e31 520 if(hidari1==1&&select==0){
shina 0:f99e0ea93e31 521 if(n==1){
shina 0:f99e0ea93e31 522 data_servo=0x05;
shina 0:f99e0ea93e31 523 n=0;
shina 0:f99e0ea93e31 524 }else if(n==0){
shina 0:f99e0ea93e31 525 data_servo=0x06;
shina 0:f99e0ea93e31 526 n=1;
shina 0:f99e0ea93e31 527 }
shina 0:f99e0ea93e31 528 }
shina 0:f99e0ea93e31 529 }*/
shina 2:283e5bb8a712 530
shina 2:283e5bb8a712 531 /* if(timer.read()>2&&o==2){
shina 2:283e5bb8a712 532 data_servo=0x04;
shina 2:283e5bb8a712 533 send_data(0x18,data_servo);
shina 2:283e5bb8a712 534 timer.stop();
shina 2:283e5bb8a712 535 timer.reset();
shina 2:283e5bb8a712 536 }*/
shina 2:283e5bb8a712 537 }
shina 2:283e5bb8a712 538
shina 0:f99e0ea93e31 539 //ラック
shina 2:283e5bb8a712 540 /*
shina 2:283e5bb8a712 541 void change_rack()
shina 2:283e5bb8a712 542 {
shina 2:283e5bb8a712 543
shina 2:283e5bb8a712 544
shina 2:283e5bb8a712 545 //右
shina 2:283e5bb8a712 546 if(triangle==1&&cross==0&&ue1==0&&shita==0&&(-32<Ly<32)) {
shina 2:283e5bb8a712 547 if(!(get_data_rs232 & 0x01)) {
shina 2:283e5bb8a712 548 data5=0x00;
shina 2:283e5bb8a712 549 } else if(get_data_rs232 & 0x01) {
shina 2:283e5bb8a712 550 data5=0x10;
shina 2:283e5bb8a712 551 }
shina 2:283e5bb8a712 552 } else if(triangle==0&&cross==1&&ue1==0&&shita==0&&(-32<Ly<32)) {
shina 2:283e5bb8a712 553 if(!(get_data_rs232 & 0x04)) {
shina 2:283e5bb8a712 554 data5=0xff;
shina 2:283e5bb8a712 555 } else if(get_data_rs232 & 0x04) {
shina 2:283e5bb8a712 556 data5=0x10;
shina 2:283e5bb8a712 557 }
shina 0:f99e0ea93e31 558 //左
shina 2:283e5bb8a712 559 } else if(triangle==0&&cross==0&&ue1==1&&shita==0&&(-32<Ly<32)) {
shina 2:283e5bb8a712 560 if(!(get_data_rs232 & 0x02)) {
shina 2:283e5bb8a712 561 data6=0xff;
shina 2:283e5bb8a712 562 } else if(get_data_rs232 & 0x02) {
shina 2:283e5bb8a712 563 data6=0x10;
shina 0:f99e0ea93e31 564 }
shina 2:283e5bb8a712 565 } else if(triangle==0&&cross==0&&ue1==0&&shita==1&&(-32<Ly<32)) {
shina 2:283e5bb8a712 566 if(!(get_data_rs232 & 0x08)) {
shina 2:283e5bb8a712 567 data6=0x00;
shina 2:283e5bb8a712 568 } else if(get_data_rs232 & 0x08) {
shina 2:283e5bb8a712 569 data6=0x10;
shina 2:283e5bb8a712 570 }
shina 2:283e5bb8a712 571 } else if(triangle==0&&cross==0&&ue1==0&&shita==0&&(-32<Ly<32)) {
shina 2:283e5bb8a712 572 data5=0x80;
shina 2:283e5bb8a712 573 data6=0x80;
shina 2:283e5bb8a712 574
shina 2:283e5bb8a712 575 }
shina 0:f99e0ea93e31 576
shina 0:f99e0ea93e31 577 }
shina 2:283e5bb8a712 578 */
shina 0:f99e0ea93e31 579
shina 0:f99e0ea93e31 580 //ラック左右
shina 2:283e5bb8a712 581 void change_rack_2()
shina 2:283e5bb8a712 582 {
shina 2:283e5bb8a712 583 /*
shina 2:283e5bb8a712 584 if(triangle==0&&cross==0&&ue1==0&&shita==0&&Ly>32) {
shina 2:283e5bb8a712 585 if(get_data_rs232 & 0x01) {
shina 0:f99e0ea93e31 586 data5=0x10;
shina 2:283e5bb8a712 587 } else {
shina 2:283e5bb8a712 588 data5=0x00;
shina 2:283e5bb8a712 589 }
shina 2:283e5bb8a712 590 if(get_data_rs232 & 0x02) {
shina 0:f99e0ea93e31 591 data6=0x10;
shina 2:283e5bb8a712 592 } else {
shina 0:f99e0ea93e31 593 data6=0xff;
shina 2:283e5bb8a712 594 }
shina 2:283e5bb8a712 595 } else if(triangle==0&&cross==0&&ue1==0&&shita==0&&Ly<-32) {
shina 2:283e5bb8a712 596 if(get_data_rs232 & 0x04) {
shina 2:283e5bb8a712 597 data5=0x10;
shina 2:283e5bb8a712 598 } else {
shina 2:283e5bb8a712 599 data5=0xff;
shina 2:283e5bb8a712 600 }
shina 2:283e5bb8a712 601 if(get_data_rs232 & 0x08) {
shina 0:f99e0ea93e31 602 data6=0x10;
shina 2:283e5bb8a712 603 } else {
shina 2:283e5bb8a712 604 data6=0x00;
shina 2:283e5bb8a712 605 }
shina 2:283e5bb8a712 606 } else if(triangle==0&&cross==0&&ue1==0&&shita==0&&(-32<Ly<32)) {
shina 2:283e5bb8a712 607 data5=0x80;
shina 2:283e5bb8a712 608 data6=0x80;
shina 2:283e5bb8a712 609 }
shina 2:283e5bb8a712 610 */
shina 2:283e5bb8a712 611
shina 2:283e5bb8a712 612 if(ue1!=old_ue1){
shina 2:283e5bb8a712 613 old_ue1=ue1;
shina 2:283e5bb8a712 614 if(ue1==1&&start==0){
shina 0:f99e0ea93e31 615 data5=0x00;
shina 0:f99e0ea93e31 616 data6=0xff;
shina 2:283e5bb8a712 617
shina 2:283e5bb8a712 618 /*data_servo=0x03;
shina 2:283e5bb8a712 619 send_data(0x18,data_servo);
shina 2:283e5bb8a712 620 data_servo=0x04;
shina 2:283e5bb8a712 621 send_data(0x18,data_servo);*/
shina 0:f99e0ea93e31 622 }
shina 2:283e5bb8a712 623 }else if(shita!=old_shita){
shina 2:283e5bb8a712 624 old_shita=shita;
shina 2:283e5bb8a712 625 if(shita==1&&start==0){
shina 0:f99e0ea93e31 626 data5=0xff;
shina 0:f99e0ea93e31 627 data6=0x00;
shina 0:f99e0ea93e31 628 }
shina 2:283e5bb8a712 629 }
shina 2:283e5bb8a712 630
shina 2:283e5bb8a712 631 if((get_data_rs232 & 0x01)&&data5==0x00){
shina 2:283e5bb8a712 632 data5=0x10;
shina 2:283e5bb8a712 633 send_data(0x20,data5);
shina 2:283e5bb8a712 634 }else if((get_data_rs232 & 0x04)&&data5==0xff){
shina 2:283e5bb8a712 635 data5=0x10;
shina 2:283e5bb8a712 636 send_data(0x20,data5);
shina 0:f99e0ea93e31 637 }
shina 2:283e5bb8a712 638
shina 2:283e5bb8a712 639 if((get_data_rs232 & 0x02)&&data6==0xff){
shina 2:283e5bb8a712 640 data6=0x10;
shina 2:283e5bb8a712 641 send_data(0x22,data6);
shina 2:283e5bb8a712 642 }else if((get_data_rs232 & 0x08)&&data6==0x00){
shina 2:283e5bb8a712 643 data6=0x10;
shina 2:283e5bb8a712 644 send_data(0x22,data6);
shina 2:283e5bb8a712 645 }
shina 2:283e5bb8a712 646
shina 2:283e5bb8a712 647
shina 0:f99e0ea93e31 648 }
shina 0:f99e0ea93e31 649
shina 0:f99e0ea93e31 650 //回収機構
shina 2:283e5bb8a712 651
shina 2:283e5bb8a712 652 void sequence_kaisyu()
shina 2:283e5bb8a712 653 {
shina 2:283e5bb8a712 654 if(q==0){
shina 2:283e5bb8a712 655 if(square!=old_square) {
shina 2:283e5bb8a712 656 old_square=square;
shina 2:283e5bb8a712 657 if(square==1&&select==0) {
shina 2:283e5bb8a712 658
shina 2:283e5bb8a712 659 data_servo=0x07;
shina 2:283e5bb8a712 660 send_data(0x18,data_servo);
shina 2:283e5bb8a712 661 wait(1.3);
shina 2:283e5bb8a712 662 data7=0x00;
shina 2:283e5bb8a712 663 get_data_rs232=get_data_rs232 & 0b11101111;
shina 2:283e5bb8a712 664 while(!(get_data_rs232 & 0x10)) {
shina 2:283e5bb8a712 665 send_data(0x24,data7);
shina 2:283e5bb8a712 666 get_data_rs232=slave.getc();
shina 2:283e5bb8a712 667 }
shina 2:283e5bb8a712 668 data7=0x10;
shina 2:283e5bb8a712 669 send_data(0x24,data7);
shina 2:283e5bb8a712 670 data_servo=0x08;
shina 2:283e5bb8a712 671 send_data(0x18,data_servo);
shina 2:283e5bb8a712 672 wait(1.3);
shina 2:283e5bb8a712 673 data7=0xff;
shina 2:283e5bb8a712 674 get_data_rs232=get_data_rs232 & 0b11011111;
shina 2:283e5bb8a712 675 while(!(get_data_rs232 & 0x20)) {
shina 2:283e5bb8a712 676 send_data(0x24,data7);
shina 2:283e5bb8a712 677 get_data_rs232=slave.getc();
shina 2:283e5bb8a712 678 }
shina 2:283e5bb8a712 679 data7=0x10;
shina 2:283e5bb8a712 680 send_data(0x24,data7);
shina 2:283e5bb8a712 681
shina 2:283e5bb8a712 682 }
shina 2:283e5bb8a712 683 }else if(triangle!=old_triangle){
shina 2:283e5bb8a712 684 if(triangle==1&&select==0){
shina 2:283e5bb8a712 685
shina 2:283e5bb8a712 686 data_servo=0x07;
shina 2:283e5bb8a712 687 send_data(0x18,data_servo);
shina 2:283e5bb8a712 688 wait(1.3);
shina 2:283e5bb8a712 689 data7=0x00;
shina 2:283e5bb8a712 690 send_data(0x24,data7);
shina 2:283e5bb8a712 691 wait(0.3);
shina 2:283e5bb8a712 692 data7=0x10;
shina 2:283e5bb8a712 693 send_data(0x24,data7);
shina 2:283e5bb8a712 694 data_servo=0x08;
shina 2:283e5bb8a712 695 send_data(0x18,data_servo);
shina 2:283e5bb8a712 696 wait(1.3);
shina 2:283e5bb8a712 697 data7=0xff;
shina 2:283e5bb8a712 698 get_data_rs232=get_data_rs232 & 0b11011111;
shina 2:283e5bb8a712 699 while(!(get_data_rs232 & 0x20)) {
shina 2:283e5bb8a712 700 send_data(0x24,data7);
shina 2:283e5bb8a712 701 get_data_rs232=slave.getc();
shina 2:283e5bb8a712 702 }
shina 2:283e5bb8a712 703 data7=0x10;
shina 2:283e5bb8a712 704 send_data(0x24,data7);
shina 2:283e5bb8a712 705 }
shina 2:283e5bb8a712 706 }
shina 2:283e5bb8a712 707 }
shina 2:283e5bb8a712 708 }
shina 2:283e5bb8a712 709
shina 2:283e5bb8a712 710
shina 2:283e5bb8a712 711 /*
shina 2:283e5bb8a712 712 void kaisyu(){
shina 2:283e5bb8a712 713 if(q==0){
shina 2:283e5bb8a712 714 if(old_square!=square){
shina 2:283e5bb8a712 715 old_square=square;
shina 2:283e5bb8a712 716 if(square==1){
shina 2:283e5bb8a712 717 p=1;
shina 0:f99e0ea93e31 718
shina 2:283e5bb8a712 719 }
shina 2:283e5bb8a712 720 }
shina 2:283e5bb8a712 721 switch(p){
shina 2:283e5bb8a712 722
shina 2:283e5bb8a712 723 case 1:
shina 2:283e5bb8a712 724 data_servo=0x07;
shina 2:283e5bb8a712 725 send_data(0x24,data_servo);
shina 2:283e5bb8a712 726 timer2.start();
shina 2:283e5bb8a712 727 if(timer2.read()>3.5){
shina 2:283e5bb8a712 728 timer2.stop();
shina 2:283e5bb8a712 729 timer2.reset();
shina 2:283e5bb8a712 730 p=2;
shina 2:283e5bb8a712 731 }
shina 2:283e5bb8a712 732 break;
shina 2:283e5bb8a712 733
shina 2:283e5bb8a712 734 case 2:
shina 2:283e5bb8a712 735 data7=0x00;
shina 2:283e5bb8a712 736 send_data(0x24,data7);
shina 2:283e5bb8a712 737 if(get_data_rs232 & 0x10){
shina 2:283e5bb8a712 738 data7=0x10;
shina 2:283e5bb8a712 739 send_data(0x24,data7);
shina 2:283e5bb8a712 740 p=3;
shina 2:283e5bb8a712 741 }
shina 2:283e5bb8a712 742 break;
shina 2:283e5bb8a712 743
shina 2:283e5bb8a712 744 case 3:
shina 2:283e5bb8a712 745 data_servo=0x08;
shina 2:283e5bb8a712 746 send_data(0x18,data_servo);
shina 2:283e5bb8a712 747 timer2.start();
shina 2:283e5bb8a712 748 if(timer2.read()>1.3){
shina 2:283e5bb8a712 749 p=4;
shina 2:283e5bb8a712 750 }
shina 2:283e5bb8a712 751
shina 2:283e5bb8a712 752 break;
shina 2:283e5bb8a712 753
shina 2:283e5bb8a712 754 case 4:
shina 2:283e5bb8a712 755 data7=0xff;
shina 2:283e5bb8a712 756 send_data(0x24,data7);
shina 2:283e5bb8a712 757 if(get_data_rs232 & 0x20){
shina 2:283e5bb8a712 758 data7=0x10;
shina 2:283e5bb8a712 759 send_data(0x24,data7);
shina 2:283e5bb8a712 760 p=5;
shina 2:283e5bb8a712 761 }
shina 2:283e5bb8a712 762 break;
shina 2:283e5bb8a712 763
shina 2:283e5bb8a712 764 default:
shina 2:283e5bb8a712 765 p=5;
shina 2:283e5bb8a712 766 break;
shina 2:283e5bb8a712 767
shina 2:283e5bb8a712 768 }
shina 2:283e5bb8a712 769 }
shina 2:283e5bb8a712 770 }
shina 2:283e5bb8a712 771 */
shina 2:283e5bb8a712 772
shina 0:f99e0ea93e31 773 /*
shina 2:283e5bb8a712 774 void sequence_hakidashi()
shina 2:283e5bb8a712 775 {
shina 2:283e5bb8a712 776 if(q==0){
shina 2:283e5bb8a712 777 if(migi1!=old_migi1) {
shina 2:283e5bb8a712 778 old_migi1=migi1;
shina 2:283e5bb8a712 779 if(migi1==1&&select==0) {
shina 2:283e5bb8a712 780 data_servo=0x07;
shina 2:283e5bb8a712 781 send_data(0x18,data_servo);
shina 2:283e5bb8a712 782 wait(1);
shina 2:283e5bb8a712 783 data7=0x00;
shina 2:283e5bb8a712 784 send_data(0x24,data7);
shina 2:283e5bb8a712 785 wait(0.5);
shina 2:283e5bb8a712 786 data7=0x10;
shina 2:283e5bb8a712 787 send_data(0x24,data7);
shina 2:283e5bb8a712 788 data11=0xff;
shina 2:283e5bb8a712 789 while(!(get_data_rs232 & 0x40)){
shina 2:283e5bb8a712 790 send_data(0x30,data11);
shina 2:283e5bb8a712 791 get_data_rs232=slave.getc();
shina 2:283e5bb8a712 792 }
shina 2:283e5bb8a712 793 data11=0x10;
shina 2:283e5bb8a712 794 send_data(0x38,data11);
shina 2:283e5bb8a712 795 data11=0x00;
shina 2:283e5bb8a712 796 while(!(get_data_rs232 & 0x80)){
shina 2:283e5bb8a712 797 send_data(0x30,data11);
shina 2:283e5bb8a712 798 get_data_rs232=slave.getc();
shina 2:283e5bb8a712 799 }
shina 0:f99e0ea93e31 800 data11=0x10;
shina 0:f99e0ea93e31 801 send_data(0x30,data11);
shina 2:283e5bb8a712 802 data7=0xff;
shina 2:283e5bb8a712 803 while(!(get_data_rs232 & 0x20)){
shina 2:283e5bb8a712 804 send_data(0x24,data7);
shina 0:f99e0ea93e31 805 get_data_rs232=slave.getc();
shina 2:283e5bb8a712 806 }
shina 2:283e5bb8a712 807 data7=0x10;
shina 2:283e5bb8a712 808 send_data(0x24,data7);
shina 2:283e5bb8a712 809
shina 2:283e5bb8a712 810 }
shina 2:283e5bb8a712 811 }
shina 2:283e5bb8a712 812 }
shina 2:283e5bb8a712 813 }
shina 2:283e5bb8a712 814 */
shina 0:f99e0ea93e31 815
shina 0:f99e0ea93e31 816 /*
shina 2:283e5bb8a712 817 void sequence_kaisyu_short(){
shina 2:283e5bb8a712 818 if(q==0){
shina 2:283e5bb8a712 819 if(old_triangle!=triangle){
shina 2:283e5bb8a712 820 old_triangle=triangle;
shina 2:283e5bb8a712 821 if(triangle==1){
shina 2:283e5bb8a712 822
shina 2:283e5bb8a712 823 data_servo=0x07;
shina 2:283e5bb8a712 824 send_data(0x18,data_servo);
shina 2:283e5bb8a712 825 wait(1.3);
shina 2:283e5bb8a712 826 data7=0x00;
shina 2:283e5bb8a712 827 send_data(0x24,data7);
shina 2:283e5bb8a712 828 wait(0.3);
shina 2:283e5bb8a712 829 data7=0x10;
shina 2:283e5bb8a712 830 send_data(0x24,data7);
shina 2:283e5bb8a712 831 data_servo=0x08;
shina 2:283e5bb8a712 832 send_data(0x18,data_servo);
shina 2:283e5bb8a712 833 wait(1.3);
shina 2:283e5bb8a712 834 data7=0xff;
shina 2:283e5bb8a712 835 get_data_rs232=get_data_rs232 & 0x;
shina 2:283e5bb8a712 836 while(!(get_data_rs232 & 0x20)) {
shina 2:283e5bb8a712 837 send_data(0x24,data7);
shina 2:283e5bb8a712 838 get_data_rs232=slave.getc();
shina 2:283e5bb8a712 839 }
shina 2:283e5bb8a712 840 data7=0x10;
shina 2:283e5bb8a712 841 send_data(0x24,data7);
shina 2:283e5bb8a712 842
shina 2:283e5bb8a712 843 }
shina 2:283e5bb8a712 844 }
shina 2:283e5bb8a712 845 }
shina 2:283e5bb8a712 846 }*/
shina 2:283e5bb8a712 847
shina 0:f99e0ea93e31 848 void hakidashi(){
shina 2:283e5bb8a712 849 if(q==0){
shina 2:283e5bb8a712 850 if(old_migi1!=migi1){
shina 2:283e5bb8a712 851 old_migi1=migi1;
shina 2:283e5bb8a712 852 if(migi1==1&&select==0){
shina 2:283e5bb8a712 853 r=1;
shina 2:283e5bb8a712 854 }
shina 2:283e5bb8a712 855 }
shina 2:283e5bb8a712 856
shina 2:283e5bb8a712 857 switch(r){
shina 2:283e5bb8a712 858 case 1:
shina 2:283e5bb8a712 859 data_servo=0x07;
shina 2:283e5bb8a712 860 send_data(0x18,data_servo);
shina 2:283e5bb8a712 861 timer2.start();
shina 2:283e5bb8a712 862 if(timer2.read()>1.5){
shina 2:283e5bb8a712 863 timer2.stop();
shina 2:283e5bb8a712 864 timer2.reset();
shina 2:283e5bb8a712 865 r=2;
shina 0:f99e0ea93e31 866 }
shina 2:283e5bb8a712 867 break;
shina 2:283e5bb8a712 868
shina 2:283e5bb8a712 869 case 2:
shina 2:283e5bb8a712 870 data7=0x00;
shina 2:283e5bb8a712 871 send_data(0x24,data7);
shina 2:283e5bb8a712 872 timer2.start();
shina 2:283e5bb8a712 873 if(timer2.read()>0.3){
shina 2:283e5bb8a712 874 timer2.stop();
shina 2:283e5bb8a712 875 timer2.reset();
shina 2:283e5bb8a712 876 data7=0x10;
shina 2:283e5bb8a712 877 send_data(0x24,data7);
shina 2:283e5bb8a712 878 r=3;
shina 2:283e5bb8a712 879 }
shina 2:283e5bb8a712 880 break;
shina 2:283e5bb8a712 881
shina 2:283e5bb8a712 882 case 3:
shina 2:283e5bb8a712 883 data11=0xff;
shina 2:283e5bb8a712 884 send_data(0x30,data11);
shina 2:283e5bb8a712 885 if(get_data_rs232 & 0x40){
shina 2:283e5bb8a712 886 data11=0x10;
shina 2:283e5bb8a712 887 send_data(0x30,data11);
shina 2:283e5bb8a712 888 r=4;
shina 2:283e5bb8a712 889 }
shina 2:283e5bb8a712 890 break;
shina 2:283e5bb8a712 891
shina 2:283e5bb8a712 892 case 4:
shina 0:f99e0ea93e31 893 data11=0x00;
shina 2:283e5bb8a712 894 send_data(0x30,data11);
shina 2:283e5bb8a712 895 if(get_data_rs232 & 0x80){
shina 2:283e5bb8a712 896 data11=0x10;
shina 2:283e5bb8a712 897 send_data(0x30,data11);
shina 2:283e5bb8a712 898 r=5;
shina 2:283e5bb8a712 899 }
shina 2:283e5bb8a712 900 break;
shina 2:283e5bb8a712 901
shina 2:283e5bb8a712 902 case 5:
shina 2:283e5bb8a712 903 data7=0xff;
shina 2:283e5bb8a712 904 send_data(0x24,data7);
shina 2:283e5bb8a712 905 if(get_data_rs232 & 0x20){
shina 2:283e5bb8a712 906 data7=0x10;
shina 2:283e5bb8a712 907 send_data(0x24,data7);
shina 2:283e5bb8a712 908 r=6;
shina 2:283e5bb8a712 909 }
shina 2:283e5bb8a712 910 break;
shina 2:283e5bb8a712 911
shina 2:283e5bb8a712 912 default:
shina 2:283e5bb8a712 913 r=6;
shina 2:283e5bb8a712 914 break;
shina 0:f99e0ea93e31 915 }
shina 2:283e5bb8a712 916 }
shina 0:f99e0ea93e31 917 }
shina 0:f99e0ea93e31 918
shina 2:283e5bb8a712 919
shina 0:f99e0ea93e31 920
shina 0:f99e0ea93e31 921 //ファン
shina 2:283e5bb8a712 922 void change_fan()
shina 2:283e5bb8a712 923 {
shina 2:283e5bb8a712 924 if(q==1){
shina 2:283e5bb8a712 925 if(left2==1&&(old_triangle==triangle)) {
shina 0:f99e0ea93e31 926 data8=0xff;
shina 0:f99e0ea93e31 927 data9=0xff;
shina 0:f99e0ea93e31 928 data10=0xff;
shina 2:283e5bb8a712 929 } else if(left2==0&&(old_triangle==triangle)) {
shina 2:283e5bb8a712 930 data8=0x00;
shina 2:283e5bb8a712 931 data9=0x00;
shina 2:283e5bb8a712 932 data10=0x00;
shina 2:283e5bb8a712 933 }else if(left2==0&&(old_triangle!=triangle)&&select==0){
shina 2:283e5bb8a712 934 old_triangle=triangle;
shina 2:283e5bb8a712 935 if(triangle==1){
shina 2:283e5bb8a712 936 data8=0xff;
shina 2:283e5bb8a712 937 data9=0xff;
shina 2:283e5bb8a712 938 data10=0xff;
shina 2:283e5bb8a712 939 data_servo=0x03;
shina 2:283e5bb8a712 940 send_data(0x26,data8);
shina 2:283e5bb8a712 941 send_data(0x28,data9);
shina 2:283e5bb8a712 942 send_data(0x40,data10);
shina 2:283e5bb8a712 943 wait(0.5);
shina 2:283e5bb8a712 944 send_data(0x18,data_servo);
shina 2:283e5bb8a712 945 wait(1);
shina 0:f99e0ea93e31 946 data8=0x00;
shina 0:f99e0ea93e31 947 data9=0x00;
shina 0:f99e0ea93e31 948 data10=0x00;
shina 2:283e5bb8a712 949 send_data(0x26,data8);
shina 2:283e5bb8a712 950 send_data(0x28,data9);
shina 2:283e5bb8a712 951 send_data(0x40,data10);
shina 2:283e5bb8a712 952 wait(0.5);
shina 2:283e5bb8a712 953 data_servo=0x04;
shina 2:283e5bb8a712 954 send_data(0x18,data_servo);
shina 2:283e5bb8a712 955 }
shina 0:f99e0ea93e31 956 }
shina 2:283e5bb8a712 957 }
shina 2:283e5bb8a712 958
shina 2:283e5bb8a712 959 }
shina 0:f99e0ea93e31 960
shina 0:f99e0ea93e31 961 //i2c
shina 2:283e5bb8a712 962 void send_data(char address,char data)
shina 2:283e5bb8a712 963 {
shina 0:f99e0ea93e31 964 tsushin=1;
shina 0:f99e0ea93e31 965 tushin_check=0;
shina 0:f99e0ea93e31 966 i2c.start();
shina 0:f99e0ea93e31 967 i2c.write(address);
shina 0:f99e0ea93e31 968 tushin_check=i2c.write(data);
shina 0:f99e0ea93e31 969 i2c.stop();
shina 2:283e5bb8a712 970 }
shina 2:283e5bb8a712 971