雄介 太田
/
YOZAKURA_ARM_USB_0503
Arm code
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 /*YOZAKURAのアームコード 00002 シリアル通信で送るデータ S_Data={linearD[4],pitchD[4],yawD[4],ThermoD1[16],ThermoD2[16],CO2[4]} 計48bytes 00003 シリアル通信で受け取るデータ R_Data={linear_ref[2bit],pitch_ref[2bit],yaw_ref[2bit]m,ode[2bit]} 計1byte 00004 */ 00005 00006 #include "mbed.h" 00007 00008 Serial pc(USBTX, USBRX); // tx, rx 00009 DigitalOut myled1(LED1); DigitalOut myled2(LED2); DigitalOut myled3(LED3); DigitalOut myled4(LED4); 00010 00011 struct ArmPacketBits { 00012 unsigned int mode : 2; 00013 unsigned int linear : 2; 00014 unsigned int pitch : 2; 00015 unsigned int yaw : 2; 00016 }; 00017 00018 union ArmPacket { 00019 struct ArmPacketBits b; 00020 unsigned char as_byte; 00021 }; 00022 00023 /*--Dynamixel:begin-----------------------------------------------------------------------------------------*/ 00024 #include "AX12.h" 00025 #include "MX28.h" 00026 00027 #define DYNA_DEBUG 0 00028 00029 DigitalOut low(p16); DigitalOut RelaySwitch(p18); 00030 00031 AX12 linear (p13, p14, 0, 1000000); //直動Dynamixel 00032 MX28 pitch (p13, p14, 1, 1000000); //ピッチDynamixel 00033 MX28 yaw (p13, p14, 2, 1000000); //ヨーDynamixel 00034 00035 int linear_goal, pitch_goal, yaw_goal; 00036 00037 //最小値,最大値,角速度,初期値を指定[unit:degree] 00038 //MX:MultiTurnモードでは-2520°~2520°(SetGoalの書き換え不要) 00039 int linear_min = 150; int linear_MAX = 300; float linear_Speed=0.1; int linear_Init = linear_MAX; 00040 int pitch_min = 172; int pitch_MAX = 334; float pitch_Speed=0.2; int pitch_Init = pitch_MAX; 00041 int yaw_min = 360; int yaw_MAX = 360; float yaw_Speed=0.2; int yaw_Init = 0; //MultiTurnモード 00042 00043 void Dyna_init() { 00044 if(DYNA_DEBUG) printf("\nDyna_init\n"); 00045 // low = 0; RelaySwitch = 1; 00046 linear.SetTorqueLimit(0); 00047 pitch.SetTorqueLimit(0); 00048 yaw.SetTorqueLimit(0); 00049 linear.SetCWLimit(linear_min); linear.SetCCWLimit(linear_MAX); 00050 linear.SetCRSpeed(linear_Speed); linear_goal=linear_Init; 00051 pitch.SetCWLimit(pitch_min); pitch.SetCCWLimit(pitch_MAX); 00052 pitch.SetCRSpeed(pitch_Speed); pitch_goal=pitch_Init; 00053 yaw.SetCWLimit(yaw_min); yaw.SetCCWLimit(yaw_MAX); 00054 yaw.SetCRSpeed(yaw_Speed); yaw_goal=yaw_Init; 00055 wait(1); 00056 } 00057 00058 void Dyna_GetData(char* data) { 00059 if(DYNA_DEBUG) printf("\nDyna_SetData\n"); 00060 float lP, lV, pP, pC, yP, yC; 00061 00062 lP = linear.GetPosition(); lV = linear.GetVolts(); 00063 pP = pitch.GetPosition(); pC = pitch.GetCurrent(); 00064 yP = yaw.GetPosition(); yC = yaw.GetCurrent(); 00065 00066 linear_goal=lP; pitch_goal=pP; yaw_goal=yP; //現在角度を目標値に設定 00067 sprintf(data,"%4.1f %4.1f %4.1f %4.1f %4.1f %4.1f ",lP,pP,yP,lV,pC,yC); 00068 } 00069 00070 void Dyna_SetGoal(int linear_mode, int pitch_mode, int yaw_mode) { 00071 if(DYNA_DEBUG) printf("\nDyna_SetGoal\n"); 00072 switch(linear_mode){ 00073 case 0: break; 00074 case 1: linear.SetTorqueLimit(1); linear_goal-=10; break; 00075 case 2: linear.SetTorqueLimit(1); linear_goal+=20; break; 00076 } 00077 if(linear_mode!=0){ 00078 if(linear_goal<linear_min) linear_goal=linear_min; 00079 if(linear_goal>linear_MAX) linear_goal=linear_MAX; 00080 linear.SetGoal(linear_goal); 00081 } 00082 00083 switch(pitch_mode){ 00084 case 0: break; 00085 case 1: pitch.SetTorqueLimit(1); pitch_goal-=10; break; 00086 case 2: pitch.SetTorqueLimit(1); pitch_goal+=20; break; 00087 } 00088 if(pitch_mode!=0){ 00089 if(pitch_goal<pitch_min) pitch_goal=pitch_min; 00090 if(pitch_goal>pitch_MAX) pitch_goal=pitch_MAX; 00091 pitch.SetGoal(pitch_goal); 00092 } 00093 00094 switch(yaw_mode){ 00095 case 0: break; 00096 case 1: yaw_goal-=20; break; 00097 case 2: yaw_goal+=20; break; 00098 } 00099 if(yaw_mode!=0){ 00100 if(yaw_goal<-2520) yaw_goal=-2520; 00101 if(yaw_goal>2520) yaw_goal=2520; 00102 yaw.SetGoal(yaw_goal); 00103 } 00104 } 00105 00106 void Dyna_home_position() { 00107 if(DYNA_DEBUG) printf("\nDyna_home_position\n"); 00108 linear.SetTorqueLimit(1); 00109 pitch.SetTorqueLimit(1); 00110 yaw.SetTorqueLimit(1); 00111 float lp; 00112 linear.SetGoal(linear_Init); 00113 lp=linear.GetPosition(); 00114 if(lp > linear_MAX - 30) { //ある程度縮んだら 00115 pitch.SetGoal(306); 00116 yaw.SetGoal(yaw_Init); 00117 } 00118 } 00119 00120 void Dyna_reset() { 00121 if(DYNA_DEBUG) printf("\nDyna_reset\n"); 00122 // RelaySwitch = 0; 00123 linear.SetTorqueLimit(0); 00124 pitch.SetTorqueLimit(0); 00125 yaw.SetTorqueLimit(0); 00126 00127 wait(1); 00128 00129 // RelaySwitch = 1; 00130 linear.SetTorqueLimit(1); 00131 pitch.SetTorqueLimit(1); 00132 yaw.SetTorqueLimit(1); 00133 } 00134 00135 void Dyna_end() { 00136 if(DYNA_DEBUG) printf("\nDyna_end\n"); 00137 // RelaySwitch = 0; 00138 linear.SetTorqueLimit(0); 00139 pitch.SetTorqueLimit(0); 00140 yaw.SetTorqueLimit(0); 00141 } 00142 /*--Dynamixel:end-----------------------------------------------------------------------------------------*/ 00143 00144 00145 /*--Thermal_Sensor:begin----------------------------------------------------------------------------------*/ 00146 /*MEMS非接触温度センサ:形D6T-44L-06 4×4素子タイプ*/ 00147 /*データシート:http://www.omron.co.jp/ecb/products/sensor/special/mems/pdf/AN-D6T-01JP_r2.pdf*/ 00148 00149 I2C MEMS1(p9, p10); // sda, scl 00150 I2C MEMS2(p28, p27); // sda, scl 00151 00152 #define D6T_addr 0x14 00153 #define D6T_cmd 0x4c 00154 00155 #define THERMO_DEBUG 0 00156 00157 char I2C_rd1[64]; // 生データ 00158 short datr1[16]; // 16点 温度データ(10倍整数) 00159 short PTAT1; // センサ内部PTAT温度データ(10倍整数) 00160 double dt1[16]; // 16点 温度データ 00161 double dt1_temp[16]; 00162 short d_PTAT1; // センサ内部PTAT温度データ 00163 00164 char I2C_rd2[64]; // 生データ 00165 short datr2[16]; // 16点 温度データ(10倍整数) 00166 short PTAT2; // センサ内部PTAT温度データ(10倍整数) 00167 double dt2[16]; // 16点 温度データ 00168 double dt2_temp[16]; 00169 short d_PTAT2; // センサ内部PTAT温度データ 00170 00171 void GetThermo(char* data) { 00172 char con[6]; 00173 sprintf(data,""); 00174 00175 /*MEMS1*/ 00176 int i,j; 00177 int itemp; 00178 00179 //// measure 00180 MEMS1.start(); 00181 MEMS1.write(D6T_addr); 00182 MEMS1.write(D6T_cmd); 00183 // Repeated Start condition 00184 MEMS1.read(D6T_addr,I2C_rd1,35); 00185 // if(check_PEC(I2C_rd) == -1) continue; // error 00186 for(i=0,j=0;i<17;i++){ 00187 itemp = (I2C_rd1[j++] & 0xff); 00188 itemp += I2C_rd1[j++] * 256; 00189 if(i == 0) PTAT1 = itemp; 00190 else datr1[i-1] = itemp; 00191 } 00192 for(i=0;i<16;i++){ 00193 dt1[i] = 0.1 * datr1[i]; 00194 } 00195 for(int i=0;i<16;i++){ 00196 if(dt1[i]<50 && dt1[i]>5) { 00197 sprintf(con,"%3.1f ",dt1[i]); 00198 dt1_temp[i]=dt1[i]; 00199 } 00200 else sprintf(con,"%3.1f ",dt1_temp[i]); 00201 strcat(data,con); 00202 } 00203 if(THERMO_DEBUG){ 00204 printf("\nThermal_Sensor 1"); 00205 for(i=0;i<16;i++){ 00206 if(i%4==0) printf("\n"); 00207 printf("%3.1f ",dt1[i]); 00208 } printf("\n"); 00209 } 00210 d_PTAT1 = 0.1 * PTAT1; 00211 // wait(0.1); 00212 /*MEMS1*/ 00213 00214 /*MEMS2*/ 00215 //// measure 00216 MEMS2.start(); 00217 MEMS2.write(D6T_addr); 00218 MEMS2.write(D6T_cmd); 00219 // Repeated Start condition 00220 MEMS2.read(D6T_addr,I2C_rd2,35); 00221 // if(check_PEC(I2C_rd) == -1) continue; // error 00222 for(i=0,j=0;i<17;i++){ 00223 itemp = (I2C_rd2[j++] & 0xff); 00224 itemp += I2C_rd2[j++] * 256; 00225 if(i == 0) PTAT2 = itemp; 00226 else datr2[i-1] = itemp; 00227 } 00228 for(i=0;i<16;i++){ 00229 dt2[i] = 0.1 * datr2[i]; 00230 } 00231 for(int i=0;i<16;i++){ 00232 if(dt2[i]<50 && dt2[i]>5) { 00233 sprintf(con,"%3.1f ",dt2[i]); 00234 dt2_temp[i]=dt2[i]; 00235 } 00236 else sprintf(con,"%3.1f ",dt2_temp[i]); 00237 strcat(data,con); 00238 } 00239 if(THERMO_DEBUG){ 00240 printf("\nThermal_Sensor 2"); 00241 for(i=0;i<16;i++){ 00242 if(i%4==0) printf("\n"); 00243 printf("%3.1f ",dt2[i]); 00244 } printf("\n"); 00245 } 00246 d_PTAT2 = 0.1 * PTAT2; 00247 // wait(0.1); 00248 /*MEMS2*/ 00249 } 00250 /*--Thermal_Sensor:end------------------------------------------------------------------------------------*/ 00251 00252 /*--CO2_Sensor:begin--------------------------------------------------------------------------------------*/ 00253 /*CO2センサモジュール:A051020-AQ6B-01*/ 00254 /*データシート:http://www.fisinc.co.jp/common/pdf/A051020-AQ6.pdf*/ 00255 /*参考 外気:396.0[ppm](2013年) 呼気:13,200[ppm] ※このセンサで測れるのは3000[ppm]まで*/ 00256 #include "mbed.h" 00257 00258 AnalogIn ain(p17); 00259 00260 #define CO2_DEBUG 0 00261 00262 float GetCO2() { 00263 float v; //生データ:電圧 00264 float sensor_v,CO2; 00265 00266 v = ain.read()*3.3; 00267 sensor_v = v * 5.0/3.3; //電圧レベルを合わせる 00268 CO2 = sensor_v * 1000 + 400; //データシートより 00269 00270 if(CO2_DEBUG) printf("\nCO2:%4.1f\n",CO2); 00271 00272 return(CO2); 00273 } 00274 /*--CO2_Sensor:end----------------------------------------------------------------------------------------*/ 00275 00276 Ticker timeout; 00277 00278 void roop(){ 00279 00280 // printf("roop restart\n"); 00281 00282 myled1=1; myled2=1; myled3=1; myled4=1; 00283 // pc.baud(38400); 00284 00285 char Dyna_data[50]; 00286 char Thermo_data[200]; 00287 float CO2_data; 00288 00289 myled1=1; myled2=1; myled3=0; myled4=0; 00290 // while(1){ 00291 Dyna_init(); 00292 // } 00293 Dyna_home_position(); 00294 wait(2); 00295 Dyna_home_position(); 00296 wait(2); 00297 myled1=0; myled2=0; myled3=1; myled4=0; 00298 00299 union ArmPacket Read_data; 00300 00301 char Send_data[250]; 00302 //int Read_data; 00303 int Joy_mode, linear_mode, pitch_mode, yaw_mode; 00304 00305 while(1) { 00306 //=pc.getc(); 00307 Read_data.as_byte=pc.getc(); 00308 wait_ms(1); 00309 if(Read_data.as_byte == 7) {printf("arm\n"); continue;} 00310 // if(Read_data.as_byte == 7) {printf("arm\n");} 00311 00312 //Joy_mode = Read_data & 0x03; //ジョイスティックのモード判定 00313 Joy_mode = Read_data.b.mode; //ジョイスティックのモード判定 00314 // if(DEBUG) printf("Joy_mode : %d\n",Joy_mode); 00315 00316 switch(Joy_mode){ 00317 case 0: //通常モード 00318 myled1=0; myled2=0; myled3=0; myled4=0; 00319 /*--Dynamixel:begin---------------------------------------------------------------*/ 00320 //Dynamixelへの命令を判定 00321 //linear_mode = Read_data & 0xc0; linear_mode = linear_mode >> 6; if(DEBUG) printf("linear_mode : %d\n",linear_mode); 00322 // pitch_mode = Read_data & 0x30; pitch_mode = pitch_mode >> 4; if(DEBUG) printf("pitch_mode : %d\n",pitch_mode); 00323 // yaw_mode = Read_data & 0x0c; yaw_mode = yaw_mode >> 2; if(DEBUG) printf("yaw_mode : %d\n",yaw_mode); 00324 linear_mode = Read_data.b.linear; 00325 pitch_mode = Read_data.b.pitch; 00326 yaw_mode = Read_data.b.yaw; 00327 00328 //目標角度を変更 00329 myled2=1; 00330 Dyna_SetGoal(linear_mode, pitch_mode, yaw_mode); 00331 myled2=0; 00332 /*--Dynamixel:end------------------------------------------------------------------*/ 00333 break; 00334 00335 case 1: //ホームポジション 00336 myled1=1; myled2=0; myled3=0; myled4=1; Dyna_home_position(); break; 00337 00338 case 2: //リセット 00339 myled1=0; myled2=1; myled3=1; myled4=0; Dyna_reset(); break; 00340 00341 case 3: //終了 00342 myled1=1; myled2=1; myled3=1; myled4=1; Dyna_end(); break; 00343 } 00344 00345 /*--Thermal_Sensor:begin-----------------------------------------------------------*/ 00346 //値を取得 00347 myled3=1; 00348 GetThermo(Thermo_data); 00349 /*--Thermal_Sensor:end-------------------------------------------------------------*/ 00350 00351 /*--CO2_Sensor:begin---------------------------------------------------------------*/ 00352 //値を取得 00353 CO2_data=GetCO2(); 00354 myled3=0; 00355 /*--CO2_Sensor:end-----------------------------------------------------------------*/ 00356 00357 //現在の角度・電圧・電流を取得 00358 myled1=1; 00359 Dyna_GetData(Dyna_data); 00360 myled1=0; 00361 00362 //値を送信 00363 myled4=1; 00364 sprintf(Send_data,"%s%s%4.1f",Dyna_data,Thermo_data,CO2_data); 00365 pc.printf("%s\n",Send_data); 00366 myled4=0; 00367 } 00368 } 00369 /*--------------- 00370 MAIN LOOP 00371 ----------------*/ 00372 #define DEBUG 0 00373 00374 00375 00376 int main() { 00377 pc.baud(38400); 00378 timeout.attach(&roop,0.2); 00379 while(1){ 00380 } 00381 } 00382 00383 /* CoolTermのSend stringを使う場合 Hex : 16進数で値を送信可能 なので 00384 home_position 40 00385 reset 80 00386 end c0 00387 */
Generated on Sat Jul 30 2022 12:10:25 by 1.7.2