Diff: main.cpp

Revision:

0:52c9292660e4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Dec 25 06:10:41 2019 +0000
@@ -0,0 +1,106 @@
+//交ロボ nucleo プログラム　 茨城D
+
+//設定
+#include "mbed.h"
+#include "PS3.h"
+I2C i2c(D14,D15);
+Serial pc(USBTX,USBRX);
+PS3 ps3(D8,D2);
+DigitalOut stop(D9);
+DigitalOut green(D7);
+DigitalOut yellow(D6);
+DigitalOut red(D5);
+
+void initialization();
+void emergency();
+void get_data();
+void change_data();
+void send_data(char address,char data);
+
+char data_right=0x00;
+char data_left=0x00;
+int Ry=0;
+int left1=0;
+int right1=0;
+int start=0;
+int old_start=0;
+bool flag=1;
+
+int main(){
+    green=1;
+    stop=0;
+    initialization();
+    while(true){
+        get_data();
+        emergency();
+        change_data();
+        send_data(0x12,data_right);
+        send_data(0x14,data_left);
+    }
+}
+
+void initialization(){
+    red=0;
+    data_right=0x00;
+    data_left=0x00;
+    send_data(0x12,data_right);
+    send_data(0x14,data_left);
+}
+
+void emergency(){
+    if(old_start!=start){
+        old_start=start;
+        if(start==1){
+            if(flag==1){
+                stop=1;
+                flag=0;
+                red=1;
+            }else if(flag==0){
+                stop=0;
+                flag=1;
+                red=0;
+            }
+        }
+    }
+}
+
+void get_data(){
+    Ry=ps3.getLeftJoystickYaxis();
+    left1=ps3.getButtonState(L1);
+    right1=ps3.getButtonState(R1);
+    start=ps3.getSTARTState();
+}
+
+void change_data(){
+    if(Ry>32&&right1==0&&left1==0){
+        red=1;
+        data_right=0xff;
+        data_left=0xaf;
+    }else if(Ry<-32&&right1==0&&left1==0){
+        red=1;
+        data_right=0xaf;
+        data_left=0xff;
+    }else if(-32<=Ry<=32&&right1==1&&left1==0){
+        red=1;
+        data_right=0xff;
+        data_left=0xff;
+    }else if(-32<=Ry<=32&&right1==0&&left1==1){
+        red=1;
+        data_right=0xaf;
+        data_left=0xaf;
+    }else{
+        red=0;
+        data_right=0x00;
+        data_left=0x00;
+    }
+    
+}
+
+void send_data(char address,char data){
+    i2c.start();
+    yellow=i2c.write(address);
+    i2c.write(data);
+    i2c.stop();
+    wait(0.003);
+    
+}
\ No newline at end of file