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 */