2019NHK_teamA / Mbed 2 deprecated NHK2019manualver2

A team manual robot program version 2

Dependencies:   mbed PS3

main.cpp@1:b4936498e47c, 2019-08-28 (annotated)

Committer:
shina
Date:
Wed Aug 28 09:05:52 2019 +0000
Revision:
1:b4936498e47c
Parent:
0:f99e0ea93e31 
A team manual robot program version 2

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 0:f99e0ea93e31 7 0x12:左前
shina 0:f99e0ea93e31 8 0x14:右後ろ
shina 0:f99e0ea93e31 9 0x16:左後ろ
shina 0:f99e0ea93e31 10 0x18:サーボ制御回路
shina 0:f99e0ea93e31 11 0x20:右ラック
shina 0:f99e0ea93e31 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 0:f99e0ea93e31 33
shina 0:f99e0ea93e31 34 //変数
shina 0:f99e0ea93e31 35 char data1;//右上
shina 0:f99e0ea93e31 36 char data2;//左上
shina 0:f99e0ea93e31 37 char data3;//右下
shina 0:f99e0ea93e31 38 char data4;//左下
shina 0:f99e0ea93e31 39 char data_servo;
shina 0:f99e0ea93e31 40 char data5;//ラック直動右
shina 0:f99e0ea93e31 41 char data6;//ラック直動左
shina 0:f99e0ea93e31 42 char data7;//回収機構
shina 0:f99e0ea93e31 43 char data8;//右ファン
shina 0:f99e0ea93e31 44 char data9;//左ファン
shina 0:f99e0ea93e31 45 char data10;//パトランプ
shina 0:f99e0ea93e31 46 char data11;//吐き出し機構
shina 0:f99e0ea93e31 47 char get_data_rs232=0x80;
shina 0:f99e0ea93e31 48 int Ry;
shina 0:f99e0ea93e31 49 int Rx;
shina 0:f99e0ea93e31 50 int Ly;
shina 0:f99e0ea93e31 51 int right1;
shina 0:f99e0ea93e31 52 int right2;
shina 0:f99e0ea93e31 53 int left1;
shina 0:f99e0ea93e31 54 int left2;
shina 0:f99e0ea93e31 55 int select;
shina 0:f99e0ea93e31 56 int start;
shina 0:f99e0ea93e31 57 int circle;
shina 0:f99e0ea93e31 58 int cross;
shina 0:f99e0ea93e31 59 int triangle;
shina 0:f99e0ea93e31 60 int square;
shina 0:f99e0ea93e31 61 int ue1;
shina 0:f99e0ea93e31 62 int shita;
shina 0:f99e0ea93e31 63 int migi1;
shina 0:f99e0ea93e31 64 int hidari1;
shina 0:f99e0ea93e31 65 int tushin_check;
shina 0:f99e0ea93e31 66 int old_select=0;
shina 0:f99e0ea93e31 67 int i=1;
shina 0:f99e0ea93e31 68 int old_start=0;
shina 0:f99e0ea93e31 69 int j=1;
shina 0:f99e0ea93e31 70 int old_circle=0;
shina 0:f99e0ea93e31 71 int k=1;
shina 0:f99e0ea93e31 72 int old_square=0;
shina 0:f99e0ea93e31 73 int old_migi1=0;
shina 0:f99e0ea93e31 74 int old_hidari1=0;
shina 0:f99e0ea93e31 75 int n=1;
shina 0:f99e0ea93e31 76 int old_right2=0;
shina 0:f99e0ea93e31 77 int o=1;
shina 0:f99e0ea93e31 78 int old_right1=0;
shina 0:f99e0ea93e31 79 int old_left1=0;
shina 0:f99e0ea93e31 80
shina 0:f99e0ea93e31 81
shina 0:f99e0ea93e31 82
shina 0:f99e0ea93e31 83
shina 0:f99e0ea93e31 84
shina 0:f99e0ea93e31 85 //関数プロトタイプ宣言
shina 0:f99e0ea93e31 86 void get_data();
shina 0:f99e0ea93e31 87 void change_data();
shina 0:f99e0ea93e31 88 void change_pwm();
shina 0:f99e0ea93e31 89 void change_servo();
shina 0:f99e0ea93e31 90 void change_rack();
shina 0:f99e0ea93e31 91 void change_fan();
shina 0:f99e0ea93e31 92 void send_data(char address,char data);
shina 0:f99e0ea93e31 93 void emergency();
shina 0:f99e0ea93e31 94 void change_rack_2();
shina 0:f99e0ea93e31 95 void sequence_kaisyu();
shina 0:f99e0ea93e31 96 //void sequence_hakidashi();
shina 0:f99e0ea93e31 97 //void hakidashi();
shina 0:f99e0ea93e31 98
shina 0:f99e0ea93e31 99 //メイン関数
shina 0:f99e0ea93e31 100 int main(){
shina 0:f99e0ea93e31 101 led=1;
shina 0:f99e0ea93e31 102 data7=0x80;
shina 0:f99e0ea93e31 103 while(true){
shina 0:f99e0ea93e31 104 emergency();
shina 0:f99e0ea93e31 105 get_data();
shina 0:f99e0ea93e31 106 change_pwm();
shina 0:f99e0ea93e31 107 change_servo();
shina 0:f99e0ea93e31 108 change_data();
shina 0:f99e0ea93e31 109 change_rack();
shina 0:f99e0ea93e31 110 change_rack_2();
shina 0:f99e0ea93e31 111 sequence_kaisyu();
shina 0:f99e0ea93e31 112 //sequence_hakidashi();
shina 0:f99e0ea93e31 113 //hakidashi();
shina 0:f99e0ea93e31 114 change_fan();
shina 0:f99e0ea93e31 115 send_data(0x10,data1);
shina 0:f99e0ea93e31 116 send_data(0x12,data2);
shina 0:f99e0ea93e31 117 send_data(0x14,data3);
shina 0:f99e0ea93e31 118 send_data(0x16,data4);
shina 0:f99e0ea93e31 119 send_data(0x18,data_servo);
shina 0:f99e0ea93e31 120 send_data(0x20,data5);
shina 0:f99e0ea93e31 121 send_data(0x22,data6);
shina 0:f99e0ea93e31 122 send_data(0x26,data8);
shina 0:f99e0ea93e31 123 send_data(0x28,data9);
shina 0:f99e0ea93e31 124 send_data(0x40,data10);
shina 0:f99e0ea93e31 125
shina 0:f99e0ea93e31 126 }
shina 0:f99e0ea93e31 127 }
shina 0:f99e0ea93e31 128
shina 0:f99e0ea93e31 129 //データ読み込み
shina 0:f99e0ea93e31 130 void get_data(){
shina 0:f99e0ea93e31 131 Ry=ps3.getRightJoystickYaxis();
shina 0:f99e0ea93e31 132 Rx=ps3.getRightJoystickXaxis();
shina 0:f99e0ea93e31 133 Ly=ps3.getLeftJoystickYaxis();
shina 0:f99e0ea93e31 134 circle=ps3.getButtonState(maru);
shina 0:f99e0ea93e31 135 cross=ps3.getButtonState(batu);
shina 0:f99e0ea93e31 136 triangle=ps3.getButtonState(sankaku);
shina 0:f99e0ea93e31 137 square=ps3.getButtonState(sikaku);
shina 0:f99e0ea93e31 138 left1=ps3.getButtonState(L1);
shina 0:f99e0ea93e31 139 left2=ps3.getButtonState(L2);
shina 0:f99e0ea93e31 140 right1=ps3.getButtonState(R1);
shina 0:f99e0ea93e31 141 right2=ps3.getButtonState(R2);
shina 0:f99e0ea93e31 142 select=ps3.getSELECTState();
shina 0:f99e0ea93e31 143 start=ps3.getSTARTState();
shina 0:f99e0ea93e31 144 ue1=ps3.getButtonState(ue);
shina 0:f99e0ea93e31 145 shita=ps3.getButtonState(sita);
shina 0:f99e0ea93e31 146 hidari1=ps3.getButtonState(hidari);
shina 0:f99e0ea93e31 147 migi1=ps3.getButtonState(migi);
shina 0:f99e0ea93e31 148 get_data_rs232=slave.getc();
shina 0:f99e0ea93e31 149 if(get_data_rs232==0x20||get_data_rs232==0x22||get_data_rs232==0x24||get_data_rs232==0x32||get_data_rs232==0x34){
shina 0:f99e0ea93e31 150 led1=1;
shina 0:f99e0ea93e31 151 }else{
shina 0:f99e0ea93e31 152 led1=0;
shina 0:f99e0ea93e31 153 }
shina 0:f99e0ea93e31 154 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 0:f99e0ea93e31 155 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 156 data_check=0;
shina 0:f99e0ea93e31 157 }else{
shina 0:f99e0ea93e31 158 data_check=1;
shina 0:f99e0ea93e31 159 }
shina 0:f99e0ea93e31 160
shina 0:f99e0ea93e31 161 }
shina 0:f99e0ea93e31 162
shina 0:f99e0ea93e31 163 //緊急停止
shina 0:f99e0ea93e31 164 void emergency(){
shina 0:f99e0ea93e31 165 if(start!=old_start){
shina 0:f99e0ea93e31 166 old_start=start;
shina 0:f99e0ea93e31 167 if(start==1){
shina 0:f99e0ea93e31 168 if(j==1){
shina 0:f99e0ea93e31 169 //緊急停止
shina 0:f99e0ea93e31 170 led=0;
shina 0:f99e0ea93e31 171 stop=1;
shina 0:f99e0ea93e31 172 j=0;
shina 0:f99e0ea93e31 173 }else if(j==0){
shina 0:f99e0ea93e31 174 //緊急停止解除
shina 0:f99e0ea93e31 175 led=1;
shina 0:f99e0ea93e31 176 stop=0;
shina 0:f99e0ea93e31 177 j=1;
shina 0:f99e0ea93e31 178 }
shina 0:f99e0ea93e31 179 }
shina 0:f99e0ea93e31 180 }
shina 0:f99e0ea93e31 181 }
shina 0:f99e0ea93e31 182
shina 0:f99e0ea93e31 183
shina 0:f99e0ea93e31 184 //データ変化(メカナム)
shina 0:f99e0ea93e31 185 void change_data(){
shina 0:f99e0ea93e31 186 if(Ry>30&&i==0){
shina 0:f99e0ea93e31 187 if(right1==1){
shina 0:f99e0ea93e31 188 data1=0xcf;
shina 0:f99e0ea93e31 189 data2=0x00;
shina 0:f99e0ea93e31 190 data3=0xcf;
shina 0:f99e0ea93e31 191 data4=0x00;
shina 0:f99e0ea93e31 192 }else if(left1==1){
shina 0:f99e0ea93e31 193 data1=0x00;
shina 0:f99e0ea93e31 194 data2=0xcf;
shina 0:f99e0ea93e31 195 data3=0x00;
shina 0:f99e0ea93e31 196 data4=0xcf;
shina 0:f99e0ea93e31 197 }else{
shina 0:f99e0ea93e31 198 data1=0x00;
shina 0:f99e0ea93e31 199 data2=0x00;
shina 0:f99e0ea93e31 200 data3=0x00;
shina 0:f99e0ea93e31 201 data4=0x00;
shina 0:f99e0ea93e31 202 }
shina 0:f99e0ea93e31 203 }else if(Ry<-30&&i==0){
shina 0:f99e0ea93e31 204 data1=0xff;
shina 0:f99e0ea93e31 205 data2=0xff;
shina 0:f99e0ea93e31 206 data3=0xff;
shina 0:f99e0ea93e31 207 data4=0xff;
shina 0:f99e0ea93e31 208 }else if(Rx>30&&i==0){
shina 0:f99e0ea93e31 209 data1=0x00;
shina 0:f99e0ea93e31 210 data2=0xff;
shina 0:f99e0ea93e31 211 data3=0xff;
shina 0:f99e0ea93e31 212 data4=0x00;
shina 0:f99e0ea93e31 213 }else if(Rx<-30&&i==0){
shina 0:f99e0ea93e31 214 data1=0xff;
shina 0:f99e0ea93e31 215 data2=0x00;
shina 0:f99e0ea93e31 216 data3=0x00;
shina 0:f99e0ea93e31 217 data4=0xff;
shina 0:f99e0ea93e31 218 }else if(right1==1&&i==0){
shina 0:f99e0ea93e31 219 data1=0xff;
shina 0:f99e0ea93e31 220 data2=0x00;
shina 0:f99e0ea93e31 221 data3=0xff;
shina 0:f99e0ea93e31 222 data4=0x00;
shina 0:f99e0ea93e31 223 }else if(left1==1&&i==0){
shina 0:f99e0ea93e31 224 data1=0x00;
shina 0:f99e0ea93e31 225 data2=0xff;
shina 0:f99e0ea93e31 226 data3=0x00;
shina 0:f99e0ea93e31 227 data4=0xff;
shina 0:f99e0ea93e31 228 }else if(Ry>30&&i==1){
shina 0:f99e0ea93e31 229 if(right1==1){
shina 0:f99e0ea93e31 230 data1=0x4f;
shina 0:f99e0ea93e31 231 data2=0x3f;
shina 0:f99e0ea93e31 232 data3=0x4f;
shina 0:f99e0ea93e31 233 data4=0x3f;
shina 0:f99e0ea93e31 234 }else if(left1==1){
shina 0:f99e0ea93e31 235 data1=0x3f;
shina 0:f99e0ea93e31 236 data2=0x4f;
shina 0:f99e0ea93e31 237 data3=0x3f;
shina 0:f99e0ea93e31 238 data4=0x4f;
shina 0:f99e0ea93e31 239 }else{
shina 0:f99e0ea93e31 240 data1=0x3f;
shina 0:f99e0ea93e31 241 data2=0x3f;
shina 0:f99e0ea93e31 242 data3=0x3f;
shina 0:f99e0ea93e31 243 data4=0x3f;
shina 0:f99e0ea93e31 244 }
shina 0:f99e0ea93e31 245 }else if(Ry<-30&&i==1){
shina 0:f99e0ea93e31 246 data1=0xbf;
shina 0:f99e0ea93e31 247 data2=0xbf;
shina 0:f99e0ea93e31 248 data3=0xbf;
shina 0:f99e0ea93e31 249 data4=0xbf;
shina 0:f99e0ea93e31 250 }else if(Rx>30&&i==1){
shina 0:f99e0ea93e31 251 data1=0x3f;
shina 0:f99e0ea93e31 252 data2=0xbf;
shina 0:f99e0ea93e31 253 data3=0xbf;
shina 0:f99e0ea93e31 254 data4=0x3f;
shina 0:f99e0ea93e31 255 }else if(Rx<-30&&i==1){
shina 0:f99e0ea93e31 256 data1=0xbf;
shina 0:f99e0ea93e31 257 data2=0x3f;
shina 0:f99e0ea93e31 258 data3=0x3f;
shina 0:f99e0ea93e31 259 data4=0xbf;
shina 0:f99e0ea93e31 260 }else if(right1==1&&i==1){
shina 0:f99e0ea93e31 261 data1=0xbf;
shina 0:f99e0ea93e31 262 data2=0x3f;
shina 0:f99e0ea93e31 263 data3=0xbf;
shina 0:f99e0ea93e31 264 data4=0x3f;
shina 0:f99e0ea93e31 265 }else if(left1==1&&i==1){
shina 0:f99e0ea93e31 266 data1=0x3f;
shina 0:f99e0ea93e31 267 data2=0xbf;
shina 0:f99e0ea93e31 268 data3=0x3f;
shina 0:f99e0ea93e31 269 data4=0xbf;
shina 0:f99e0ea93e31 270 }else{
shina 0:f99e0ea93e31 271 data1=0x80;
shina 0:f99e0ea93e31 272 data2=0x80;
shina 0:f99e0ea93e31 273 data3=0x80;
shina 0:f99e0ea93e31 274 data4=0x80;
shina 0:f99e0ea93e31 275 }
shina 0:f99e0ea93e31 276 }
shina 0:f99e0ea93e31 277
shina 0:f99e0ea93e31 278
shina 0:f99e0ea93e31 279 //pwm変化
shina 0:f99e0ea93e31 280 void change_pwm(){
shina 0:f99e0ea93e31 281 if(select!=old_select){
shina 0:f99e0ea93e31 282 old_select=select;
shina 0:f99e0ea93e31 283 if(select==1){
shina 0:f99e0ea93e31 284 if(i==1){
shina 0:f99e0ea93e31 285 pwm=0;
shina 0:f99e0ea93e31 286 i=0;
shina 0:f99e0ea93e31 287 }else if(i==0){
shina 0:f99e0ea93e31 288 pwm=1;
shina 0:f99e0ea93e31 289 i=1;
shina 0:f99e0ea93e31 290 }
shina 0:f99e0ea93e31 291 }
shina 0:f99e0ea93e31 292 }
shina 0:f99e0ea93e31 293 }
shina 0:f99e0ea93e31 294
shina 0:f99e0ea93e31 295 //サーボモーター
shina 0:f99e0ea93e31 296 void change_servo(){
shina 0:f99e0ea93e31 297 if(right2!=old_right2){
shina 0:f99e0ea93e31 298 old_right2=right2;
shina 0:f99e0ea93e31 299 //ハンガー機構
shina 0:f99e0ea93e31 300 if(right2==1){
shina 0:f99e0ea93e31 301 if(o==1){
shina 0:f99e0ea93e31 302 data_servo=0x01;
shina 0:f99e0ea93e31 303 o=0;
shina 0:f99e0ea93e31 304 }else if(o==0){
shina 0:f99e0ea93e31 305 data_servo=0x02;
shina 0:f99e0ea93e31 306 o=1;
shina 0:f99e0ea93e31 307 }
shina 0:f99e0ea93e31 308 }
shina 0:f99e0ea93e31 309 }else if(circle!=old_circle){
shina 0:f99e0ea93e31 310 old_circle=circle;
shina 0:f99e0ea93e31 311 //バスタオル挟む機構
shina 0:f99e0ea93e31 312 if(circle==1){
shina 0:f99e0ea93e31 313 if(k==1){
shina 0:f99e0ea93e31 314 data_servo=0x03;
shina 0:f99e0ea93e31 315 k=0;
shina 0:f99e0ea93e31 316 }else if(k==0){
shina 0:f99e0ea93e31 317 data_servo=0x04;
shina 0:f99e0ea93e31 318 k=1;
shina 0:f99e0ea93e31 319 }
shina 0:f99e0ea93e31 320 }
shina 0:f99e0ea93e31 321 }/*else if(hidari1!=old_hidari1){
shina 0:f99e0ea93e31 322 old_hidari1=hidari1;
shina 0:f99e0ea93e31 323
shina 0:f99e0ea93e31 324 if(hidari1==1&&select==0){
shina 0:f99e0ea93e31 325 if(n==1){
shina 0:f99e0ea93e31 326 data_servo=0x05;
shina 0:f99e0ea93e31 327 n=0;
shina 0:f99e0ea93e31 328 }else if(n==0){
shina 0:f99e0ea93e31 329 data_servo=0x06;
shina 0:f99e0ea93e31 330 n=1;
shina 0:f99e0ea93e31 331 }
shina 0:f99e0ea93e31 332 }
shina 0:f99e0ea93e31 333 }*/
shina 0:f99e0ea93e31 334 }
shina 0:f99e0ea93e31 335
shina 0:f99e0ea93e31 336 //ラック
shina 0:f99e0ea93e31 337 void change_rack(){
shina 0:f99e0ea93e31 338 //右
shina 0:f99e0ea93e31 339 if(triangle==1&&cross==0&&ue1==0&&shita==0&&(-32<Ly<32)){
shina 0:f99e0ea93e31 340 if(get_data_rs232!=0x20&&get_data_rs232!=0x22){
shina 0:f99e0ea93e31 341 data5=0x00;
shina 0:f99e0ea93e31 342 }else if(get_data_rs232==0x20||get_data_rs232==0x22){
shina 0:f99e0ea93e31 343 data5=0x10;
shina 0:f99e0ea93e31 344 }
shina 0:f99e0ea93e31 345 }else if(triangle==0&&cross==1&&ue1==0&&shita==0&&(-32<Ly<32)){
shina 0:f99e0ea93e31 346 if(get_data_rs232!=0x26&&get_data_rs232!=0x28){
shina 0:f99e0ea93e31 347 data5=0xff;
shina 0:f99e0ea93e31 348 }else if(get_data_rs232==0x26||get_data_rs232==0x28){
shina 0:f99e0ea93e31 349 data5=0x10;
shina 0:f99e0ea93e31 350 }
shina 0:f99e0ea93e31 351 //左
shina 0:f99e0ea93e31 352 }else if(triangle==0&&cross==0&&ue1==1&&shita==0&&(-32<Ly<32)){
shina 0:f99e0ea93e31 353 if(get_data_rs232!=0x20&&get_data_rs232!=0x24){
shina 0:f99e0ea93e31 354 data6=0xff;
shina 0:f99e0ea93e31 355 }else if(get_data_rs232==0x20||get_data_rs232==0x24){
shina 0:f99e0ea93e31 356 data6=0x10;
shina 0:f99e0ea93e31 357 }
shina 0:f99e0ea93e31 358 }else if(triangle==0&&cross==0&&ue1==0&&shita==1&&(-32<Ly<32)){
shina 0:f99e0ea93e31 359 if(get_data_rs232!=0x26&&get_data_rs232!=0x30){
shina 0:f99e0ea93e31 360 data6=0x00;
shina 0:f99e0ea93e31 361 }else if(get_data_rs232==0x26||get_data_rs232==0x30){
shina 0:f99e0ea93e31 362 data6=0x10;
shina 0:f99e0ea93e31 363 }
shina 0:f99e0ea93e31 364 }else if(triangle==0&&cross==0&&ue1==0&&shita==0&&(-32<Ly<32)){
shina 0:f99e0ea93e31 365 data5=0x80;
shina 0:f99e0ea93e31 366 data6=0x80;
shina 0:f99e0ea93e31 367
shina 0:f99e0ea93e31 368 }
shina 0:f99e0ea93e31 369
shina 0:f99e0ea93e31 370 }
shina 0:f99e0ea93e31 371
shina 0:f99e0ea93e31 372 //ラック左右
shina 0:f99e0ea93e31 373 void change_rack_2(){
shina 0:f99e0ea93e31 374 if(triangle==0&&cross==0&&ue1==0&&shita==0&&Ly>32){
shina 0:f99e0ea93e31 375 if(get_data_rs232==0x20){
shina 0:f99e0ea93e31 376 data5=0x10;
shina 0:f99e0ea93e31 377 data6=0x10;
shina 0:f99e0ea93e31 378 }else if(get_data_rs232==0x22){
shina 0:f99e0ea93e31 379 data5=0x10;
shina 0:f99e0ea93e31 380 data6=0xff;
shina 0:f99e0ea93e31 381 }else if(get_data_rs232==0x24){
shina 0:f99e0ea93e31 382 data5=0x00;
shina 0:f99e0ea93e31 383 data6=0x10;
shina 0:f99e0ea93e31 384 }else{
shina 0:f99e0ea93e31 385 data5=0x00;
shina 0:f99e0ea93e31 386 data6=0xff;
shina 0:f99e0ea93e31 387 }
shina 0:f99e0ea93e31 388 }else if(triangle==0&&cross==0&&ue1==0&&shita==0&&Ly<-32){
shina 0:f99e0ea93e31 389 if(get_data_rs232==0x26){
shina 0:f99e0ea93e31 390 data5=0x10;
shina 0:f99e0ea93e31 391 data6=0x10;
shina 0:f99e0ea93e31 392 }else if(get_data_rs232==0x28){
shina 0:f99e0ea93e31 393 data5=0x10;
shina 0:f99e0ea93e31 394 data6=0x00;
shina 0:f99e0ea93e31 395 }else if(get_data_rs232==0x30){
shina 0:f99e0ea93e31 396 data5=0xff;
shina 0:f99e0ea93e31 397 data6=0x10;
shina 0:f99e0ea93e31 398 }else{
shina 0:f99e0ea93e31 399 data5=0xff;
shina 0:f99e0ea93e31 400 data6=0x00;
shina 0:f99e0ea93e31 401 }
shina 0:f99e0ea93e31 402 }else if(triangle==0&&cross==0&&ue1==0&&shita==0&&(-32<Ly<32)){
shina 0:f99e0ea93e31 403 data5=0x80;
shina 0:f99e0ea93e31 404 data6=0x80;
shina 0:f99e0ea93e31 405 }
shina 0:f99e0ea93e31 406 }
shina 0:f99e0ea93e31 407
shina 0:f99e0ea93e31 408 //回収機構
shina 0:f99e0ea93e31 409 void sequence_kaisyu(){
shina 0:f99e0ea93e31 410 if(square!=old_square){
shina 0:f99e0ea93e31 411 old_square=square;
shina 0:f99e0ea93e31 412 if(square==1){
shina 0:f99e0ea93e31 413 /*data_servo=0x07;
shina 0:f99e0ea93e31 414 send_data(0x18,data_servo);
shina 0:f99e0ea93e31 415 wait(2);
shina 0:f99e0ea93e31 416 data7=0xff;
shina 0:f99e0ea93e31 417 send_data(0x24,data7);
shina 0:f99e0ea93e31 418 wait(4);
shina 0:f99e0ea93e31 419 data7=0x10;
shina 0:f99e0ea93e31 420 send_data(0x24,data7);
shina 0:f99e0ea93e31 421 data_servo=0x08;
shina 0:f99e0ea93e31 422 send_data(0x18,data_servo);
shina 0:f99e0ea93e31 423 wait(2);
shina 0:f99e0ea93e31 424 data7=0x00;
shina 0:f99e0ea93e31 425 send_data(0x24,data7);
shina 0:f99e0ea93e31 426 wait(4);
shina 0:f99e0ea93e31 427 data7=0x10;
shina 0:f99e0ea93e31 428 send_data(0x24,data7);*/
shina 0:f99e0ea93e31 429
shina 0:f99e0ea93e31 430 data_servo=0x07;
shina 0:f99e0ea93e31 431 send_data(0x18,data_servo);
shina 0:f99e0ea93e31 432 wait(1.5);
shina 0:f99e0ea93e31 433 data7=0x00;
shina 0:f99e0ea93e31 434 while(get_data_rs232!=0x32){
shina 0:f99e0ea93e31 435 send_data(0x24,data7);
shina 0:f99e0ea93e31 436 get_data_rs232=slave.getc();
shina 0:f99e0ea93e31 437 }
shina 0:f99e0ea93e31 438 data7=0x10;
shina 0:f99e0ea93e31 439 send_data(0x24,data7);
shina 0:f99e0ea93e31 440 data_servo=0x08;
shina 0:f99e0ea93e31 441 send_data(0x18,data_servo);
shina 0:f99e0ea93e31 442 wait(2);
shina 0:f99e0ea93e31 443 data7=0xff;
shina 0:f99e0ea93e31 444 while(get_data_rs232!=0x34){
shina 0:f99e0ea93e31 445 send_data(0x24,data7);
shina 0:f99e0ea93e31 446 get_data_rs232=slave.getc();
shina 0:f99e0ea93e31 447 }
shina 0:f99e0ea93e31 448 data7=0x10;
shina 0:f99e0ea93e31 449 send_data(0x24,data7);
shina 0:f99e0ea93e31 450
shina 0:f99e0ea93e31 451 }
shina 0:f99e0ea93e31 452 }
shina 0:f99e0ea93e31 453 }
shina 0:f99e0ea93e31 454 /*
shina 0:f99e0ea93e31 455 void sequence_hakidashi(){
shina 0:f99e0ea93e31 456 if(migi1!=old_migi1){
shina 0:f99e0ea93e31 457 old_migi1=migi1;
shina 0:f99e0ea93e31 458 if(migi1==1){
shina 0:f99e0ea93e31 459 data_servo=0x07;
shina 0:f99e0ea93e31 460 send_data(0x18,data_servo);
shina 0:f99e0ea93e31 461 wait(2);
shina 0:f99e0ea93e31 462 data11=0xff;
shina 0:f99e0ea93e31 463 while(get_data_rs232!=0x36){
shina 0:f99e0ea93e31 464 send_data(0x30,data11);
shina 0:f99e0ea93e31 465 get_data_rs232=slave.getc();
shina 0:f99e0ea93e31 466 }
shina 0:f99e0ea93e31 467 data11=0x10;
shina 0:f99e0ea93e31 468 send_data(0x30,data11);
shina 0:f99e0ea93e31 469 wait(0.5);
shina 0:f99e0ea93e31 470 data11=0x00;
shina 0:f99e0ea93e31 471 while(get_data_rs232!=0x38){
shina 0:f99e0ea93e31 472 send_data(0x30,data11);
shina 0:f99e0ea93e31 473 get_data_rs232=slave.getc();
shina 0:f99e0ea93e31 474 }
shina 0:f99e0ea93e31 475 data11=0x10;
shina 0:f99e0ea93e31 476 send_data(0x30,data11);
shina 0:f99e0ea93e31 477 data_servo=0x08;
shina 0:f99e0ea93e31 478 send_data(0x18,data_servo);
shina 0:f99e0ea93e31 479 wait(2);
shina 0:f99e0ea93e31 480 }
shina 0:f99e0ea93e31 481 }
shina 0:f99e0ea93e31 482 }
shina 0:f99e0ea93e31 483 */
shina 0:f99e0ea93e31 484
shina 0:f99e0ea93e31 485 /*
shina 0:f99e0ea93e31 486 void hakidashi(){
shina 0:f99e0ea93e31 487 if(migi1==1&&hidari1==0){
shina 0:f99e0ea93e31 488 data_servo=0x07;
shina 0:f99e0ea93e31 489 if(get_data_rs232==0x36){
shina 0:f99e0ea93e31 490 data11=0x10;
shina 0:f99e0ea93e31 491 }else if(get_data_rs232!=0x38){
shina 0:f99e0ea93e31 492 data11=0xff;
shina 0:f99e0ea93e31 493 }
shina 0:f99e0ea93e31 494 }else if(migi1==0&&hidari1==1){
shina 0:f99e0ea93e31 495 data_servo=0x08;
shina 0:f99e0ea93e31 496 if(get_data_rs232==0x38){
shina 0:f99e0ea93e31 497 data11=0x10;
shina 0:f99e0ea93e31 498 }else if(get_data_rs232!=0x38){
shina 0:f99e0ea93e31 499 data11=0x00;
shina 0:f99e0ea93e31 500 }
shina 0:f99e0ea93e31 501 }else{
shina 0:f99e0ea93e31 502 data11=0x80;
shina 0:f99e0ea93e31 503 }
shina 0:f99e0ea93e31 504 }
shina 0:f99e0ea93e31 505
shina 0:f99e0ea93e31 506 */
shina 0:f99e0ea93e31 507
shina 0:f99e0ea93e31 508 //ファン
shina 0:f99e0ea93e31 509 void change_fan(){
shina 0:f99e0ea93e31 510 if(left2==1){
shina 0:f99e0ea93e31 511 data8=0xff;
shina 0:f99e0ea93e31 512 data9=0xff;
shina 0:f99e0ea93e31 513 data10=0xff;
shina 0:f99e0ea93e31 514 }else if(left2==0){
shina 0:f99e0ea93e31 515 data8=0x00;
shina 0:f99e0ea93e31 516 data9=0x00;
shina 0:f99e0ea93e31 517 data10=0x00;
shina 0:f99e0ea93e31 518 }
shina 0:f99e0ea93e31 519 }
shina 0:f99e0ea93e31 520
shina 0:f99e0ea93e31 521
shina 0:f99e0ea93e31 522
shina 0:f99e0ea93e31 523
shina 0:f99e0ea93e31 524 //i2c
shina 0:f99e0ea93e31 525 void send_data(char address,char data){
shina 0:f99e0ea93e31 526 tsushin=1;
shina 0:f99e0ea93e31 527 tushin_check=0;
shina 0:f99e0ea93e31 528 i2c.frequency(100000);
shina 0:f99e0ea93e31 529 i2c.start();
shina 0:f99e0ea93e31 530 i2c.write(address);
shina 0:f99e0ea93e31 531 tushin_check=i2c.write(data);
shina 0:f99e0ea93e31 532 i2c.stop();
shina 0:f99e0ea93e31 533 wait(0.003);
shina 0:f99e0ea93e31 534 }