Diff: main.cpp

Revision:

0:c7e17c2fd542

Child:

1:99294241f2ba

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Jun 25 08:53:27 2019 +0000
@@ -0,0 +1,354 @@
+/////////////////////////////////////
+/*NHK2019*/
+/*Aチーム手動機プログラム*/
+/*担当者: 品川敦哉*/
+/*
+アドレス
+0x10:右前
+0x12:左前
+0x14:右後ろ
+0x16:左後ろ
+0x18:サーボ制御回路
+0x20:右ラック
+0x22:左ラック
+0x24:回収機構
+0x26:右ファン
+0x28:左ファン
+*/
+/////////////////////////////////////
+
+//宣言
+#include "mbed.h"
+#include "PS3.h"
+I2C i2c(D14,D15);
+Serial pc(USBTX,USBRX);
+Serial slave(PC_6,PC_7);
+PS3 ps3(D8,D2);
+DigitalOut led(D5);//電源確認
+DigitalOut tsushin(D6);//通信確認
+DigitalOut data_check(D7);
+DigitalOut pwm(D4);
+DigitalOut stop(D10);
+DigitalOut led1(LED1);
+
+//変数
+char data1;//右上
+char data2;//左上
+char data3;//右下
+char data4;//左下
+char data_servo;
+char data5;//ラック直動右
+char data6;//ラック直動左
+char data7;//回収機構
+char data8;//右ファン
+char data9;//左ファン
+char get_data_rs232=0x80;
+int Ry;
+int Rx;
+int right1;
+int left1;
+int left2;
+int select;
+int start;
+int circle;
+int cross;
+int ue1;
+int shita;
+int migi1;
+int hidari1;
+int tushin_check;
+int old_select=0;
+int i=1;
+int old_start=0;
+int j=1;
+int old_circle=0;
+int k=1;
+int old_cross=0;
+int l=1;
+
+
+//関数プロトタイプ宣言
+void get_data();
+void change_data();
+void change_pwm();
+void change_servo();
+void change_rack();
+void change_kaisyu();
+void change_fan();
+void send_data(char address,char data);
+void emergency();
+void get_limit_data();
+
+//メイン関数
+int main(){
+    led=1;
+    while(true){
+        emergency();
+        get_data();
+        change_pwm();
+        change_servo();
+        change_data();
+        change_rack();
+        change_kaisyu();
+        change_fan();
+        get_limit_data();
+        send_data(0x10,data1);
+        send_data(0x12,data2);
+        send_data(0x14,data3);
+        send_data(0x16,data4);
+        send_data(0x18,data_servo);
+        send_data(0x20,data5);
+        send_data(0x22,data6);
+        send_data(0x24,data7);
+        send_data(0x26,data8);
+        send_data(0x28,data9);
+        }
+    }
+    
+//データ読み込み
+void get_data(){
+    Ry=ps3.getRightJoystickYaxis();
+    Rx=ps3.getRightJoystickXaxis();
+    circle=ps3.getButtonState(maru);
+    cross=ps3.getButtonState(batu);
+    left1=ps3.getButtonState(L1);
+    left2=ps3.getButtonState(L2);
+    right1=ps3.getButtonState(R1);
+    select=ps3.getSELECTState();
+    start=ps3.getSTARTState();
+    ue1=ps3.getButtonState(ue);
+    shita=ps3.getButtonState(sita);
+    hidari1=ps3.getButtonState(hidari);
+    migi1=ps3.getButtonState(migi);
+    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);
+    if(Ry==0&&Rx==0&&left1==0&&right1==0&&left2==0&&select==0&&start==0&&shita==0&&ue1==0&&migi1==0&&hidari1==0){
+        data_check=0;
+        }else{
+            data_check=1;
+            }
+    
+    }
+    
+//緊急停止
+void emergency(){
+    if(start!=old_start){
+    old_start=start;
+    if(start==1){
+        if(j==1){
+    //緊急停止
+            data_servo=0x00;
+            send_data(0x18,data_servo);
+            led=0;
+            stop=1;
+            j=0;
+        }else if(j==0){
+    //緊急停止解除
+            led=1;
+            stop=0;
+            j=1;
+            }
+        }
+    }  
+}
+
+    
+//データ変化(メカナム)
+void change_data(){
+    if(Ry>30&&i==0){
+        data1=0xff;
+        data2=0x00;
+        data3=0xff;
+        data4=0x00;
+        }else if(Ry<-30&&i==0){
+        data1=0x00;
+        data2=0xff;
+        data3=0x00;
+        data4=0xff;
+        }else if(Rx>30&&i==0){
+        data1=0xff;
+        data2=0xff;
+        data3=0x00;
+        data4=0x00;
+        }else if(Rx<-30&&i==0){
+        data1=0x00;
+        data2=0x00;
+        data3=0xff;
+        data4=0xff;
+        }else if(right1==1&&i==0){
+        data1=0x00;
+        data2=0x00;
+        data3=0x00;
+        data4=0x00;
+        }else if(left1==1&&i==0){
+        data1=0xff;
+        data2=0xff;
+        data3=0xff;
+        data4=0xff;
+        }else if(Ry>30&&i==1){
+        data1=0xbf;
+        data2=0x3f;
+        data3=0xbf;
+        data4=0x3f;
+        }else if(Ry<-30&&i==1){
+        data1=0x3f;
+        data2=0xbf;
+        data3=0x3f;
+        data4=0xbf;
+        }else if(Rx>30&&i==1){
+        data1=0xbf;
+        data2=0xbf;
+        data3=0x3f;
+        data4=0x3f;
+        }else if(Rx<-30&&i==1){
+        data1=0x3f;
+        data2=0x3f;
+        data3=0xbf;
+        data4=0xbf;
+        }else if(right1==1&&i==1){
+        data1=0x3f;
+        data2=0x3f;
+        data3=0x3f;
+        data4=0x3f;
+        }else if(left1==1&&i==1){
+        data1=0xbf;
+        data2=0xbf;
+        data3=0xbf;
+        data4=0xbf;
+        }else{
+        data1=0x80;
+        data2=0x80;
+        data3=0x80;
+        data4=0x80;
+        }
+    }
+
+//pwm変化    
+void change_pwm(){
+if(select!=old_select){
+    old_select=select;
+    if(select==1){
+    if(i==1){
+        pwm=0;
+        i=0;
+        }else if(i==0){
+            pwm=1;
+            i=1;
+            }
+        }
+    }
+}
+
+//サーボモーター
+void change_servo(){
+    if(j!=0){
+        if(circle!=old_circle){
+                    if(circle==1){
+                        if(k==1){
+                            data_servo=0x01;
+                            k=0;
+                        }else if(k==0){
+                            data_servo=0x02;
+                            k=1;
+                        }
+                    }
+            }else if(cross!=old_cross){
+                    if(cross==1){
+                        if(l==1){
+                            data_servo=0x03;
+                            l=0;
+                        }else if(l==0){
+                            data_servo=0x04;
+                            l=1;   
+                        }
+                    }
+            }
+        }
+     }
+    
+//ラック
+void change_rack(){
+    if(ue1==1&&shita==0){
+        data5=0x00;
+        data6=0x00;
+        }else if(ue1==0&&shita==1){
+            data5=0xff;
+            data6=0xff;
+            }else{
+                data5=0x80;
+                data6=0x80;
+                }
+    }
+
+//回収機構
+void change_kaisyu(){
+    if(migi1==1&&hidari1==0){
+        data7=0xff;
+        }else if(migi1==0&&hidari1==1){
+            data7=0x00;
+            }else{
+                data7=0x80;
+                }
+    }
+    
+//ファン
+void change_fan(){
+    if(left2==1){
+        data8=0xff;
+        data9=0xff;
+        }else if(left2==0){
+            data8=0x00;
+            data9=0x00;
+            }
+    }
+  
+//リミットスイッチデータ取得  
+void get_limit_data(){
+    get_data_rs232=slave.getc();
+        if(get_data_rs232==0x20){
+              if(ue1==1){
+                data5=0x80;
+                data6=0x80;
+                }
+        }else if(get_data_rs232==0x22){
+            if(ue1==1){
+                data5=0x80;
+                }
+        }else if(get_data_rs232==0x24){
+            if(ue1==1){
+                data6=0x80;
+                }
+        }else if(get_data_rs232==0x26){
+            if(shita==1){
+                data5=0x80;
+                data6=0x80;
+                }
+        }else if(get_data_rs232==0x28){
+            if(shita==1){
+                data5=0x80;
+                }
+        }else if(get_data_rs232==0x30){
+            if(shita==1){
+                data6=0x80;
+                }
+        }else if(get_data_rs232==0x32){
+            if(migi1==1){
+                data7=0x80;
+                }
+        }else if(get_data_rs232==0x34){
+            if(hidari1==1){
+                data7=0x80;
+                }
+        }
+    }
+    
+//i2c
+void send_data(char address,char data){
+    tsushin=1;
+    tushin_check=0;
+    i2c.frequency(100000);
+    i2c.start();
+    i2c.write(address);
+    tushin_check=i2c.write(data);
+    i2c.stop();
+    wait(0.003);
+    }
\ No newline at end of file