Diff: main.cpp

Revision:

0:5cafeb163455

Child:

1:b2edadf8c3d9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Sep 07 05:19:29 2018 +0000
@@ -0,0 +1,107 @@
+#include "mbed.h"
+#include "R1370P.h"
+
+Serial pc(USBTX, USBRX);
+R1370P gyro(PA_9,PA_10);
+
+SPISlave spi(PA_7,PA_6,PA_5,PA_4);
+
+PwmOut servo1(PB_4);  //servo1 = アサリつかみ（手前）
+PwmOut servo2(PB_5);  //servo2 = アサリつかみ（奥）
+PwmOut servo3(PA_11);  //servo3 = アサリアーム
+PwmOut servo4(PA_8);  //servo4 = ガチアサリアーム
+
+DigitalOut out(PA_1);  //エアシリンダー
+
+int main()
+{
+
+    int flag1=0, flag2=0, flag3=0, flag4=0, flag5=0;
+    int data1;
+    int old_data;
+
+    servo1.period_ms(20);
+    servo2.period_ms(20);
+    servo3.period_ms(20);
+    servo4.period_ms(20);
+
+    spi.format(16,3);
+    spi.frequency(1000000);
+
+    //int debug=0;
+
+    double angle, reply_angle;
+    int a;
+    gyro.initialize();
+
+
+    while(1) {
+
+        //angle = gyro.getAngle();
+        //printf("%f\r\n",angle);
+        if(spi.receive()) {
+            data1=spi.read();
+
+            angle = gyro.getAngle();
+            if(angle >= 0) {  //反時計回りに0~359°となるよう修正
+               a = angle / 360;
+               reply_angle = (angle - (360 * a))*100;  //現在の角度
+            } else {
+               a = angle / 360;
+               reply_angle = (360 + angle - (360 * a))*100;
+            }
+            
+            printf("angle = %f\r\n",reply_angle);
+            spi.reply(reply_angle);
+
+            /*printf("%d\r\n",debug);
+            debug++;
+            if(debug>100) debug=0;*/
+        }
+
+        if(data1 != old_data) {
+            if((flag1 == 1) && (data1 == 1)) {
+                servo1.pulsewidth_us(1000);
+                flag1=0;
+            } else if((flag1 == 0) && (data1 == 1)) {
+                servo1.pulsewidth_us(2050);
+                flag1=1;
+            }
+
+            if((flag2 == 1) && (data1 == 2)) {
+                servo2.pulsewidth_us(1000);
+                flag2=0;
+            } else if((flag2 == 0) && (data1 == 2)) {
+                servo2.pulsewidth_us(2050);
+                flag2=1;
+            }
+
+            if((flag3 == 1) && (data1 == 3)) {
+                servo3.pulsewidth_us(800);
+                flag3=0;
+            } else if((flag3 == 0) && (data1 == 3)) {
+                servo3.pulsewidth_us(1250);
+                flag3=1;
+            }
+
+            if((flag4 == 1) && (data1 == 4)) {
+                servo4.pulsewidth_us(1100);
+                flag4=0;
+            } else if((flag4 == 0) && (data1 == 4)) {
+                servo4.pulsewidth_us(2350);
+                flag4=1;
+            }
+
+            if((flag5 == 1) && (data1 == 5)) {
+                out = 1;
+                flag5=0;
+            } else if((flag5 == 0) && (data1 == 5)) {
+                out = 0;
+                flag5=1;
+            }
+        }
+
+        old_data = data1;
+
+    }
+}
\ No newline at end of file