control YOZAKURA arm
Dependencies: Dynamixel SerialHalfDuplex mbed
Fork of YOZAKURA_ARM_USB_0427 by
Diff: main.cpp
- Revision:
- 2:f0efe1d10f8a
- Parent:
- 1:bf4d524625f5
- Child:
- 3:6a932d0aac7d
--- a/main.cpp Fri Apr 24 17:14:48 2015 +0000 +++ b/main.cpp Sat Apr 25 03:36:16 2015 +0000 @@ -63,12 +63,8 @@ switch(pitch_mode){ case 0: break; - case 1: pitch_goal-=5; - if(pitch_goal>pitch_MAX) pitch_goal=pitch_MAX; - break; - case 2: pitch_goal+=5; - if(pitch_goal<pitch_min) pitch_goal=pitch_min; - break; + case 1: pitch_goal-=5; break; + case 2: pitch_goal+=5; break; } if(pitch_goal>pitch_MAX) pitch_goal=pitch_MAX; if(pitch_goal<pitch_min) pitch_goal=pitch_min; @@ -103,7 +99,7 @@ pitch.SetTorqueLimit(0); yaw.SetTorqueLimit(0); - wait_ms(10); + wait(0.1); // RelaySwitch = 1; linear.SetTorqueLimit(1); @@ -130,7 +126,7 @@ #define D6T_addr 0x14 #define D6T_cmd 0x4c -#define THERMO_DEBUG 1 +#define THERMO_DEBUG 0 void GetThermo(char* data) { char con[10]; @@ -165,6 +161,13 @@ sprintf(con,"%3.1f ",dt1[i]); strcat(data,con); } + if(THERMO_DEBUG){ + printf("Thermal_Sensor 1"); + for(i=0;i<16;i++){ + if(i%4==0) printf("\n"); + printf("%3.1f ",dt1[i]); + } printf("\n"); + } d_PTAT1 = 0.1 * PTAT1; wait(0.1); /*MEMS1*/ @@ -191,13 +194,18 @@ } for(i=0;i<16;i++){ dt2[i] = 0.1 * datr2[i]; - if(i%4==0) printf("\n"); - printf("%3.1f ",dt2[i]); } for(int i=0;i<16;i++){ sprintf(con,"%3.1f ",dt2[i]); strcat(data,con); } + if(THERMO_DEBUG){ + printf("Thermal_Sensor 2"); + for(i=0;i<16;i++){ + if(i%4==0) printf("\n"); + printf("%3.1f ",dt2[i]); + } printf("\n"); + } d_PTAT2 = 0.1 * PTAT2; wait(0.1); /*MEMS2*/ @@ -213,7 +221,7 @@ AnalogIn ain(p20); -#define CO2_DEBUG 1 +#define CO2_DEBUG 0 float GetCO2() { float v; //生データ:電圧 @@ -248,16 +256,16 @@ Read_data=pc.getc(); wait_ms(1); - Joy_mode = Read_data & 0x03; //ジョイスティックのモード判定 + Joy_mode = Read_data & 0xc0; Joy_mode = Joy_mode >> 6; //ジョイスティックのモード判定 switch(Joy_mode){ case 0: //通常モード myled1=0; myled2=0; myled3=0; myled4=0; /*--Dynamixel:begin---------------------------------------------------------------*/ //Dynamixelへの命令を判定 - linear_mode = Read_data & 0x0c; linear_mode = linear_mode >> 2; - pitch_mode = Read_data & 0x30; pitch_mode = pitch_mode >> 4; - yaw_mode = Read_data & 0xc0; yaw_mode = yaw_mode >> 6; + linear_mode = Read_data & 0x30; linear_mode = linear_mode >> 4; + pitch_mode = Read_data & 0x0c; pitch_mode = pitch_mode >> 2; + yaw_mode = Read_data & 0x03; //現在の角度・電圧・電流を取得 myled1=1;