ROBOSTEP_3rd_SHARE
/
PS3conOut2
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed
Fork of
PS3conOut
by 
ROBOSTEP_3rd_SHARE
Revision 5:6f6948b8056b, committed 2015-10-16
- Comitter:
- yuji8822
- Date:
- Fri Oct 16 01:21:12 2015 +0000
- Parent:
- 4:ee01e28db9d4
- Commit message:
- kkkk;
Changed in this revision
| User.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/User.cpp Thu Oct 15 12:30:50 2015 +0000
+++ b/User.cpp Fri Oct 16 01:21:12 2015 +0000
@@ -22,6 +22,7 @@
#define LSYcent 128
#define RSXcent 128
#define RSYcent 128
+#define M_MAX 1000
CAN can1(p9, p10);
CANMessage msg;
@@ -101,20 +102,34 @@
ButtonState = ((ps3report*)(data + 1))->ButtonState;
}
- motor1 = (M_MAX/180)*((LSX-128)/sqrt(2)+((LSY-128)/sqrt(2));
- motor2 = (M_MAX/180)*(-(LSX-128)/sqrt(2)+((LSY-128)/sqrt(2));
+
+ if(LSX<150 &&LSX>100 && LSY<150 && LSY>100){
+ LSX=128;
+ LSY=128;
+ }
+ /*
+ else if(LSX<150 && LSX>78 && LSY>=0 && LSY<22){
+ LSX=128;
+ LSY=0;
+ }
+ else if(LSX<150 && LSX>78 && LSY>233 && LSY<255){
+ LSX=128;
+ LSY=255;
+ }*/
+ motor1 = (M_MAX/180)*((LSX-128)/(2^(1/2)))+((128-LSY)/(2^(1/2)));
+ motor2 = (M_MAX/180)*(-(LSX-128)/(2^(1/2)))+((128-LSY)/(2^(1/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;
+ Send_data[0] = motor4 >>8;
+ Send_data[1] = motor4 &&255;
+ Send_data[2] = motor1 >>8;
+ Send_data[3] = motor1 &&255;
+ Send_data[4] = motor2 >>8;
+ Send_data[5] = motor2 &&255;
+ Send_data[6] = motor3 >>8;
+ Send_data[7] = motor3 &&255;
can1.write(CANMessage(1, &Send_data[0], 8)); //id=1 自動 id=23 手動
static int can1_counter = 0;
@@ -126,7 +141,9 @@
serial_counter ++;
- printf("%d,%d,%d,%d,%f\n",Send_data[0],Send_data[1],RSX-128,128-RSY,theta2-M_PI/4);
+ printf("motor1=%d,motor2=%d,motor3=%d,motor4=%d,x=%d,y=%d\n\r",motor1,motor2,motor3,motor4,LSX-128,128-LSY);
+ //printf("motor1=%d,motor2=%d,motor3=%d,motor4=%d\n\r",motor1,motor2,motor3,motor4);
+ //printf("x=%d,y=%d\n\r",LSX-128,128-LSY);
#ifdef DEBUG_MODE
if(serial_counter > REFRESHRATE_PRINTF){