Diff: spi_nucleo.h

Revision:

7:4f5cd0051db0

Parent:

6:bb86e2999be0

Parent:

4:ef6f18eda3e2

Child:

8:554e3d643b2d

--- a/spi_nucleo.h	Fri Apr 01 05:58:11 2016 +0000
+++ b/spi_nucleo.h	Fri Apr 01 06:15:24 2016 +0000
@@ -4,59 +4,80 @@
 #include <string.h>
 
 //SPI mbed
-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);
     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()) {
+        RR=0;
+        LR=0;
+        FM=from_mbed.read();
+        Xpm=FM>>13;
+        Ypm=(FM&0x40)>>6;
+        if(FM>>15==1) {
+            RR=1;
+        } //右旋回の関数へ
+        else if(FM>>14==1) {
+            LR=1;
+        } //左旋回の関数へ
+        else if(Xpm==1) { //コントローラーのＸ方向成分入力
+            speed_X=FM&0x1F80;
+            speed_X=speed_X>>7;
+            if(Ypm==0) speed_Y=FM&0x3F;
+            else {
+                speed_Y=FM&0x3F;
+                speed_Y=(-1)*speed_Y;
+            }
+        }
+        else {
+            speed_X=FM&0x1F80;
+            speed_X=speed_X>>7;
+            speed_X=(-1)*speed_X;
+            if(Ypm==0) speed_Y=FM&0x3F;
+            else {
+                speed_Y=FM&0x3F;
+                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);
     }
+}
+