Diff: User.cpp

Revision:

4:ee01e28db9d4

Parent:

3:4d19e16fdf4c

Child:

5:6f6948b8056b

--- a/User.cpp	Wed Oct 14 12:24:17 2015 +0000
+++ b/User.cpp	Thu Oct 15 12:30:50 2015 +0000
@@ -58,6 +58,10 @@
 int can_counter = 0;
 double  avr_accel_x, avr_accel_y;
 
+int motor1=0;
+int motor2=0;
+int motor3=0;
+int motor4=0;
 
 //
 u8 RSX,RSY,LSX,LSY,BSU,BSL;
@@ -76,7 +80,7 @@
                 //PS3conSTATE--->value
                 u16 ButtonState;
                 char Send_data[8] = {};
-                u16 spi_send_data[10] = {};
+                //u16 spi_send_data[10] = {};
                 if(n==0){//有線Ps3USB.cpp
                     RSX = ((ps3report*)data)->RightStickX;
                     RSY = ((ps3report*)data)->RightStickY;
@@ -96,15 +100,22 @@
                     //ボタンの処理
                     ButtonState =  ((ps3report*)(data + 1))->ButtonState;
                 }
-                Send_data[0] = RSX;             //MAX 1000 MIN -1000    MD MAX 4096 MIN -4096
-                Send_data[1] = RSY;
-                Send_data[2] = LSX;
-                Send_data[3] = LSY;
-                Send_data[4] = BSU;
-                Send_data[5] = BSL;
-                Send_data[6] =  ButtonState       & 0xFF;
-                Send_data[7] = (ButtonState >> 8) & 0xFF;
-                can1.write(CANMessage(23, &Send_data[0], 8));       //id=1 自動　id=23 手動
+                
+                motor1 = (M_MAX/180)*((LSX-128)/sqrt(2)+((LSY-128)/sqrt(2));
+                motor2 = (M_MAX/180)*(-(LSX-128)/sqrt(2)+((LSY-128)/sqrt(2));
+                motor3 = motor1*-1;
+                motor4 = motor2*-1;
+                
+                //Send_data[0] = RSX;             //MAX 1000 MIN -1000    MD MAX 4096 MIN -4096
+                Send_data[0] = motor1 >>8;
+                Send_data[1] = motor1 &&255;
+                Send_data[2] = motor2 >>8;
+                Send_data[3] = motor2 &&255;
+                send_data[4] = motor3 >>8;
+                Send_data[5] = motor3 &&255;
+                Send_data[6] = motor4 >>8;
+                Send_data[7] = motor4 &&255;
+                can1.write(CANMessage(1, &Send_data[0], 8));       //id=1 自動　id=23 手動
                 
                 static int can1_counter = 0;
                 can1_counter ++;
@@ -115,6 +126,7 @@
                                             
                 serial_counter ++;
                 
+                printf("%d,%d,%d,%d,%f\n",Send_data[0],Send_data[1],RSX-128,128-RSY,theta2-M_PI/4);
                
 #ifdef DEBUG_MODE                
                 if(serial_counter > REFRESHRATE_PRINTF){