Diff: main.cpp

Revision:

1:be4238d819bb

Parent:

0:9969c88facec

--- a/main.cpp	Sun Feb 24 13:34:41 2019 +0000
+++ b/main.cpp	Sat Mar 02 01:11:16 2019 +0000
@@ -1,26 +1,27 @@
 #include "mbed.h"
 
-int val = 0;
-
 Serial pc( USBTX, USBRX);
 CAN can( PA_11, PA_12);
 CANMessage msg;
 DigitalOut led (PA_5);
 DigitalIn start(PB_13);
 
+
+typedef union{
+    float val;
+    char binary[4];
+}transFloat;
+
+//P,I,D,YPOWER,ZPOWERの順
+const float parameter[4][5] = {
+    {0.0006, 0.001, 0.0, 0.25, 0.25},
+    {0.0006, 0.001, 0.0, 0.30, 0.25},
+    {0.0006, 0.001, 0.0, 0.25, 0.25},
+    {0.0006, 0.001, 0.0, 0.25, 0.25}};
+    
+
 bool finishFlags[4] = {false, false, false, false};
-/*
-void inter(){
-    can.read(msg);
-    if(msg.id >= 4 && msg.id <= 7){
-        if(msg.data[0] == 0){
-            finishFlags[msg.id - 4] = false;
-        }else{
-            finishFlags[msg.id - 4] = true;
-        }
-    }
-}
-*/
+
 void checkFlags(int id){
     char data = 10;
     if(!finishFlags[id]){
@@ -30,11 +31,11 @@
                 break;
             }
         }
-        if(msg.id == id+4){
+        if(msg.id == id+14){
             if(msg.data[0] == 0){
-                finishFlags[msg.id - 4] = false;
+                finishFlags[msg.id - 14] = false;
             }else{
-                finishFlags[msg.id - 4] = true;
+                finishFlags[msg.id - 14] = true;
             }
         }
     }
@@ -44,19 +45,7 @@
 void waitReach(int id){
     while(1){
         checkFlags(id);
-        pc.printf("%d \t",id);
-        
         pc.printf("%d \n",finishFlags[id]);
-        //bool flag = true;
-        /*
-        for(int i=0;i<4;i++){
-            flag = flag & finishFlags[i];
-            pc.printf("%d \t",finishFlags[i]);
-        }
-        */
-        //pc.printf("\n");
-        //pc.printf("%d \n",start.read());
-       // pc.printf("%d \n",finishFlags[id]);
         if(finishFlags[id] == true){
             break;
         }
@@ -67,7 +56,6 @@
     for(int i=0;i<4;i++){
         waitReach(i);
     }
-    //pc.printf(" \n");
 }
 
 void step(int id, int y, bool z){
@@ -81,8 +69,10 @@
     wait(1);
     start.mode(PullUp);
     can.frequency(100000);
-    //can.attach(&inter);
     led.write(0);
+    
+    
+    
     int count = 0;
     while(1){
         if(start.read() == 0){
@@ -103,14 +93,28 @@
     while (true) {
         /*
         //led.write(1);
-        step(2,10,1);
-        allWaitReach();
+        step(0,20,1);
+        step(1,200,1);
+        step(2,20,0);
+        step(3,200,0);
+        waitReach(0);
+        waitReach(1);
+        waitReach(2);
+        waitReach(3);
         //pc.printf("ok \n");
         //wait(3.0);
-        step(2,200,0);
-        allWaitReach();
+        step(0,200,0);
+        step(1,20,0);
+        step(2,200,1);
+        step(3,20,1);
+        
+        waitReach(0);
+        waitReach(1);
+        waitReach(2);
+        waitReach(3);
+        */
         //pc.printf("ok \n");
-        */
+        
         
         step(1,200,0);
         step(2,200,0);
@@ -121,7 +125,10 @@
         step(1,200,1);
         step(2,200,1);
         step(3,10,0);
-        allWaitReach();
+        waitReach(0);
+        waitReach(1);
+        waitReach(2);
+        waitReach(3);
         
         step(1,10,1);
         step(2,10,1);
@@ -137,7 +144,10 @@
         step(1,10,0);
         step(2,10,0);
         step(3,200,1);
-        allWaitReach();
+        waitReach(0);
+        waitReach(1);
+        waitReach(2);
+        waitReach(3);
         
         step(0,10,1);
         step(3,10,1);