Diff: main.cpp

Revision:

1:70efadf7bcb0

Parent:

0:54e12ff8691e

Child:

2:6745a5554c65

--- a/main.cpp	Fri Aug 10 10:39:55 2018 +0000
+++ b/main.cpp	Fri Aug 10 11:04:46 2018 +0000
@@ -2,6 +2,10 @@
 #include <mbed.h>
 #include <math.h>
 
+DigitalOut motor1(p21);
+DigitalOut motor2(p22);
+DigitalOut motor3(p23);
+DigitalOut motor4(p24);
 I2C i2c(p9, p10);
 Serial pc(USBTX, USBRX);   //TWELITE使う予定なら13,14ピン
 DigitalOut led1(p12);
@@ -35,6 +39,11 @@
 void i2cRead(uint8_t addr,uint8_t reg, uint8_t bytes,uint8_t* data);
 void i2cWrite(uint8_t addr,uint8_t reg, uint8_t data);
 uint8_t IDcheck();
+void Brake();
+void Turn();
+void Return();
+void Right();
+void Left();
 
 uint8_t accgyrodata[14];
 uint8_t magneticdata[7];
@@ -69,9 +78,9 @@
             pc.printf("%d\n\r", theta);
             if(acc <= 1.10){
                 pc.printf("PWRON!!");
-                led1 = 1;
+                Turn();
             }
-            else led1 = 0;
+            else Breky;
         }
         wait(0.5);
     }
@@ -134,4 +143,40 @@
   cmd[0] = reg;     //レジスタ指定
   cmd[1] = data;    //送信するデータ
   i2c.write(addr, cmd, 2); //レジスタ指定はどうする?
+}
+
+//各種モータの関数
+void Breky(){
+    motor1 = 0;
+    motor2 = 0;
+    motor3 = 0;
+    motor4 = 0;    
+}
+
+void Turn(){
+    motor1 = 1;
+    motor2 = 0; 
+    motor3 = 1;
+    motor4 = 0;     
+}
+
+void Return(){
+    motor1 = 0;
+    motor2 = 1;
+    motor3 = 0;
+    motor4 = 1;      
+}
+
+void Left(){
+    motor1 = 1;
+    motor2 = 0;
+    motor3 = 0;
+    motor4 = 1;      
+}
+
+void Right(){
+    motor1 = 0;
+    motor2 = 1;
+    motor3 = 1;
+    motor4 = 0;      
 }
\ No newline at end of file