YOZAKURA ARM program, usb serial
Dependencies: Dynamixel SerialHalfDuplex mbed
Diff: main.cpp
- Revision:
- 4:d9ea010641ab
- Parent:
- 3:6a932d0aac7d
- Child:
- 5:581b55deaeb1
diff -r 6a932d0aac7d -r d9ea010641ab main.cpp --- a/main.cpp Sat Apr 25 06:08:18 2015 +0000 +++ b/main.cpp Sat Apr 25 06:42:51 2015 +0000 @@ -12,6 +12,8 @@ #include "AX12.h" #include "MX28.h" +#define DYNA_DEBUG 1 + DigitalOut low(p16); DigitalOut RelaySwitch(p18); AX12 linear (p13, p14, 0, 1000000); //直動Dynamixel @@ -27,6 +29,7 @@ int yaw_min = 360; int yaw_MAX = 360; float yaw_Speed=0.2; int yaw_Init = 0; //MultiTurnモード void Dyna_init() { + if(DYNA_DEBUG) printf("\nDyna_init\n"); // low = 0; RelaySwitch = 1; linear.SetTorqueLimit(0); pitch.SetTorqueLimit(0); @@ -41,6 +44,7 @@ } void Dyna_GetData(char* data) { + if(DYNA_DEBUG) printf("\nDyna_SetData\n"); float lP, lV, pP, pC, yP, yC; lP = linear.GetPosition(); lV = linear.GetVolts(); @@ -52,6 +56,7 @@ } void Dyna_SetGoal(int linear_mode, int pitch_mode, int yaw_mode) { + if(DYNA_DEBUG) printf("\nDyna_SetGoal\n"); switch(linear_mode){ case 0: break; case 1: linear.SetTorqueLimit(1); linear_goal-=2; break; @@ -81,6 +86,7 @@ } void Dyna_home_position() { + if(DYNA_DEBUG) printf("\nDyna_home_position\n"); linear.SetTorqueLimit(1); pitch.SetTorqueLimit(1); yaw.SetTorqueLimit(1); @@ -94,12 +100,13 @@ } void Dyna_reset() { + if(DYNA_DEBUG) printf("\nDyna_reset\n"); // RelaySwitch = 0; linear.SetTorqueLimit(0); pitch.SetTorqueLimit(0); yaw.SetTorqueLimit(0); - wait(0.1); + wait(1); // RelaySwitch = 1; linear.SetTorqueLimit(1); @@ -108,6 +115,7 @@ } void Dyna_end() { + if(DYNA_DEBUG) printf("\nDyna_end\n"); // RelaySwitch = 0; linear.SetTorqueLimit(0); pitch.SetTorqueLimit(0); @@ -251,6 +259,7 @@ /*--------------- MAIN ROOP ----------------*/ +#define DEBUG 1 int main() { char Dyna_data[50]; @@ -268,15 +277,16 @@ Read_data=pc.getc(); wait_ms(1); Joy_mode = Read_data & 0xc0; Joy_mode = Joy_mode >> 6; //ジョイスティックのモード判定 + if(DEBUG) printf("Joy_mode : %d\n",Joy_mode); switch(Joy_mode){ case 0: //通常モード myled1=0; myled2=0; myled3=0; myled4=0; /*--Dynamixel:begin---------------------------------------------------------------*/ //Dynamixelへの命令を判定 - 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; + linear_mode = Read_data & 0x30; linear_mode = linear_mode >> 4; if(DEBUG) printf("linear_mode : %d\n",linear_mode); + pitch_mode = Read_data & 0x0c; pitch_mode = pitch_mode >> 2; if(DEBUG) printf("pitch_mode : %d\n",pitch_mode); + yaw_mode = Read_data & 0x03; if(DEBUG) printf("yaw_mode : %d\n",yaw_mode); //現在の角度・電圧・電流を取得 myled1=1; @@ -303,7 +313,7 @@ //値を送信 myled4=1; - sprintf(Send_data,"%s%s%f",Dyna_data,Thermo_data,CO2_data); + sprintf(Send_data,"%s%s%4.1f",Dyna_data,Thermo_data,CO2_data); printf("Send_data : %s\n",Send_data); myled4=0; break; @@ -318,4 +328,10 @@ myled1=1; myled2=1; myled3=1; myled4=1; Dyna_end(); break; } } -} \ No newline at end of file +} + +/* CoolTermのSend stringを使う場合 Hex : 16進数で値を送信可能 なので + home_position 40 + reset 80 + end c0 +*/ \ No newline at end of file