main.cpp

00001 #include "mbed.h"
00002 #include "IWCMD.h"
00003 #warning CM 's program
00004 
00005 /* 設定 */
00006 const int kModuleID = 1;    // コントロールモジュールのID(い今のところ1だけ)
00007 const float k_vbat_offset = -0.1f;
00008 #define BATTERY_VOLTAGE_LOWER_LIMIT 3.0f   // バッテリー電圧下限，下回ったらxbeeオフ
00009 double Servo_slow_period   = 0.01;        // スローモード時，サーボ角度を変更させる周期
00010 const double Servo_slow_velocity = 0.02e-3;    // スローモード時，周期ごとに変更させるサーボ角度  <= slowMode時の回転速度はここで調整！
00011 // Servo pulsewidth             MIN     MID     MAX
00012 double Servo_1_pulsewidth_range[3] = {0.0005, 0.0015, 0.0025};
00013 double Servo_2_pulsewidth_range[3] = {0.0005, 0.0015, 0.0025};
00014 double Servo_3_pulsewidth_range[3] = {0.0005, 0.0015, 0.0025};
00015 double Servo_4_pulsewidth_range[3] = {0.0005, 0.0015, 0.0025};
00016 double Servo_5_pulsewidth_range[3] = {0.0005, 0.0015, 0.0025};
00017 double Servo_6_pulsewidth_range[3] = {0.0005, 0.0015, 0.0025};
00018 
00019 
00020 /* 時間設定 */
00021 /* WAIT_TIME_PM_REPLY + WAIT_TIME_PC_CMD > WDT_PERIOD                      => 接続状態でもWDTが必ず発動．
00022 *  WDT_PERIOD/2 < WAIT_TIME_PM_REPLY + WAIT_TIME_PC_CMD < WDT_PERIOD       => 1or2回接続確認に失敗するとWDTが発動．
00023 *  WDT_PERIOD/(N+1) < WAIT_TIME_PM_REPLY + WAIT_TIME_PC_CMD < WDT_PERIOD/N => N回以上接続確認に失敗するとWDTが発動するかも．
00024 */
00025 const float WAIT_TIME_PM_REPLY = 0.2f;  // PMの接続確認を待機する時間．xbeeの通信時間を考えて十分に長くとるべし．しかしこの時間はPCからの操作を受け付けないのでできるだけ短いほうが望ましい．
00026 const float WAIT_TIME_PC_CMD   = 3.0f;  // PCからの命令を待機する時間．長いほうがより長い時間PCからのコマンドを受け付けるが，WDTの周期に注意．
00027 
00028 
00029 
00030 
00031 /* ピン設定 */
00032 Serial pc(USBTX, USBRX, 9600);
00033 Serial xbee(PA_9, PA_10, 9600);
00034 DigitalOut xbee_reset(PA_1);
00035 DigitalOut led[] = {
00036     DigitalOut(PB_1),
00037     DigitalOut(PF_0),
00038     DigitalOut(PF_1),
00039 };
00040 AnalogIn raw_battery_voltage(PA_4);
00041 
00042 PwmOut Servo_1(PA_8);
00043 // PwmOut Servo_2(PA_0);  // us_tickerにてTIM2を利用中のため使用不可
00044 DigitalOut Servo_2(PA_0);
00045 PwmOut Servo_3(PA_6_ALT0);
00046 PwmOut Servo_4(PA_3);
00047 PwmOut Servo_5(PB_4);
00048 PwmOut Servo_6(PA_7_ALT1);
00049 
00050 
00051 
00052 /* タイマー設定 */
00053 Timer tim;
00054 Ticker  s2_pwm_tick;
00055 Timeout s2_pwm_timo;
00056 Ticker Servo_tick;
00057 
00058 
00059 /* グローバル変数 */
00060 double s2_pulsewidth_curr = 0.0015;
00061 double Servo_pulsewidth_target[6] = {1.5e-3,1.5e-3,1.5e-3,1.5e-3,1.5e-3,1.5e-3};
00062 double Servo_pulsewidth_current[6] = {1.5e-3,1.5e-3,1.5e-3,1.5e-3,1.5e-3,1.5e-3};
00063 
00064 
00065 /* プロトタイプ宣言 */
00066 void pwm_init();
00067 void xbee_init();
00068 void PM_checkconnection(int);
00069 void transmission_xbee2pc_wait_for(float);
00070 void Servo_2_off();
00071 void Servo_2_on();
00072 void Servo_2_pwm_interrupt();
00073 void Servo_2_period_ms(int);
00074 void Servo_2_pulsewidth(double);
00075 void cmd_exe_wait_for(float);
00076 void cmd_exe(char);
00077 void reset_all_motors();
00078 float read_battery_voltage();
00079 void xbee_enable();
00080 void xbee_disable();
00081 void Servo_change_angle_interrupt();
00082 
00083 
00084 Timer opt_tim;
00085 
00086 /* メイン文 */
00087 int main(){
00088     // 初期化
00089     pwm_init();
00090     xbee_init();
00091     
00092     wait(1);
00093     
00094     // ループ
00095     while(1){
00096         
00097         // バッテリー監視
00098         float bv = read_battery_voltage();
00099         if(bv <= BATTERY_VOLTAGE_LOWER_LIMIT){
00100             int i=1;
00101             while(read_battery_voltage() <= BATTERY_VOLTAGE_LOWER_LIMIT){
00102                 i++;
00103                 if(i==100){
00104                     xbee_disable();
00105                     break;
00106                 }
00107             }
00108         }else{
00109             xbee_enable();
00110         }
00111 
00112         // CM接続確認
00113         pc.printf("CM : %02d, BV : %04.2f\n", kModuleID, bv);
00114         
00115         // PM1接続確認
00116         PM_checkconnection(1);                               // PM1に接続確認
00117         transmission_xbee2pc_wait_for(WAIT_TIME_PM_REPLY);   // xbeeからの受信待機
00118         
00119         // コマンド実行
00120         cmd_exe_wait_for(WAIT_TIME_PC_CMD/2.0f);             // PCからのコマンド待機
00121         
00122         // PM2接続確認
00123         PM_checkconnection(2);                               // PM2に接続確認
00124         transmission_xbee2pc_wait_for(WAIT_TIME_PM_REPLY);   // xbeeからの受信待機
00125         
00126         // コマンド実行
00127         cmd_exe_wait_for(WAIT_TIME_PC_CMD/2.0f);             // PCからのコマンド待機
00128     }
00129 }
00130 
00131 /* PWM初期化 */
00132 void pwm_init(){
00133     Servo_1.period_ms(20);
00134     Servo_2_period_ms(20);
00135     Servo_3.period_ms(20);
00136     Servo_4.period_ms(20);
00137     Servo_5.period_ms(20);
00138     Servo_6.period_ms(20);
00139     reset_all_motors();
00140     Servo_tick.attach(&Servo_change_angle_interrupt, Servo_slow_period);
00141 }
00142 
00143 /* xbee */
00144 // 初期化
00145 void xbee_init(){
00146     xbee_enable();
00147 }
00148 // xbee動作許可
00149 void xbee_enable(){
00150     xbee_reset = 1;
00151     led[0] = 1;
00152 }
00153 // xbee動作禁止
00154 void xbee_disable(){
00155     xbee_reset = 0;
00156     led[0] = 0;
00157 }
00158 
00159 
00160 /* PMとの接続確認，PMのWDT更新 */
00161 void PM_checkconnection(int id){
00162     if(id == 1){
00163         xbee.putc(IWCMD_PM1_CCO);
00164     }else if(id == 2){
00165         xbee.putc(IWCMD_PM2_CCO);
00166     }
00167 }
00168 
00169 
00170 /* wait_time秒間，xbeeからの通信を受信してPCに表示 */
00171 void transmission_xbee2pc_wait_for(float wait_time){
00172     tim.reset();
00173     tim.start();
00174     while(tim.read() < wait_time){
00175         if(xbee.readable()){
00176             pc.putc(xbee.getc());
00177         }
00178     }
00179     tim.stop();
00180 }
00181 
00182 
00183 /* wait_time秒間，PCからの通信を受信してコマンド実行 */
00184 void cmd_exe_wait_for(float wait_time){
00185     tim.reset();
00186     tim.start();
00187     while(tim.read() < wait_time){
00188         if(pc.readable()){
00189             cmd_exe(pc.getc());
00190         }
00191     }
00192     tim.stop();
00193 }
00194 
00195 
00196 /* センシング */
00197 // バッテリー電圧読み取り
00198 float read_battery_voltage(){
00199     return k_vbat_offset + raw_battery_voltage.read() * 33.0f;
00200 }
00201 
00202 
00203 /*各コマンド実行内容*/
00204 // 全てのモータを停止,初期位置に．
00205 void reset_all_motors(){
00206     Servo_pulsewidth_target[1-1] = Servo_1_pulsewidth_range[1];
00207     Servo_pulsewidth_target[2-1] = Servo_2_pulsewidth_range[1];
00208     Servo_pulsewidth_target[3-1] = Servo_3_pulsewidth_range[1];
00209     Servo_pulsewidth_target[4-1] = Servo_4_pulsewidth_range[1];
00210     Servo_pulsewidth_target[5-1] = Servo_5_pulsewidth_range[1];
00211     Servo_pulsewidth_target[6-1] = Servo_6_pulsewidth_range[1];
00212     xbee.putc(IWCMD_PM1_SPS);
00213     xbee.putc(IWCMD_PM2_SPS);
00214 }
00215 
00216 /* Servoスローモード関数 */
00217 void Servo_change_angle_interrupt(){
00218     static const double Servo_pulsewidth_range[6][3] = {
00219         {Servo_1_pulsewidth_range[0], Servo_1_pulsewidth_range[1], Servo_1_pulsewidth_range[2]},
00220         {Servo_2_pulsewidth_range[0], Servo_2_pulsewidth_range[1], Servo_2_pulsewidth_range[2]},
00221         {Servo_3_pulsewidth_range[0], Servo_3_pulsewidth_range[1], Servo_3_pulsewidth_range[2]},
00222         {Servo_4_pulsewidth_range[0], Servo_4_pulsewidth_range[1], Servo_4_pulsewidth_range[2]},
00223         {Servo_5_pulsewidth_range[0], Servo_5_pulsewidth_range[1], Servo_5_pulsewidth_range[2]},
00224         {Servo_6_pulsewidth_range[0], Servo_6_pulsewidth_range[1], Servo_6_pulsewidth_range[2]},
00225     };
00226     for(int i=0; i<6; i++){
00227         double v;
00228         if(abs(Servo_pulsewidth_current[i]-Servo_pulsewidth_target[i])<Servo_slow_velocity){
00229             v = abs(Servo_pulsewidth_current[i]-Servo_pulsewidth_target[i]);
00230         }else{
00231             v = Servo_slow_velocity;
00232         }
00233         if      (Servo_pulsewidth_current[i] > Servo_pulsewidth_target[i]){
00234             Servo_pulsewidth_current[i] -= v;
00235         }else if(Servo_pulsewidth_current[i] < Servo_pulsewidth_target[i]){
00236             Servo_pulsewidth_current[i] += v;
00237         }
00238         if      (Servo_pulsewidth_current[i] > Servo_pulsewidth_range[i][2]){
00239             Servo_pulsewidth_current[i] = Servo_pulsewidth_range[i][2];
00240         }else if(Servo_pulsewidth_current[i] < Servo_pulsewidth_range[i][0]){
00241             Servo_pulsewidth_current[i] = Servo_pulsewidth_range[i][0];
00242         }
00243     }
00244     Servo_1.pulsewidth(Servo_pulsewidth_current[0]);
00245     Servo_2_pulsewidth(Servo_pulsewidth_current[1]);
00246     Servo_3.pulsewidth(Servo_pulsewidth_current[2]);
00247     Servo_4.pulsewidth(Servo_pulsewidth_current[3]);
00248     Servo_5.pulsewidth(Servo_pulsewidth_current[4]);
00249     Servo_6.pulsewidth(Servo_pulsewidth_current[5]);
00250 }
00251 
00252 
00253 /* Servo2用手動PWM */
00254 void Servo_2_off(){
00255     Servo_2 = 0;
00256 }
00257 void Servo_2_on(){
00258     Servo_2 = 1;
00259     s2_pwm_timo.attach(&Servo_2_off, s2_pulsewidth_curr);
00260 }
00261 void Servo_2_period_ms(int p_ms){
00262     Servo_2_off();
00263     s2_pwm_tick.attach(&Servo_2_on, (double)p_ms / 1.0e3);
00264 }
00265 void Servo_2_pulsewidth(double p){
00266     s2_pulsewidth_curr = p;
00267 }
00268 //const int s2_division = 100;   // PWM用分割数
00269 //int s2_duty;                   // PWM用分割数のうちduty個分High出力
00270 //int s2_period_us;              // PWM用分割数1つあたりの周期
00271 //
00272 //void Servo_2_period_ms(int p_ms){
00273 //    s2_period_us = (p_ms*1000) / s2_division;
00274 //    s2_pwm_tick.attach_us(&Servo_2_pwm_interrupt, s2_period_us);
00275 //}
00276 //void Servo_2_pulsewidth(double pw){
00277 //    s2_duty = pw*1000000.0 / (double)s2_period_us;
00278 //}
00279 //void Servo_2_pwm_interrupt(){
00280 //    static int t = 0;
00281 //    if(t == s2_division){
00282 //        Servo_2 = 0;
00283 //        t = 0;
00284 //        return;
00285 //    }
00286 //    if(t == s2_division-s2_duty)
00287 //        Servo_2 = 1;
00288 //    t++;
00289 //}
00290 
00291 
00292 /* コマンド実行 */
00293 void cmd_exe(char cmd){
00294     switch(cmd){
00295     case IWCMD_CM_SS1_PP: //スラスターサーボ1を正位置
00296         Servo_pulsewidth_target[1-1] = Servo_1_pulsewidth_range[2];
00297         break;
00298     case IWCMD_CM_SS1_SP: //スラスターサーボ1を零位置
00299         Servo_pulsewidth_target[1-1] = Servo_1_pulsewidth_range[1];
00300         break;
00301     case IWCMD_CM_SS1_NP: //スラスターサーボ1を負位置
00302         Servo_pulsewidth_target[1-1] = Servo_1_pulsewidth_range[0];
00303         break;
00304     case IWCMD_CM_SS2_PP: //スラスターサーボ2を正位置
00305         Servo_pulsewidth_target[2-1] = Servo_2_pulsewidth_range[2];
00306         break;
00307     case IWCMD_CM_SS2_SP: //スラスターサーボ2を零位置
00308         Servo_pulsewidth_target[2-1] = Servo_2_pulsewidth_range[1];
00309         break;
00310     case IWCMD_CM_SS2_NP: //スラスターサーボ2を負位置
00311         Servo_pulsewidth_target[2-1] = Servo_2_pulsewidth_range[0];
00312         break;
00313     case IWCMD_CM_SS3_PP: //スラスターサーボ3を正位置
00314         Servo_pulsewidth_target[3-1] = Servo_3_pulsewidth_range[2];
00315         break;
00316     case IWCMD_CM_SS3_SP: //スラスターサーボ3を零位置
00317         Servo_pulsewidth_target[3-1] = Servo_3_pulsewidth_range[1];
00318         break;
00319     case IWCMD_CM_SS3_NP: //スラスターサーボ3を負位置
00320         Servo_pulsewidth_target[3-1] = Servo_3_pulsewidth_range[0];
00321         break;
00322     case IWCMD_CM_SS4_PP: //スラスターサーボ4を正位置
00323         Servo_pulsewidth_target[4-1] = Servo_4_pulsewidth_range[2];
00324         break;
00325     case IWCMD_CM_SS4_SP: //スラスターサーボ4を零位置
00326         Servo_pulsewidth_target[4-1] = Servo_4_pulsewidth_range[1];
00327         break;
00328     case IWCMD_CM_SS4_NP: //スラスターサーボ4を負位置
00329         Servo_pulsewidth_target[4-1] = Servo_4_pulsewidth_range[0];
00330         break;
00331     case IWCMD_CM_SS5_PP: //スラスターサーボ5を正位置
00332         Servo_pulsewidth_target[5-1] = Servo_5_pulsewidth_range[2];
00333         break;
00334     case IWCMD_CM_SS5_SP: //スラスターサーボ5を零位置
00335         Servo_pulsewidth_target[5-1] = Servo_5_pulsewidth_range[1];
00336         break;
00337     case IWCMD_CM_SS5_NP: //スラスターサーボ5を負位置
00338         Servo_pulsewidth_target[5-1] = Servo_5_pulsewidth_range[0];
00339         break;
00340     case IWCMD_CM_SS6_PP: //スラスターサーボ6を正位置
00341         Servo_pulsewidth_target[6-1] = Servo_6_pulsewidth_range[2];
00342         break;
00343     case IWCMD_CM_SS6_SP: //スラスターサーボ6を零位置
00344         Servo_pulsewidth_target[6-1] = Servo_6_pulsewidth_range[1];
00345         break;
00346     case IWCMD_CM_SS6_NP: //スラスターサーボ6を負位置
00347         Servo_pulsewidth_target[6-1] = Servo_6_pulsewidth_range[0];
00348         break;
00349     case IWCMD_CM_PM1_FR: //PM1を正転(IWCMD_PM1_SUU)
00350         xbee.putc(IWCMD_PM1_SUU);
00351         break;
00352     case IWCMD_CM_PM1_ST: //PM1を停止(IWCMD_PM1_SPS)
00353         xbee.putc(IWCMD_PM1_SPS);
00354         break;
00355     case IWCMD_CM_PM2_FR: //PM2を正転(IWCMD_PM2_SUU)
00356         xbee.putc(IWCMD_PM2_SUU);
00357         break;
00358     case IWCMD_CM_PM2_ST: //PM2を停止(IWCMD_PM2_SPS)
00359         xbee.putc(IWCMD_PM2_SPS);
00360         break;
00361     default:
00362         reset_all_motors();
00363         break;
00364     }
00365     led[2] = !led[2];
00366 }