main.cpp

00001 /*YOZAKURAのアームコード
00002   シリアル通信で送るデータ S_Data={linearD[4],pitchD[4],yawD[4],ThermoD1[16],ThermoD2[16],CO2[4]} 計48bytes
00003   シリアル通信で受け取るデータ　R_Data=｛mode[2bit],linear_ref[2bit],pitch_ref[2bit],yaw_ref[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 /*--Dynamixel:begin-----------------------------------------------------------------------------------------*/
00012 #include "AX12.h"
00013 #include "MX28.h"
00014 
00015 DigitalOut low(p16); DigitalOut RelaySwitch(p18);
00016 
00017 AX12 linear (p13, p14, 0, 1000000);      //直動Dynamixel
00018 MX28 pitch (p13, p14, 1, 1000000);       //ピッチDynamixel
00019 MX28 yaw (p13, p14, 2, 1000000);         //ヨーDynamixel
00020 
00021 int linear_goal, pitch_goal, yaw_goal;
00022 
00023 //最小値，最大値，角速度，初期値を指定[unit:degree]
00024 //MX:MultiTurnモードでは-2520°～2520°(SetGoalの書き換え不要)
00025 int linear_min = 100; int linear_MAX = 300; float linear_Speed=0.1; int linear_Init = linear_MAX;
00026 int pitch_min = 172; int pitch_MAX = 334; float pitch_Speed=0.2; int pitch_Init = pitch_MAX;
00027 int yaw_min = 360; int yaw_MAX = 360; float yaw_Speed=0.2; int yaw_Init = 0; //MultiTurnモード
00028 
00029 void Dyna_init() {
00030     low = 0; RelaySwitch = 1;
00031     linear.SetCWLimit(linear_min); linear.SetCCWLimit(linear_MAX); 
00032     linear.SetCRSpeed(linear_Speed); linear_goal=linear_Init; linear.SetGoal(linear_Init);
00033     pitch.SetCWLimit(pitch_min); pitch.SetCCWLimit(pitch_MAX); 
00034     pitch.SetCRSpeed(pitch_Speed); pitch_goal=linear_Init; pitch.SetGoal(pitch_Init);
00035     yaw.SetCWLimit(yaw_min); yaw.SetCCWLimit(yaw_MAX); 
00036     yaw.SetCRSpeed(yaw_Speed); yaw_goal=yaw_Init; yaw.SetGoal(yaw_Init);
00037     wait(1);
00038 }
00039 
00040 void Dyna_GetData(char* data) {
00041     float lP, lV, pP, pC, yP, yC;
00042     
00043     lP = linear.GetPosition(); lV = linear.GetVolts();
00044     pP = pitch.GetPosition(); pC = pitch.GetCurrent();
00045     yP = yaw.GetPosition(); yC = yaw.GetCurrent();
00046     
00047     linear_goal=lP; pitch_goal=pP; yaw_goal=yP; //現在角度を目標値に設定
00048     
00049     sprintf(data,"%4f %4f %4f %4f %4f %4f ",lP,lV,pP,pC,yP,yC);  
00050 }
00051 
00052 void Dyna_SetGoal(int linear_mode, int pitch_mode, int yaw_mode) {
00053     switch(linear_mode){
00054         case 0: break;
00055         case 1: linear.SetTorqueLimit(1); linear_goal++; break;
00056         case 2: linear.SetTorqueLimit(1); linear_goal--; break;
00057     }
00058     linear.SetGoal(linear_goal);
00059     
00060     switch(pitch_mode){
00061         case 0: break;
00062         case 1: pitch_goal++; break;
00063         case 2: pitch_goal--; break;
00064     }    
00065     pitch.SetGoal(pitch_goal);
00066     
00067     switch(yaw_mode){
00068         case 0: break;
00069         case 1: yaw_goal++; break;
00070         case 2: yaw_goal--; break;
00071     }
00072     yaw.SetGoal(yaw_goal);
00073 }
00074 
00075 void Dyna_home_position() {
00076     float lp;
00077     linear.SetGoal(linear_Init);
00078     lp=linear.GetPosition();
00079     if(lp > linear_MAX - 30) {  //ある程度縮んだら
00080         pitch.SetGoal(pitch_Init);
00081         yaw.SetGoal(yaw_Init);
00082     }
00083 }
00084 
00085 void Dyna_reset() {
00086     RelaySwitch = 0;
00087     wait_ms(10);
00088     RelaySwitch = 1;
00089 }
00090 
00091 void Dyna_end() {
00092     RelaySwitch = 0;
00093 }
00094 /*--Dynamixel:end-----------------------------------------------------------------------------------------*/
00095 
00096 
00097 /*--Thermal_Sensor:begin----------------------------------------------------------------------------------*/
00098 /*MEMS非接触温度センサ：形D6T-44L-06 4×4素子タイプ*/
00099 /*データシート：http://www.omron.co.jp/ecb/products/sensor/special/mems/pdf/AN-D6T-01JP_r2.pdf*/
00100 #include "MEMS.h"
00101 
00102 MEMS MEMS1(p9, p10); // sda, scl
00103 MEMS MEMS2(p28, p27); // sda, scl
00104 
00105 #define THERMO_DEBUG 0
00106 
00107 void GetThermo(char* data) {
00108     float ThD1[16], ThD2[16];
00109     char con[10];
00110     
00111     MEMS1.temp(ThD1);
00112     MEMS2.temp(ThD2);
00113     
00114     for(int i=0;i<16;i++){
00115         sprintf(con,"%3.1f ",ThD1[i]);
00116         strcat(data,con);
00117     }
00118     
00119     if(THERMO_DEBUG) {
00120         for(int i=0;i<16;i++){
00121             if(i%4==0) printf("\n");
00122             printf("%3.1f ",ThD1[i]);
00123         }
00124     }
00125     
00126     for(int i=0;i<16;i++){
00127         sprintf(con,"%3.1f ",ThD2[i]);
00128         strcat(data,con);
00129     }
00130     
00131     if(THERMO_DEBUG) {
00132         for(int i=0;i<16;i++){
00133             if(i%4==0) printf("\n");
00134             printf("%3.1f ",ThD2[i]);
00135         }
00136     }
00137 }
00138 /*--Thermal_Sensor:end------------------------------------------------------------------------------------*/
00139 
00140 
00141 /*--CO2_Sensor:begin--------------------------------------------------------------------------------------*/
00142 /*CO2センサモジュール：A051020-AQ6B-01*/
00143 /*データシート：http://www.fisinc.co.jp/common/pdf/A051020-AQ6.pdf*/
00144 /*参考 外気:396.0[ppm](2013年) 呼気:13,200[ppm] ※このセンサで測れるのは3000[ppm]まで*/
00145 #include "mbed.h"
00146 
00147 AnalogIn ain(p20);
00148 
00149 #define CO2_DEBUG 0
00150 
00151 float GetCO2() {
00152     float v; //生データ：電圧
00153     float sensor_v,CO2;
00154     
00155     v = ain.read()*3.3;
00156     sensor_v = v * 5.0/3.3; //電圧レベルを合わせる
00157     CO2 = sensor_v * 1000 + 400;  //データシートより
00158     
00159     if(CO2_DEBUG) printf("\n%4f",CO2);
00160 
00161     return(CO2);
00162 }
00163 /*--CO2_Sensor:end----------------------------------------------------------------------------------------*/
00164 
00165 
00166 /*---------------
00167     MAIN ROOP
00168 ----------------*/
00169 int main() {
00170     char Dyna_data[100];
00171     char Thermo_data[100];
00172     float CO2_data;
00173     
00174     Dyna_init();
00175     
00176     char Send_data[200];
00177     int Read_data;
00178     int Joy_mode, linear_mode, pitch_mode, yaw_mode;
00179     
00180     while(1) {
00181         
00182         Read_data=pc.getc();
00183         wait_ms(1);
00184         Joy_mode = Read_data & 0x03; //ジョイスティックのモード判定
00185         
00186         switch(Joy_mode){
00187             case 0: //通常モード
00188             /*--Dynamixel:begin---------------------------------------------------------------*/       
00189                 //Dynamixelへの命令を判定
00190                 linear_mode = R_data & 0x0c; linear_mode = linear_mode >> 2;
00191                 pitch_mode = R_data & 0x30; pitch_mode = pitch_mode >> 4;
00192                 yaw_mode = R_data & 0xc0; yaw_mode = yaw_mode >> 6;                
00193 
00194                 //現在の角度・電圧・電流を取得
00195                 myled1=1;
00196                 Dyna_GetData(Dyna_data);
00197                 myled1=0;
00198                 
00199                 //目標角度を変更
00200                 myled2=1;
00201                 Dyna_SetGoal(linear_mode, pitch_mode, yaw_mode);
00202                 myled2=0;
00203             /*--Dynamixel:end------------------------------------------------------------------*/       
00204         
00205             /*--Thermal_Sensor:begin-----------------------------------------------------------*/
00206                 //値を取得
00207                 myled3=1;
00208                 GetThermo(Thermo_data);
00209             /*--Thermal_Sensor:end-------------------------------------------------------------*/
00210         
00211             /*--CO2_Sensor:begin---------------------------------------------------------------*/
00212                 //値を取得
00213                 CO2_data=GetCO2();
00214                 myled3=0;
00215             /*--CO2_Sensor:end-----------------------------------------------------------------*/
00216             
00217             //値を送信
00218                 myled4=1;
00219                 sprintf(Send_data,"%s%s%f",Dyna_data,Thermo_data,CO2_data);
00220                 printf("%s\n",Send_data);
00221                 myled4=0;           
00222                 break;
00223             
00224             case 1: //ホームポジション
00225                 Dyna_home_position(); break;
00226                 
00227             case 2: //リセット
00228                 Dyna_reset(); break;
00229             
00230             case 3: //終了
00231                 myled1=1; myled2=1; myled3=1; myled4=1; Dyna_end(); return 0;
00232         }
00233     }
00234 }