Diff: main.cpp

Revision:

1:a78e5f6154d3

Parent:

0:926cd923a480

Child:

2:388988e5733a

--- a/main.cpp	Fri Jul 22 07:57:56 2016 +0000
+++ b/main.cpp	Mon Jul 25 06:38:27 2016 +0000
@@ -66,8 +66,17 @@
 float err[3]= {0};
 bool s0 = false;
 char dire;
+char c_for_get;
+char c_for_send;
+SPI spi(D11, D12, D13); // mosi, miso, sclk
+DigitalOut cs(D10);
 
+Serial pc(SERIAL_TX, SERIAL_RX);
 
+char *delim = " ";
+char *pch;
+float cha_L[3];
+   
 int main()
 {
 
@@ -144,7 +153,7 @@
             XL[2]=0+pi/3;
             break;
     }
-
+    
     printf("PWM Reset: [%.2f(%%) %.2f(%%) %.2f(%%)]\n",pwm_1.read()*100,pwm_2.read()*100,pwm_3.read()*100);
     printf("Initial Pose : [% .1f(cm) % .1f(cm) % .1f(deg)]\n", Current_X[0]*100, Current_X[1]*100 ,Current_X[2]*180/pi-60);
     printf("Desired Pose : [% .1f(cm) % .1f(cm) % .1f(deg)]\n", XL[0]*100, XL[1]*100 ,XL[2]*180/pi-60);
@@ -159,6 +168,32 @@
     }
 
     while(1) {
+        
+        cs = 0;
+        for (const char *p = "32\n" ; c_for_get = *p; p++)
+        {
+        spi.write (c_for_get);    
+        }       
+        wait(0.1);
+        float bb = spi.write (0x00);
+        char ddest[3];
+        sprintf(ddest,"%.1f \n", bb);
+        pc.printf(ddest);
+        cs = 1;//須將ddest矩陣拆開成三個不同的數
+        
+        int strtok_n = 0;
+        pch = strtok(ddest,delim);
+        while(pch != NULL)
+        {
+            cha_L[strtok_n] = atof(pch);
+            //printf("%s\n",pch);
+            pch = strtok(NULL, delim);   
+            strtok_n = strtok_n+1;
+        }
+        XL[0] = cha_L[0];
+        XL[1] = cha_L[1];
+        XL[2] = cha_L[2];
+        
         //
         myled = 0;
         X1[0] = Current_X[0];
@@ -233,6 +268,17 @@
         err[1] = Current_X[1]-XL[1];
         err[2] = Current_X[2]-XL[2];
         // END //
+        char send_array[6];
+        sprintf(send_array, "%.1f %.1f %.1f\n", Current_X[0], Current_X[1], Current_X[2]);
+        
+        cs = 0;
+        for (const char *p = send_array ; c = *p; p++) //尚未實測
+        {
+        spi.write (c);    
+        }
+        wait(0.1);
+        cs = 1;
+        
         if ( t.read_ms() > 200) { // print
 
             t.reset();
@@ -280,6 +326,6 @@
                         */
         }
 
-        wait_ms(10);// 會有什麼影響?
+        wait_ms(50);// 會有什麼影響?
     }
 }