Diff: main.cpp

Revision:

2:6ac9ac3e5704

Parent:

1:b2edadf8c3d9

--- a/main.cpp	Thu Sep 13 13:33:33 2018 +0000
+++ b/main.cpp	Sun Aug 04 12:22:34 2019 +0000
@@ -1,4 +1,3 @@
-
 #include "mbed.h"
 #include "R1370P.h"
 
@@ -7,7 +6,7 @@
 
 SPISlave spi(PA_7,PA_6,PA_5,PA_4);
 
-PwmOut servo1(PA_3);  //servo1 = アサリつかみ（手前）
+PwmOut servo1(PB_4);  //servo1 = アサリつかみ（手前）
 PwmOut servo2(PB_5);  //servo2 = アサリつかみ（奥）
 PwmOut servo3(PA_11);  //servo3 = アサリアーム
 PwmOut servo4(PA_8);  //servo4 = ガチアサリアーム
@@ -18,7 +17,7 @@
 {
 
     int flag1=0, flag2=0, flag3=0, flag4=0, flag5=0;
-    int data1;
+    int data1, debug=0;
     int old_data;
 
     servo1.period_ms(20);
@@ -38,7 +37,7 @@
 
     while(1) {
 
-        //angle = gyro.getAngle();
+        angle = gyro.getAngle();
         //printf("%f\r\n",angle);
         if(spi.receive()) {
             data1=spi.read();
@@ -55,41 +54,45 @@
             //printf("angle = %f\r\n",reply_angle);
             spi.reply(reply_angle);
 
-            /*printf("%d\r\n",debug);
+            printf("%d\r\n",debug);
             debug++;
-            if(debug>100) debug=0;*/
+            if(debug>100) debug=0;
         }
 
         if(data1 != old_data) {
             if((flag1 == 1) && (data1 == 1)) {
-                servo1.pulsewidth_us(1000);
+                servo1.pulsewidth_us(1200); //閉じる
                 flag1=0;
+                printf("a");
             } else if((flag1 == 0) && (data1 == 1)) {
-                servo1.pulsewidth_us(1800);
+                servo1.pulsewidth_us(2000); //開く
                 flag1=1;
+                printf("b");
             }
 
             if((flag2 == 1) && (data1 == 2)) {
-                servo2.pulsewidth_us(1200);
+                servo2.pulsewidth_us(500); //開く
                 flag2=0;
-            } else if((flag2 == 0) && (data1 == 2)) {
-                servo2.pulsewidth_us(1800);
+                printf("c");
+            } else if((flag2 == 0) && (data1 == 2)) { //閉じる
+                servo2.pulsewidth_us(1500);
                 flag2=1;
+                printf("d");
             }
 
-            if((flag3 == 1) && (data1 == 3)) {
-                servo3.pulsewidth_us(800);
+            if((flag3 == 1) && (data1 = 3)) {
+                servo3.pulsewidth_us(1000);
                 flag3=0;
             } else if((flag3 == 0) && (data1 == 3)) {
-                servo3.pulsewidth_us(1250);
+                servo3.pulsewidth_us(1800);
                 flag3=1;
             }
 
-            if((flag4 == 1) && (data1 == 4)) {
-                servo4.pulsewidth_us(1500);
+            if((flag4 == 1) && (data1 == 4)) { //アーム小
+                servo4.pulsewidth_us(1400); //上がる
                 flag4=0;
-            } else if((flag4 == 0) && (data1 == 4)) {
-                servo4.pulsewidth_us(2000);
+            } else if((flag4 == 0) && (data1 == 4)) { //下がる
+                servo4.pulsewidth_us(2100);
                 flag4=1;
             }