2019NHK_teamA / Mbed 2 deprecated NHK2019robokonaaaaaaaaaaaaa

NHK 2019 team A manual machine program. 改良版(回収動作がおかしい)

Dependencies:   mbed 2019ROBOKONmanualProgram

main.cpp@0:c7e17c2fd542, 2019-06-25 (annotated)

Committer:
shina
Date:
Tue Jun 25 08:53:27 2019 +0000
Revision:
0:c7e17c2fd542
Child:
1:99294241f2ba
This program is NHK 2019 team A manual machine program.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shina 0:c7e17c2fd542 1 /////////////////////////////////////
shina 0:c7e17c2fd542 2 /*NHK2019*/
shina 0:c7e17c2fd542 3 /*Aチーム手動機プログラム*/
shina 0:c7e17c2fd542 4 /*担当者: 品川敦哉*/
shina 0:c7e17c2fd542 5 /*
shina 0:c7e17c2fd542 6 アドレス
shina 0:c7e17c2fd542 7 0x10:右前
shina 0:c7e17c2fd542 8 0x12:左前
shina 0:c7e17c2fd542 9 0x14:右後ろ
shina 0:c7e17c2fd542 10 0x16:左後ろ
shina 0:c7e17c2fd542 11 0x18:サーボ制御回路
shina 0:c7e17c2fd542 12 0x20:右ラック
shina 0:c7e17c2fd542 13 0x22:左ラック
shina 0:c7e17c2fd542 14 0x24:回収機構
shina 0:c7e17c2fd542 15 0x26:右ファン
shina 0:c7e17c2fd542 16 0x28:左ファン
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 0:c7e17c2fd542 34 //変数
shina 0:c7e17c2fd542 35 char data1;//右上
shina 0:c7e17c2fd542 36 char data2;//左上
shina 0:c7e17c2fd542 37 char data3;//右下
shina 0:c7e17c2fd542 38 char data4;//左下
shina 0:c7e17c2fd542 39 char data_servo;
shina 0:c7e17c2fd542 40 char data5;//ラック直動右
shina 0:c7e17c2fd542 41 char data6;//ラック直動左
shina 0:c7e17c2fd542 42 char data7;//回収機構
shina 0:c7e17c2fd542 43 char data8;//右ファン
shina 0:c7e17c2fd542 44 char data9;//左ファン
shina 0:c7e17c2fd542 45 char get_data_rs232=0x80;
shina 0:c7e17c2fd542 46 int Ry;
shina 0:c7e17c2fd542 47 int Rx;
shina 0:c7e17c2fd542 48 int right1;
shina 0:c7e17c2fd542 49 int left1;
shina 0:c7e17c2fd542 50 int left2;
shina 0:c7e17c2fd542 51 int select;
shina 0:c7e17c2fd542 52 int start;
shina 0:c7e17c2fd542 53 int circle;
shina 0:c7e17c2fd542 54 int cross;
shina 0:c7e17c2fd542 55 int ue1;
shina 0:c7e17c2fd542 56 int shita;
shina 0:c7e17c2fd542 57 int migi1;
shina 0:c7e17c2fd542 58 int hidari1;
shina 0:c7e17c2fd542 59 int tushin_check;
shina 0:c7e17c2fd542 60 int old_select=0;
shina 0:c7e17c2fd542 61 int i=1;
shina 0:c7e17c2fd542 62 int old_start=0;
shina 0:c7e17c2fd542 63 int j=1;
shina 0:c7e17c2fd542 64 int old_circle=0;
shina 0:c7e17c2fd542 65 int k=1;
shina 0:c7e17c2fd542 66 int old_cross=0;
shina 0:c7e17c2fd542 67 int l=1;
shina 0:c7e17c2fd542 68
shina 0:c7e17c2fd542 69
shina 0:c7e17c2fd542 70 //関数プロトタイプ宣言
shina 0:c7e17c2fd542 71 void get_data();
shina 0:c7e17c2fd542 72 void change_data();
shina 0:c7e17c2fd542 73 void change_pwm();
shina 0:c7e17c2fd542 74 void change_servo();
shina 0:c7e17c2fd542 75 void change_rack();
shina 0:c7e17c2fd542 76 void change_kaisyu();
shina 0:c7e17c2fd542 77 void change_fan();
shina 0:c7e17c2fd542 78 void send_data(char address,char data);
shina 0:c7e17c2fd542 79 void emergency();
shina 0:c7e17c2fd542 80 void get_limit_data();
shina 0:c7e17c2fd542 81
shina 0:c7e17c2fd542 82 //メイン関数
shina 0:c7e17c2fd542 83 int main(){
shina 0:c7e17c2fd542 84 led=1;
shina 0:c7e17c2fd542 85 while(true){
shina 0:c7e17c2fd542 86 emergency();
shina 0:c7e17c2fd542 87 get_data();
shina 0:c7e17c2fd542 88 change_pwm();
shina 0:c7e17c2fd542 89 change_servo();
shina 0:c7e17c2fd542 90 change_data();
shina 0:c7e17c2fd542 91 change_rack();
shina 0:c7e17c2fd542 92 change_kaisyu();
shina 0:c7e17c2fd542 93 change_fan();
shina 0:c7e17c2fd542 94 get_limit_data();
shina 0:c7e17c2fd542 95 send_data(0x10,data1);
shina 0:c7e17c2fd542 96 send_data(0x12,data2);
shina 0:c7e17c2fd542 97 send_data(0x14,data3);
shina 0:c7e17c2fd542 98 send_data(0x16,data4);
shina 0:c7e17c2fd542 99 send_data(0x18,data_servo);
shina 0:c7e17c2fd542 100 send_data(0x20,data5);
shina 0:c7e17c2fd542 101 send_data(0x22,data6);
shina 0:c7e17c2fd542 102 send_data(0x24,data7);
shina 0:c7e17c2fd542 103 send_data(0x26,data8);
shina 0:c7e17c2fd542 104 send_data(0x28,data9);
shina 0:c7e17c2fd542 105 }
shina 0:c7e17c2fd542 106 }
shina 0:c7e17c2fd542 107
shina 0:c7e17c2fd542 108 //データ読み込み
shina 0:c7e17c2fd542 109 void get_data(){
shina 0:c7e17c2fd542 110 Ry=ps3.getRightJoystickYaxis();
shina 0:c7e17c2fd542 111 Rx=ps3.getRightJoystickXaxis();
shina 0:c7e17c2fd542 112 circle=ps3.getButtonState(maru);
shina 0:c7e17c2fd542 113 cross=ps3.getButtonState(batu);
shina 0:c7e17c2fd542 114 left1=ps3.getButtonState(L1);
shina 0:c7e17c2fd542 115 left2=ps3.getButtonState(L2);
shina 0:c7e17c2fd542 116 right1=ps3.getButtonState(R1);
shina 0:c7e17c2fd542 117 select=ps3.getSELECTState();
shina 0:c7e17c2fd542 118 start=ps3.getSTARTState();
shina 0:c7e17c2fd542 119 ue1=ps3.getButtonState(ue);
shina 0:c7e17c2fd542 120 shita=ps3.getButtonState(sita);
shina 0:c7e17c2fd542 121 hidari1=ps3.getButtonState(hidari);
shina 0:c7e17c2fd542 122 migi1=ps3.getButtonState(migi);
shina 0:c7e17c2fd542 123 pc.printf("%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n",Ry,Rx,left1,left2,right1,select,start,ue1,shita,migi1,hidari1,tushin_check,i);
shina 0:c7e17c2fd542 124 if(Ry==0&&Rx==0&&left1==0&&right1==0&&left2==0&&select==0&&start==0&&shita==0&&ue1==0&&migi1==0&&hidari1==0){
shina 0:c7e17c2fd542 125 data_check=0;
shina 0:c7e17c2fd542 126 }else{
shina 0:c7e17c2fd542 127 data_check=1;
shina 0:c7e17c2fd542 128 }
shina 0:c7e17c2fd542 129
shina 0:c7e17c2fd542 130 }
shina 0:c7e17c2fd542 131
shina 0:c7e17c2fd542 132 //緊急停止
shina 0:c7e17c2fd542 133 void emergency(){
shina 0:c7e17c2fd542 134 if(start!=old_start){
shina 0:c7e17c2fd542 135 old_start=start;
shina 0:c7e17c2fd542 136 if(start==1){
shina 0:c7e17c2fd542 137 if(j==1){
shina 0:c7e17c2fd542 138 //緊急停止
shina 0:c7e17c2fd542 139 data_servo=0x00;
shina 0:c7e17c2fd542 140 send_data(0x18,data_servo);
shina 0:c7e17c2fd542 141 led=0;
shina 0:c7e17c2fd542 142 stop=1;
shina 0:c7e17c2fd542 143 j=0;
shina 0:c7e17c2fd542 144 }else if(j==0){
shina 0:c7e17c2fd542 145 //緊急停止解除
shina 0:c7e17c2fd542 146 led=1;
shina 0:c7e17c2fd542 147 stop=0;
shina 0:c7e17c2fd542 148 j=1;
shina 0:c7e17c2fd542 149 }
shina 0:c7e17c2fd542 150 }
shina 0:c7e17c2fd542 151 }
shina 0:c7e17c2fd542 152 }
shina 0:c7e17c2fd542 153
shina 0:c7e17c2fd542 154
shina 0:c7e17c2fd542 155 //データ変化(メカナム)
shina 0:c7e17c2fd542 156 void change_data(){
shina 0:c7e17c2fd542 157 if(Ry>30&&i==0){
shina 0:c7e17c2fd542 158 data1=0xff;
shina 0:c7e17c2fd542 159 data2=0x00;
shina 0:c7e17c2fd542 160 data3=0xff;
shina 0:c7e17c2fd542 161 data4=0x00;
shina 0:c7e17c2fd542 162 }else if(Ry<-30&&i==0){
shina 0:c7e17c2fd542 163 data1=0x00;
shina 0:c7e17c2fd542 164 data2=0xff;
shina 0:c7e17c2fd542 165 data3=0x00;
shina 0:c7e17c2fd542 166 data4=0xff;
shina 0:c7e17c2fd542 167 }else if(Rx>30&&i==0){
shina 0:c7e17c2fd542 168 data1=0xff;
shina 0:c7e17c2fd542 169 data2=0xff;
shina 0:c7e17c2fd542 170 data3=0x00;
shina 0:c7e17c2fd542 171 data4=0x00;
shina 0:c7e17c2fd542 172 }else if(Rx<-30&&i==0){
shina 0:c7e17c2fd542 173 data1=0x00;
shina 0:c7e17c2fd542 174 data2=0x00;
shina 0:c7e17c2fd542 175 data3=0xff;
shina 0:c7e17c2fd542 176 data4=0xff;
shina 0:c7e17c2fd542 177 }else if(right1==1&&i==0){
shina 0:c7e17c2fd542 178 data1=0x00;
shina 0:c7e17c2fd542 179 data2=0x00;
shina 0:c7e17c2fd542 180 data3=0x00;
shina 0:c7e17c2fd542 181 data4=0x00;
shina 0:c7e17c2fd542 182 }else if(left1==1&&i==0){
shina 0:c7e17c2fd542 183 data1=0xff;
shina 0:c7e17c2fd542 184 data2=0xff;
shina 0:c7e17c2fd542 185 data3=0xff;
shina 0:c7e17c2fd542 186 data4=0xff;
shina 0:c7e17c2fd542 187 }else if(Ry>30&&i==1){
shina 0:c7e17c2fd542 188 data1=0xbf;
shina 0:c7e17c2fd542 189 data2=0x3f;
shina 0:c7e17c2fd542 190 data3=0xbf;
shina 0:c7e17c2fd542 191 data4=0x3f;
shina 0:c7e17c2fd542 192 }else if(Ry<-30&&i==1){
shina 0:c7e17c2fd542 193 data1=0x3f;
shina 0:c7e17c2fd542 194 data2=0xbf;
shina 0:c7e17c2fd542 195 data3=0x3f;
shina 0:c7e17c2fd542 196 data4=0xbf;
shina 0:c7e17c2fd542 197 }else if(Rx>30&&i==1){
shina 0:c7e17c2fd542 198 data1=0xbf;
shina 0:c7e17c2fd542 199 data2=0xbf;
shina 0:c7e17c2fd542 200 data3=0x3f;
shina 0:c7e17c2fd542 201 data4=0x3f;
shina 0:c7e17c2fd542 202 }else if(Rx<-30&&i==1){
shina 0:c7e17c2fd542 203 data1=0x3f;
shina 0:c7e17c2fd542 204 data2=0x3f;
shina 0:c7e17c2fd542 205 data3=0xbf;
shina 0:c7e17c2fd542 206 data4=0xbf;
shina 0:c7e17c2fd542 207 }else if(right1==1&&i==1){
shina 0:c7e17c2fd542 208 data1=0x3f;
shina 0:c7e17c2fd542 209 data2=0x3f;
shina 0:c7e17c2fd542 210 data3=0x3f;
shina 0:c7e17c2fd542 211 data4=0x3f;
shina 0:c7e17c2fd542 212 }else if(left1==1&&i==1){
shina 0:c7e17c2fd542 213 data1=0xbf;
shina 0:c7e17c2fd542 214 data2=0xbf;
shina 0:c7e17c2fd542 215 data3=0xbf;
shina 0:c7e17c2fd542 216 data4=0xbf;
shina 0:c7e17c2fd542 217 }else{
shina 0:c7e17c2fd542 218 data1=0x80;
shina 0:c7e17c2fd542 219 data2=0x80;
shina 0:c7e17c2fd542 220 data3=0x80;
shina 0:c7e17c2fd542 221 data4=0x80;
shina 0:c7e17c2fd542 222 }
shina 0:c7e17c2fd542 223 }
shina 0:c7e17c2fd542 224
shina 0:c7e17c2fd542 225 //pwm変化
shina 0:c7e17c2fd542 226 void change_pwm(){
shina 0:c7e17c2fd542 227 if(select!=old_select){
shina 0:c7e17c2fd542 228 old_select=select;
shina 0:c7e17c2fd542 229 if(select==1){
shina 0:c7e17c2fd542 230 if(i==1){
shina 0:c7e17c2fd542 231 pwm=0;
shina 0:c7e17c2fd542 232 i=0;
shina 0:c7e17c2fd542 233 }else if(i==0){
shina 0:c7e17c2fd542 234 pwm=1;
shina 0:c7e17c2fd542 235 i=1;
shina 0:c7e17c2fd542 236 }
shina 0:c7e17c2fd542 237 }
shina 0:c7e17c2fd542 238 }
shina 0:c7e17c2fd542 239 }
shina 0:c7e17c2fd542 240
shina 0:c7e17c2fd542 241 //サーボモーター
shina 0:c7e17c2fd542 242 void change_servo(){
shina 0:c7e17c2fd542 243 if(j!=0){
shina 0:c7e17c2fd542 244 if(circle!=old_circle){
shina 0:c7e17c2fd542 245 if(circle==1){
shina 0:c7e17c2fd542 246 if(k==1){
shina 0:c7e17c2fd542 247 data_servo=0x01;
shina 0:c7e17c2fd542 248 k=0;
shina 0:c7e17c2fd542 249 }else if(k==0){
shina 0:c7e17c2fd542 250 data_servo=0x02;
shina 0:c7e17c2fd542 251 k=1;
shina 0:c7e17c2fd542 252 }
shina 0:c7e17c2fd542 253 }
shina 0:c7e17c2fd542 254 }else if(cross!=old_cross){
shina 0:c7e17c2fd542 255 if(cross==1){
shina 0:c7e17c2fd542 256 if(l==1){
shina 0:c7e17c2fd542 257 data_servo=0x03;
shina 0:c7e17c2fd542 258 l=0;
shina 0:c7e17c2fd542 259 }else if(l==0){
shina 0:c7e17c2fd542 260 data_servo=0x04;
shina 0:c7e17c2fd542 261 l=1;
shina 0:c7e17c2fd542 262 }
shina 0:c7e17c2fd542 263 }
shina 0:c7e17c2fd542 264 }
shina 0:c7e17c2fd542 265 }
shina 0:c7e17c2fd542 266 }
shina 0:c7e17c2fd542 267
shina 0:c7e17c2fd542 268 //ラック
shina 0:c7e17c2fd542 269 void change_rack(){
shina 0:c7e17c2fd542 270 if(ue1==1&&shita==0){
shina 0:c7e17c2fd542 271 data5=0x00;
shina 0:c7e17c2fd542 272 data6=0x00;
shina 0:c7e17c2fd542 273 }else if(ue1==0&&shita==1){
shina 0:c7e17c2fd542 274 data5=0xff;
shina 0:c7e17c2fd542 275 data6=0xff;
shina 0:c7e17c2fd542 276 }else{
shina 0:c7e17c2fd542 277 data5=0x80;
shina 0:c7e17c2fd542 278 data6=0x80;
shina 0:c7e17c2fd542 279 }
shina 0:c7e17c2fd542 280 }
shina 0:c7e17c2fd542 281
shina 0:c7e17c2fd542 282 //回収機構
shina 0:c7e17c2fd542 283 void change_kaisyu(){
shina 0:c7e17c2fd542 284 if(migi1==1&&hidari1==0){
shina 0:c7e17c2fd542 285 data7=0xff;
shina 0:c7e17c2fd542 286 }else if(migi1==0&&hidari1==1){
shina 0:c7e17c2fd542 287 data7=0x00;
shina 0:c7e17c2fd542 288 }else{
shina 0:c7e17c2fd542 289 data7=0x80;
shina 0:c7e17c2fd542 290 }
shina 0:c7e17c2fd542 291 }
shina 0:c7e17c2fd542 292
shina 0:c7e17c2fd542 293 //ファン
shina 0:c7e17c2fd542 294 void change_fan(){
shina 0:c7e17c2fd542 295 if(left2==1){
shina 0:c7e17c2fd542 296 data8=0xff;
shina 0:c7e17c2fd542 297 data9=0xff;
shina 0:c7e17c2fd542 298 }else if(left2==0){
shina 0:c7e17c2fd542 299 data8=0x00;
shina 0:c7e17c2fd542 300 data9=0x00;
shina 0:c7e17c2fd542 301 }
shina 0:c7e17c2fd542 302 }
shina 0:c7e17c2fd542 303
shina 0:c7e17c2fd542 304 //リミットスイッチデータ取得
shina 0:c7e17c2fd542 305 void get_limit_data(){
shina 0:c7e17c2fd542 306 get_data_rs232=slave.getc();
shina 0:c7e17c2fd542 307 if(get_data_rs232==0x20){
shina 0:c7e17c2fd542 308 if(ue1==1){
shina 0:c7e17c2fd542 309 data5=0x80;
shina 0:c7e17c2fd542 310 data6=0x80;
shina 0:c7e17c2fd542 311 }
shina 0:c7e17c2fd542 312 }else if(get_data_rs232==0x22){
shina 0:c7e17c2fd542 313 if(ue1==1){
shina 0:c7e17c2fd542 314 data5=0x80;
shina 0:c7e17c2fd542 315 }
shina 0:c7e17c2fd542 316 }else if(get_data_rs232==0x24){
shina 0:c7e17c2fd542 317 if(ue1==1){
shina 0:c7e17c2fd542 318 data6=0x80;
shina 0:c7e17c2fd542 319 }
shina 0:c7e17c2fd542 320 }else if(get_data_rs232==0x26){
shina 0:c7e17c2fd542 321 if(shita==1){
shina 0:c7e17c2fd542 322 data5=0x80;
shina 0:c7e17c2fd542 323 data6=0x80;
shina 0:c7e17c2fd542 324 }
shina 0:c7e17c2fd542 325 }else if(get_data_rs232==0x28){
shina 0:c7e17c2fd542 326 if(shita==1){
shina 0:c7e17c2fd542 327 data5=0x80;
shina 0:c7e17c2fd542 328 }
shina 0:c7e17c2fd542 329 }else if(get_data_rs232==0x30){
shina 0:c7e17c2fd542 330 if(shita==1){
shina 0:c7e17c2fd542 331 data6=0x80;
shina 0:c7e17c2fd542 332 }
shina 0:c7e17c2fd542 333 }else if(get_data_rs232==0x32){
shina 0:c7e17c2fd542 334 if(migi1==1){
shina 0:c7e17c2fd542 335 data7=0x80;
shina 0:c7e17c2fd542 336 }
shina 0:c7e17c2fd542 337 }else if(get_data_rs232==0x34){
shina 0:c7e17c2fd542 338 if(hidari1==1){
shina 0:c7e17c2fd542 339 data7=0x80;
shina 0:c7e17c2fd542 340 }
shina 0:c7e17c2fd542 341 }
shina 0:c7e17c2fd542 342 }
shina 0:c7e17c2fd542 343
shina 0:c7e17c2fd542 344 //i2c
shina 0:c7e17c2fd542 345 void send_data(char address,char data){
shina 0:c7e17c2fd542 346 tsushin=1;
shina 0:c7e17c2fd542 347 tushin_check=0;
shina 0:c7e17c2fd542 348 i2c.frequency(100000);
shina 0:c7e17c2fd542 349 i2c.start();
shina 0:c7e17c2fd542 350 i2c.write(address);
shina 0:c7e17c2fd542 351 tushin_check=i2c.write(data);
shina 0:c7e17c2fd542 352 i2c.stop();
shina 0:c7e17c2fd542 353 wait(0.003);
shina 0:c7e17c2fd542 354 }