雄介 太田 / Mbed 2 deprecated YOZAKURA_ARM_USB_Keyboard_0818

linear -> MX28

Dependencies:   Dynamixel SerialHalfDuplex mbed

main.cpp@0:3b39104fae2f, 2015-08-18 (annotated)

Committer:
yusuke_kyo
Date:
Tue Aug 18 11:43:26 2015 +0000
Revision:
0:3b39104fae2f
linear -> MX28

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yusuke_kyo 0:3b39104fae2f 1 /*YOZAKURAのアームコード
yusuke_kyo 0:3b39104fae2f 2 シリアル通信で送るデータ S_Data={linearD[4],pitchD[4],yawD[4],ThermoD1[16],ThermoD2[16],CO2[4]} 計48bytes
yusuke_kyo 0:3b39104fae2f 3 シリアル通信で受け取るデータ　R_Data=｛linear_ref[2bit],pitch_ref[2bit],yaw_ref[2bit]m,ode[2bit]} 計1byte
yusuke_kyo 0:3b39104fae2f 4 */
yusuke_kyo 0:3b39104fae2f 5
yusuke_kyo 0:3b39104fae2f 6 #include "mbed.h"
yusuke_kyo 0:3b39104fae2f 7
yusuke_kyo 0:3b39104fae2f 8 Serial pc(USBTX, USBRX); // tx, rx
yusuke_kyo 0:3b39104fae2f 9 DigitalOut myled1(LED1); DigitalOut myled2(LED2); DigitalOut myled3(LED3); DigitalOut myled4(LED4);
yusuke_kyo 0:3b39104fae2f 10
yusuke_kyo 0:3b39104fae2f 11 struct ArmPacketBits {
yusuke_kyo 0:3b39104fae2f 12 unsigned int mode : 2;
yusuke_kyo 0:3b39104fae2f 13 unsigned int linear : 2;
yusuke_kyo 0:3b39104fae2f 14 unsigned int pitch : 2;
yusuke_kyo 0:3b39104fae2f 15 unsigned int yaw : 2;
yusuke_kyo 0:3b39104fae2f 16 };
yusuke_kyo 0:3b39104fae2f 17
yusuke_kyo 0:3b39104fae2f 18 union ArmPacket {
yusuke_kyo 0:3b39104fae2f 19 struct ArmPacketBits b;
yusuke_kyo 0:3b39104fae2f 20 unsigned char as_byte;
yusuke_kyo 0:3b39104fae2f 21 };
yusuke_kyo 0:3b39104fae2f 22
yusuke_kyo 0:3b39104fae2f 23 /*--Dynamixel:begin-----------------------------------------------------------------------------------------*/
yusuke_kyo 0:3b39104fae2f 24 #include "AX12.h"
yusuke_kyo 0:3b39104fae2f 25 #include "MX28.h"
yusuke_kyo 0:3b39104fae2f 26
yusuke_kyo 0:3b39104fae2f 27 #define DYNA_DEBUG 1
yusuke_kyo 0:3b39104fae2f 28
yusuke_kyo 0:3b39104fae2f 29 DigitalOut low(p16); DigitalOut RelaySwitch(p18);
yusuke_kyo 0:3b39104fae2f 30
yusuke_kyo 0:3b39104fae2f 31 MX28 linear (p13, p14, 0, 1000000); //直動Dynamixel
yusuke_kyo 0:3b39104fae2f 32 MX28 pitch (p13, p14, 1, 1000000); //ピッチDynamixel
yusuke_kyo 0:3b39104fae2f 33 MX28 yaw (p13, p14, 2, 1000000); //ヨーDynamixel
yusuke_kyo 0:3b39104fae2f 34
yusuke_kyo 0:3b39104fae2f 35 int linear_goal, pitch_goal, yaw_goal;
yusuke_kyo 0:3b39104fae2f 36
yusuke_kyo 0:3b39104fae2f 37 //最小値，最大値，角速度，初期値を指定[unit:degree]
yusuke_kyo 0:3b39104fae2f 38 //MX:MultiTurnモードでは-2520°～2520°(SetGoalの書き換え不要)
yusuke_kyo 0:3b39104fae2f 39 int linear_min = 170; int linear_MAX = 280; float linear_Speed=0.1; int linear_Init = linear_MAX;
yusuke_kyo 0:3b39104fae2f 40 int pitch_min = 172; int pitch_MAX = 334; float pitch_Speed=0.2; int pitch_Init = pitch_MAX;
yusuke_kyo 0:3b39104fae2f 41 int yaw_min = 360; int yaw_MAX = 360; float yaw_Speed=0.4; int yaw_Init = 0; //MultiTurnモード
yusuke_kyo 0:3b39104fae2f 42
yusuke_kyo 0:3b39104fae2f 43 void Dyna_init() {
yusuke_kyo 0:3b39104fae2f 44 if(DYNA_DEBUG) printf("\nDyna_init\n");
yusuke_kyo 0:3b39104fae2f 45
yusuke_kyo 0:3b39104fae2f 46 // while(!s.writeable()){
yusuke_kyo 0:3b39104fae2f 47 // if(DYNA_DEBUG) printf("Dynamixel is disconnected.\n");
yusuke_kyo 0:3b39104fae2f 48 // wait(0.5);
yusuke_kyo 0:3b39104fae2f 49 // }
yusuke_kyo 0:3b39104fae2f 50
yusuke_kyo 0:3b39104fae2f 51 // low = 0; RelaySwitch = 1;
yusuke_kyo 0:3b39104fae2f 52 linear.SetTorqueLimit(0);
yusuke_kyo 0:3b39104fae2f 53 pitch.SetTorqueLimit(0);
yusuke_kyo 0:3b39104fae2f 54 yaw.SetTorqueLimit(0);
yusuke_kyo 0:3b39104fae2f 55 linear.SetCWLimit(linear_min); linear.SetCCWLimit(linear_MAX);
yusuke_kyo 0:3b39104fae2f 56 linear.SetCRSpeed(linear_Speed); linear_goal=linear_Init;
yusuke_kyo 0:3b39104fae2f 57 pitch.SetCWLimit(pitch_min); pitch.SetCCWLimit(pitch_MAX);
yusuke_kyo 0:3b39104fae2f 58 pitch.SetCRSpeed(pitch_Speed); pitch_goal=pitch_Init;
yusuke_kyo 0:3b39104fae2f 59 yaw.SetCWLimit(yaw_min); yaw.SetCCWLimit(yaw_MAX);
yusuke_kyo 0:3b39104fae2f 60 yaw.SetCRSpeed(yaw_Speed); yaw_goal=yaw_Init;
yusuke_kyo 0:3b39104fae2f 61 wait(1);
yusuke_kyo 0:3b39104fae2f 62 }
yusuke_kyo 0:3b39104fae2f 63
yusuke_kyo 0:3b39104fae2f 64 void Dyna_GetData(char* data) {
yusuke_kyo 0:3b39104fae2f 65 if(DYNA_DEBUG) printf("\nDyna_SetData\n");
yusuke_kyo 0:3b39104fae2f 66 float lP, lV, pP, pC, yP, yC;
yusuke_kyo 0:3b39104fae2f 67
yusuke_kyo 0:3b39104fae2f 68 lP = linear.GetPosition(); lV = linear.GetVolts();
yusuke_kyo 0:3b39104fae2f 69 pP = pitch.GetPosition(); pC = pitch.GetCurrent();
yusuke_kyo 0:3b39104fae2f 70 yP = yaw.GetPosition(); yC = yaw.GetCurrent();
yusuke_kyo 0:3b39104fae2f 71
yusuke_kyo 0:3b39104fae2f 72 linear_goal=lP; pitch_goal=pP; yaw_goal=yP; //現在角度を目標値に設定
yusuke_kyo 0:3b39104fae2f 73 sprintf(data,"%4.1f %4.1f %4.1f %4.1f %4.1f %4.1f ",lP,pP,yP,lV,pC,yC);
yusuke_kyo 0:3b39104fae2f 74 }
yusuke_kyo 0:3b39104fae2f 75
yusuke_kyo 0:3b39104fae2f 76 void Dyna_SetGoal(int mode) {
yusuke_kyo 0:3b39104fae2f 77 if(DYNA_DEBUG) printf("\nDyna_SetGoal\n");
yusuke_kyo 0:3b39104fae2f 78 switch(mode){
yusuke_kyo 0:3b39104fae2f 79 case 97:linear.SetTorqueLimit(1); linear.TorqueEnable(1); linear_goal-=10;
yusuke_kyo 0:3b39104fae2f 80 if(linear_goal<linear_min) linear_goal=linear_min;
yusuke_kyo 0:3b39104fae2f 81 if(linear_goal>linear_MAX) linear_goal=linear_MAX;
yusuke_kyo 0:3b39104fae2f 82 linear.SetGoal(linear_goal);
yusuke_kyo 0:3b39104fae2f 83 break;
yusuke_kyo 0:3b39104fae2f 84 case 102: linear.SetTorqueLimit(1); linear.TorqueEnable(1); linear_goal+=20;
yusuke_kyo 0:3b39104fae2f 85 if(linear_goal<linear_min) linear_goal=linear_min;
yusuke_kyo 0:3b39104fae2f 86 if(linear_goal>linear_MAX) linear_goal=linear_MAX;
yusuke_kyo 0:3b39104fae2f 87 linear.SetGoal(linear_goal);
yusuke_kyo 0:3b39104fae2f 88 break;
yusuke_kyo 0:3b39104fae2f 89 case 30:pitch.SetTorqueLimit(1); pitch_goal-=20;
yusuke_kyo 0:3b39104fae2f 90 if(pitch_goal<pitch_min) pitch_goal=pitch_min;
yusuke_kyo 0:3b39104fae2f 91 if(pitch_goal>pitch_MAX) pitch_goal=pitch_MAX;
yusuke_kyo 0:3b39104fae2f 92 pitch.SetGoal(pitch_goal);
yusuke_kyo 0:3b39104fae2f 93 break;
yusuke_kyo 0:3b39104fae2f 94 case 31:pitch.SetTorqueLimit(1); pitch_goal+=5;
yusuke_kyo 0:3b39104fae2f 95 if(pitch_goal<pitch_min) pitch_goal=pitch_min;
yusuke_kyo 0:3b39104fae2f 96 if(pitch_goal>pitch_MAX) pitch_goal=pitch_MAX;
yusuke_kyo 0:3b39104fae2f 97 pitch.SetGoal(pitch_goal);
yusuke_kyo 0:3b39104fae2f 98 break;
yusuke_kyo 0:3b39104fae2f 99 case 28:yaw_goal-=10;
yusuke_kyo 0:3b39104fae2f 100 if(yaw_goal<-2520) yaw_goal=-2520;
yusuke_kyo 0:3b39104fae2f 101 if(yaw_goal>2520) yaw_goal=2520;
yusuke_kyo 0:3b39104fae2f 102 yaw.SetGoal(yaw_goal);
yusuke_kyo 0:3b39104fae2f 103 break;
yusuke_kyo 0:3b39104fae2f 104 case 29:yaw_goal+=10;
yusuke_kyo 0:3b39104fae2f 105 if(yaw_goal<-2520) yaw_goal=-2520;
yusuke_kyo 0:3b39104fae2f 106 if(yaw_goal>2520) yaw_goal=2520;
yusuke_kyo 0:3b39104fae2f 107 yaw.SetGoal(yaw_goal);
yusuke_kyo 0:3b39104fae2f 108 break; }
yusuke_kyo 0:3b39104fae2f 109 }
yusuke_kyo 0:3b39104fae2f 110
yusuke_kyo 0:3b39104fae2f 111 void Dyna_home_position() {
yusuke_kyo 0:3b39104fae2f 112 if(DYNA_DEBUG) printf("\nDyna_home_position\n");
yusuke_kyo 0:3b39104fae2f 113 linear.SetTorqueLimit(1);
yusuke_kyo 0:3b39104fae2f 114 pitch.SetTorqueLimit(1);
yusuke_kyo 0:3b39104fae2f 115 yaw.SetTorqueLimit(1);
yusuke_kyo 0:3b39104fae2f 116 float lp;
yusuke_kyo 0:3b39104fae2f 117 linear.SetGoal(linear_Init);
yusuke_kyo 0:3b39104fae2f 118 lp=linear.GetPosition();
yusuke_kyo 0:3b39104fae2f 119 if(lp > linear_MAX - 30) { //ある程度縮んだら
yusuke_kyo 0:3b39104fae2f 120 pitch.SetGoal(pitch_Init);
yusuke_kyo 0:3b39104fae2f 121 yaw.SetGoal(yaw_Init);
yusuke_kyo 0:3b39104fae2f 122 }
yusuke_kyo 0:3b39104fae2f 123 }
yusuke_kyo 0:3b39104fae2f 124
yusuke_kyo 0:3b39104fae2f 125 void Dyna_reset() {
yusuke_kyo 0:3b39104fae2f 126 if(DYNA_DEBUG) printf("\nDyna_reset\n");
yusuke_kyo 0:3b39104fae2f 127 // RelaySwitch = 0;
yusuke_kyo 0:3b39104fae2f 128 linear.SetTorqueLimit(0);
yusuke_kyo 0:3b39104fae2f 129 pitch.SetTorqueLimit(0);
yusuke_kyo 0:3b39104fae2f 130 yaw.SetTorqueLimit(0);
yusuke_kyo 0:3b39104fae2f 131
yusuke_kyo 0:3b39104fae2f 132 wait(1);
yusuke_kyo 0:3b39104fae2f 133
yusuke_kyo 0:3b39104fae2f 134 // RelaySwitch = 1;
yusuke_kyo 0:3b39104fae2f 135 linear.SetTorqueLimit(1);
yusuke_kyo 0:3b39104fae2f 136 pitch.SetTorqueLimit(1);
yusuke_kyo 0:3b39104fae2f 137 yaw.SetTorqueLimit(1);
yusuke_kyo 0:3b39104fae2f 138 }
yusuke_kyo 0:3b39104fae2f 139
yusuke_kyo 0:3b39104fae2f 140 void Dyna_end() {
yusuke_kyo 0:3b39104fae2f 141 if(DYNA_DEBUG) printf("\nDyna_end\n");
yusuke_kyo 0:3b39104fae2f 142 // RelaySwitch = 0;
yusuke_kyo 0:3b39104fae2f 143 linear.SetTorqueLimit(0);
yusuke_kyo 0:3b39104fae2f 144 pitch.SetTorqueLimit(0);
yusuke_kyo 0:3b39104fae2f 145 yaw.SetTorqueLimit(0);
yusuke_kyo 0:3b39104fae2f 146 }
yusuke_kyo 0:3b39104fae2f 147 /*--Dynamixel:end-----------------------------------------------------------------------------------------*/
yusuke_kyo 0:3b39104fae2f 148
yusuke_kyo 0:3b39104fae2f 149 /*---------------
yusuke_kyo 0:3b39104fae2f 150 MAIN LOOP
yusuke_kyo 0:3b39104fae2f 151 ----------------*/
yusuke_kyo 0:3b39104fae2f 152 #define DEBUG 1
yusuke_kyo 0:3b39104fae2f 153
yusuke_kyo 0:3b39104fae2f 154 int main() {
yusuke_kyo 0:3b39104fae2f 155 myled1=1; myled2=1; myled3=1; myled4=1;
yusuke_kyo 0:3b39104fae2f 156 pc.baud(38400);
yusuke_kyo 0:3b39104fae2f 157
yusuke_kyo 0:3b39104fae2f 158 char Dyna_data[50];
yusuke_kyo 0:3b39104fae2f 159
yusuke_kyo 0:3b39104fae2f 160 myled1=1; myled2=1; myled3=0; myled4=0;
yusuke_kyo 0:3b39104fae2f 161 Dyna_init();
yusuke_kyo 0:3b39104fae2f 162 Dyna_home_position();
yusuke_kyo 0:3b39104fae2f 163 wait(2);
yusuke_kyo 0:3b39104fae2f 164 Dyna_home_position();
yusuke_kyo 0:3b39104fae2f 165 wait(2);
yusuke_kyo 0:3b39104fae2f 166 myled1=0; myled2=0; myled3=1; myled4=0;
yusuke_kyo 0:3b39104fae2f 167
yusuke_kyo 0:3b39104fae2f 168 union ArmPacket Read_data;
yusuke_kyo 0:3b39104fae2f 169 int mode;
yusuke_kyo 0:3b39104fae2f 170
yusuke_kyo 0:3b39104fae2f 171 while(1) {
yusuke_kyo 0:3b39104fae2f 172 myled1=0; myled2=0; myled3=0; myled4=0;
yusuke_kyo 0:3b39104fae2f 173 /*--Dynamixel:begin---------------------------------------------------------------*/
yusuke_kyo 0:3b39104fae2f 174 //Dynamixelへの命令を判定
yusuke_kyo 0:3b39104fae2f 175 mode=pc.getc();
yusuke_kyo 0:3b39104fae2f 176 printf("mode : %d\n",mode);
yusuke_kyo 0:3b39104fae2f 177
yusuke_kyo 0:3b39104fae2f 178 //目標角度を変更
yusuke_kyo 0:3b39104fae2f 179 myled2=1;
yusuke_kyo 0:3b39104fae2f 180 Dyna_SetGoal(mode);
yusuke_kyo 0:3b39104fae2f 181 myled2=0;
yusuke_kyo 0:3b39104fae2f 182 /*--Dynamixel:end------------------------------------------------------------------*/
yusuke_kyo 0:3b39104fae2f 183
yusuke_kyo 0:3b39104fae2f 184 //現在の角度・電圧・電流を取得
yusuke_kyo 0:3b39104fae2f 185 myled1=1;
yusuke_kyo 0:3b39104fae2f 186 Dyna_GetData(Dyna_data);
yusuke_kyo 0:3b39104fae2f 187 myled1=0;
yusuke_kyo 0:3b39104fae2f 188
yusuke_kyo 0:3b39104fae2f 189 //値を送信
yusuke_kyo 0:3b39104fae2f 190 myled4=1;
yusuke_kyo 0:3b39104fae2f 191 pc.printf("%s\n",Dyna_data)
yusuke_kyo 0:3b39104fae2f 192 myled4=0;
yusuke_kyo 0:3b39104fae2f 193 }
yusuke_kyo 0:3b39104fae2f 194 }
yusuke_kyo 0:3b39104fae2f 195
yusuke_kyo 0:3b39104fae2f 196 /* CoolTermのSend stringを使う場合 Hex : 16進数で値を送信可能 なので
yusuke_kyo 0:3b39104fae2f 197 home_position 40
yusuke_kyo 0:3b39104fae2f 198 reset 80
yusuke_kyo 0:3b39104fae2f 199 end c0
yusuke_kyo 0:3b39104fae2f 200 */