Diff: spi_nucleo.h

Revision:

3:6ab4e8e3aa02

Parent:

2:bed6b204e64c

Child:

4:ef6f18eda3e2

diff -r bed6b204e64c -r 6ab4e8e3aa02 spi_nucleo.h
--- a/spi_nucleo.h	Fri Mar 25 04:21:52 2016 +0000
+++ b/spi_nucleo.h	Tue Mar 29 12:36:50 2016 +0000
@@ -4,10 +4,13 @@
 #include <string.h>
 
 //SPI mbed
+int FM=0;
 bool RR;
 bool LR;
 char speed_X;
 char speed_Y;
+bool Xpm=0;
+bool Ypm=0;
 
 //SPI NUCLEO
 void spi_mbed();
@@ -26,31 +29,40 @@
     from_gyro.frequency(1000000);
 }
 
-void spi_mbed(){
-    u16 FM;
-    if(from_mbed.receive()){
-    FM=from_mbed.read();
-    RR=FM>>15;        
-    LR=FM>>14;
-        if(RR==1){
-            } //右旋回の関数へ
-        else if(LR==1){
-            } //左旋回の関数へ
-        else if(FM>>13==1){ //コントローラーのＸ方向成分入力
-            speed_X=FM>>7;
-            speed_X=FM-128;
-            if(FM>>6==1) speed_Y=FM-128;
-            else speed_Y=FM;
-                }
-        else{
-            speed_X=FM>>7;
-            speed_Y=(-1)*speed_Y;
-            if(FM>>6==1) speed_Y=FM-128;
-            else speed_Y=FM;
+void spi_mbed()
+{
+    if(from_mbed.receive()) {
+        FM=from_mbed.read();
+        RR=FM>>15;
+        LR=FM>>14;
+        Xpm=FM>>13;
+        Ypm=FM>>6;
+        if(RR==1) {
+        } //右旋回の関数へ
+        else if(LR==1) {
+        } //左旋回の関数へ
+        else if(Xpm==1) { //コントローラーのＸ方向成分入力
+            speed_X=FM&0x1F80;
+            speed_X=speed_X>>7;
+            if(Ypm==1) speed_Y=FM&0x3F;
+            else {
+                speed_Y=FM&0x3F;
+                speed_Y=speed_Y*(-1);
+            }
+        }
+        else {
+            speed_X=FM&0x1F80;
+            speed_X=speed_X>>7;
+            speed_X=(-1)*speed_X;
+            if(Ypm==1) speed_Y=FM&0x3F;
+            else {
+                speed_Y=FM&0x3F;
+                speed_Y=speed_Y*(-1);
             }
         }
     }
-        
+}
+     
 void getGyro(){
     u16 data;
     if(from_gyro.receive()){