![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
linear -> MX28
Dependencies: Dynamixel SerialHalfDuplex mbed
main.cpp
- Committer:
- yusuke_kyo
- Date:
- 2015-08-18
- Revision:
- 0:3b39104fae2f
File content as of revision 0:3b39104fae2f:
/*YOZAKURAのアームコード シリアル通信で送るデータ S_Data={linearD[4],pitchD[4],yawD[4],ThermoD1[16],ThermoD2[16],CO2[4]} 計48bytes シリアル通信で受け取るデータ R_Data={linear_ref[2bit],pitch_ref[2bit],yaw_ref[2bit]m,ode[2bit]} 計1byte */ #include "mbed.h" Serial pc(USBTX, USBRX); // tx, rx DigitalOut myled1(LED1); DigitalOut myled2(LED2); DigitalOut myled3(LED3); DigitalOut myled4(LED4); struct ArmPacketBits { unsigned int mode : 2; unsigned int linear : 2; unsigned int pitch : 2; unsigned int yaw : 2; }; union ArmPacket { struct ArmPacketBits b; unsigned char as_byte; }; /*--Dynamixel:begin-----------------------------------------------------------------------------------------*/ #include "AX12.h" #include "MX28.h" #define DYNA_DEBUG 1 DigitalOut low(p16); DigitalOut RelaySwitch(p18); MX28 linear (p13, p14, 0, 1000000); //直動Dynamixel MX28 pitch (p13, p14, 1, 1000000); //ピッチDynamixel MX28 yaw (p13, p14, 2, 1000000); //ヨーDynamixel int linear_goal, pitch_goal, yaw_goal; //最小値,最大値,角速度,初期値を指定[unit:degree] //MX:MultiTurnモードでは-2520°~2520°(SetGoalの書き換え不要) int linear_min = 170; int linear_MAX = 280; float linear_Speed=0.1; int linear_Init = linear_MAX; int pitch_min = 172; int pitch_MAX = 334; float pitch_Speed=0.2; int pitch_Init = pitch_MAX; int yaw_min = 360; int yaw_MAX = 360; float yaw_Speed=0.4; int yaw_Init = 0; //MultiTurnモード void Dyna_init() { if(DYNA_DEBUG) printf("\nDyna_init\n"); // while(!s.writeable()){ // if(DYNA_DEBUG) printf("Dynamixel is disconnected.\n"); // wait(0.5); // } // low = 0; RelaySwitch = 1; linear.SetTorqueLimit(0); pitch.SetTorqueLimit(0); yaw.SetTorqueLimit(0); linear.SetCWLimit(linear_min); linear.SetCCWLimit(linear_MAX); linear.SetCRSpeed(linear_Speed); linear_goal=linear_Init; pitch.SetCWLimit(pitch_min); pitch.SetCCWLimit(pitch_MAX); pitch.SetCRSpeed(pitch_Speed); pitch_goal=pitch_Init; yaw.SetCWLimit(yaw_min); yaw.SetCCWLimit(yaw_MAX); yaw.SetCRSpeed(yaw_Speed); yaw_goal=yaw_Init; wait(1); } 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(); pP = pitch.GetPosition(); pC = pitch.GetCurrent(); yP = yaw.GetPosition(); yC = yaw.GetCurrent(); linear_goal=lP; pitch_goal=pP; yaw_goal=yP; //現在角度を目標値に設定 sprintf(data,"%4.1f %4.1f %4.1f %4.1f %4.1f %4.1f ",lP,pP,yP,lV,pC,yC); } void Dyna_SetGoal(int mode) { if(DYNA_DEBUG) printf("\nDyna_SetGoal\n"); switch(mode){ case 97:linear.SetTorqueLimit(1); linear.TorqueEnable(1); linear_goal-=10; if(linear_goal<linear_min) linear_goal=linear_min; if(linear_goal>linear_MAX) linear_goal=linear_MAX; linear.SetGoal(linear_goal); break; case 102: linear.SetTorqueLimit(1); linear.TorqueEnable(1); linear_goal+=20; if(linear_goal<linear_min) linear_goal=linear_min; if(linear_goal>linear_MAX) linear_goal=linear_MAX; linear.SetGoal(linear_goal); break; case 30:pitch.SetTorqueLimit(1); pitch_goal-=20; if(pitch_goal<pitch_min) pitch_goal=pitch_min; if(pitch_goal>pitch_MAX) pitch_goal=pitch_MAX; pitch.SetGoal(pitch_goal); break; case 31:pitch.SetTorqueLimit(1); pitch_goal+=5; if(pitch_goal<pitch_min) pitch_goal=pitch_min; if(pitch_goal>pitch_MAX) pitch_goal=pitch_MAX; pitch.SetGoal(pitch_goal); break; case 28:yaw_goal-=10; if(yaw_goal<-2520) yaw_goal=-2520; if(yaw_goal>2520) yaw_goal=2520; yaw.SetGoal(yaw_goal); break; case 29:yaw_goal+=10; if(yaw_goal<-2520) yaw_goal=-2520; if(yaw_goal>2520) yaw_goal=2520; yaw.SetGoal(yaw_goal); break; } } void Dyna_home_position() { if(DYNA_DEBUG) printf("\nDyna_home_position\n"); linear.SetTorqueLimit(1); pitch.SetTorqueLimit(1); yaw.SetTorqueLimit(1); float lp; linear.SetGoal(linear_Init); lp=linear.GetPosition(); if(lp > linear_MAX - 30) { //ある程度縮んだら pitch.SetGoal(pitch_Init); yaw.SetGoal(yaw_Init); } } void Dyna_reset() { if(DYNA_DEBUG) printf("\nDyna_reset\n"); // RelaySwitch = 0; linear.SetTorqueLimit(0); pitch.SetTorqueLimit(0); yaw.SetTorqueLimit(0); wait(1); // RelaySwitch = 1; linear.SetTorqueLimit(1); pitch.SetTorqueLimit(1); yaw.SetTorqueLimit(1); } void Dyna_end() { if(DYNA_DEBUG) printf("\nDyna_end\n"); // RelaySwitch = 0; linear.SetTorqueLimit(0); pitch.SetTorqueLimit(0); yaw.SetTorqueLimit(0); } /*--Dynamixel:end-----------------------------------------------------------------------------------------*/ /*--------------- MAIN LOOP ----------------*/ #define DEBUG 1 int main() { myled1=1; myled2=1; myled3=1; myled4=1; pc.baud(38400); char Dyna_data[50]; myled1=1; myled2=1; myled3=0; myled4=0; Dyna_init(); Dyna_home_position(); wait(2); Dyna_home_position(); wait(2); myled1=0; myled2=0; myled3=1; myled4=0; union ArmPacket Read_data; int mode; while(1) { myled1=0; myled2=0; myled3=0; myled4=0; /*--Dynamixel:begin---------------------------------------------------------------*/ //Dynamixelへの命令を判定 mode=pc.getc(); printf("mode : %d\n",mode); //目標角度を変更 myled2=1; Dyna_SetGoal(mode); myled2=0; /*--Dynamixel:end------------------------------------------------------------------*/ //現在の角度・電圧・電流を取得 myled1=1; Dyna_GetData(Dyna_data); myled1=0; //値を送信 myled4=1; pc.printf("%s\n",Dyna_data) myled4=0; } } /* CoolTermのSend stringを使う場合 Hex : 16進数で値を送信可能 なので home_position 40 reset 80 end c0 */