Diff: spi_nucleo.h

Revision:

4:ef6f18eda3e2

Parent:

3:6ab4e8e3aa02

Child:

7:4f5cd0051db0

--- a/spi_nucleo.h	Tue Mar 29 12:36:50 2016 +0000
+++ b/spi_nucleo.h	Thu Mar 31 00:39:06 2016 +0000
@@ -4,25 +4,29 @@
 #include <string.h>
 
 //SPI mbed
-int FM=0;
-bool RR;
-bool LR;
-char speed_X;
-char speed_Y;
+bool RR=0;
+bool LR=0;
+int speed_X;
+int speed_Y;
 bool Xpm=0;
 bool Ypm=0;
-
 //SPI NUCLEO
 void spi_mbed();
 void spi_nucleo();
 void getGyro();
 void spiInit();
-typedef unsigned short u16;
+typedef unsigned u16;
+u16 FM;
 long int angle;
 double now_angle;
 Serial pc(USBTX,USBRX);
-#endif
-void spiInit(){
+void spiInit();
+void spi_mbed();
+void getGyro();
+#endif 
+
+void spiInit()
+{
     from_mbed.format(16,3);
     from_gyro.format(16,3);
     from_mbed.frequency(1000000);
@@ -30,45 +34,50 @@
 }
 
 void spi_mbed()
-{
+{       
     if(from_mbed.receive()) {
+        RR=0;
+        LR=0;
         FM=from_mbed.read();
-        RR=FM>>15;
-        LR=FM>>14;
         Xpm=FM>>13;
-        Ypm=FM>>6;
-        if(RR==1) {
+        Ypm=(FM&0x40)>>6;
+        if(FM>>15==1) {
+            RR=1;
         } //右旋回の関数へ
-        else if(LR==1) {
+        else if(FM>>14==1) {
+            LR=1;
         } //左旋回の関数へ
         else if(Xpm==1) { //コントローラーのＸ方向成分入力
             speed_X=FM&0x1F80;
             speed_X=speed_X>>7;
-            if(Ypm==1) speed_Y=FM&0x3F;
+            if(Ypm==0) speed_Y=FM&0x3F;
             else {
                 speed_Y=FM&0x3F;
-                speed_Y=speed_Y*(-1);
+                speed_Y=(-1)*speed_Y;
             }
         }
         else {
             speed_X=FM&0x1F80;
             speed_X=speed_X>>7;
             speed_X=(-1)*speed_X;
-            if(Ypm==1) speed_Y=FM&0x3F;
+            if(Ypm==0) speed_Y=FM&0x3F;
             else {
                 speed_Y=FM&0x3F;
-                speed_Y=speed_Y*(-1);
+                speed_Y=(-1)*speed_Y;
             }
         }
     }
 }
-     
-void getGyro(){
-    u16 data;
-    if(from_gyro.receive()){
+
+void getGyro()
+{
+    int data;
+    if(from_gyro.receive()) {
         data = from_gyro.read();
-        if(data >> 15) angle = -(data & 0x7fff);
+    if(data >> 15) angle = -(data & 0x7fff);
         else angle = data & 0x7fff;
         now_angle = (double)angle / 10;
-        }
+//        pc.printf("%d\r\n",data);
     }
+}
+