Control YOZAKURA arm vir USB
Dependencies: Dynamixel MEMS_Thermal_Sensor SerialHalfDuplex mbed
Diff: main.cpp
- Revision:
- 3:6dbba9951f9e
- Parent:
- 2:3a4f83edb424
- Child:
- 4:dd8c3543adc7
diff -r 3a4f83edb424 -r 6dbba9951f9e main.cpp --- a/main.cpp Wed Apr 15 15:03:49 2015 +0000 +++ b/main.cpp Thu Apr 16 02:18:04 2015 +0000 @@ -102,7 +102,8 @@ Dyna_init(); - char S_data[200], R_data[2]; + char S_data[200]; + int R_data; int Joy_mode, linear_mode, pitch_mode, yaw_mode; while(1) { @@ -110,8 +111,7 @@ // sock.receiveFrom(echo_server, R_data, sizeof(R_data)); //オペステからデータを取得 R_data=pc.getc(); wait_ms(1); - int J = atoi(R_data); - Joy_mode = J & 0x03; //ジョイスティックのモード判定 + Joy_mode = R_data & 0x03; //ジョイスティックのモード判定 switch(Joy_mode){ case 0: //通常モード @@ -124,9 +124,9 @@ myled1=0; //Dynamixelへの命令を判定 - linear_mode = R_data[0] & 0x0c; linear_mode = linear_mode >> 2; - pitch_mode = R_data[0] & 0x30; pitch_mode = linear_mode >> 4; - yaw_mode = R_data[0] & 0xc0; yaw_mode = linear_mode >> 6; + linear_mode = R_data & 0x0c; linear_mode = linear_mode >> 2; + pitch_mode = R_data & 0x30; pitch_mode = linear_mode >> 4; + yaw_mode = R_data & 0xc0; yaw_mode = linear_mode >> 6; //目標角度を変更 myled2=1; @@ -175,7 +175,7 @@ ThD2[8],ThD2[9],ThD2[10],ThD2[11],ThD2[12],ThD2[13],ThD2[14],ThD2[15]); sprintf(S_data, "%f %f %f %f %f %f %s %f",lP,lV,pP,pC,yP,yC,Thermo_data,CO2_data); // sock.sendTo(echo_server, S_data, sizeof(S_data)); - pc.printf("%s\n",S_data) + pc.printf("%s\n",S_data); myled3=0; break;