main.cpp

00001 #include "mbed.h"                           //mbed関数
00002 #include "Suteppa.h"                        //ステッピングモータ関数
00003 #include "ServoMotor.h"                     //駆動速度を設定可能なサーボモータ関数
00004 #include "Serial.h"                         //シリアル通信関数
00005 
00006 //マイクロステップ駆動段階(ステッピングモータの1ステップをさらにいくつで分割するか)
00007 #define MICRO_STEP  128     
00008  
00009 Serial usbSerial(USBTX, USBRX);             //PCとの接続用
00010 DigitalIn LimitSW(D2);                      //方位角0セット用リミットスイッチ
00011 SERVO_MOTOR Elv(D10, 90, 590, 0, 1490);     //仰角用サーボモータ(使用ピン,角度1,パルス幅1,角度2,パルス幅2)
00012 Suteppa Azm;                                //方位角用ステッピングモータ
00013 
00014 //方位角ステッピングモータ マイクロステップ駆動用PWMピン
00015 PwmOut AIN1(D7);
00016 PwmOut AIN2(D6);
00017 PwmOut BIN1(D5);
00018 PwmOut BIN2(D4);
00019 
00020 
00021 //方位角ゼロ点設定
00022 void Azm_home(bool init = true){
00023     //10ステップを加減速に使い, 開始速度は10000us
00024     Azm.beginSmooth(10*MICRO_STEP, 10000);
00025     //最終速度は1000us
00026     Azm.setSpeed(1000);
00027 
00028     if(init == false){
00029         Azm.rotate(Suteppa::ABSOLUTE, 0*MICRO_STEP);
00030         wait(0.5);
00031     }
00032     
00033     //リミットスイッチが反応している間は時計回りに回す。
00034     Azm.rotate(Suteppa::RELATIVE, 100*MICRO_STEP,false);
00035     while(LimitSW == 0 && Azm.tick());
00036 
00037     //リミットスイッチが反応するまで反時計回りに回す。
00038     Azm.rotate(Suteppa::RELATIVE, -100*MICRO_STEP,false);
00039     while(LimitSW == 1 && Azm.tick());
00040 
00041     //少し時計回りに回す。
00042     Azm.rotate(Suteppa::RELATIVE, 1*MICRO_STEP,true);
00043 
00044     //リミットスイッチが反応するまで反時計回りに回す。
00045     Azm.rotate(Suteppa::RELATIVE, -100*MICRO_STEP,false);
00046     while(LimitSW == 1 && Azm.tick());
00047 
00048     //ゼロ点セット
00049     Azm.setHome();
00050     Azm.rotate(Suteppa::RELATIVE, 0,true);
00051 }
00052 
00053 //d(= -1 or +1 )ステップ変える関数
00054 void step(int d){
00055 
00056     static int i = 0;   //現在ステップ数記憶用
00057     double x,y;         //マイクロステップ用電圧値計算用
00058     i -= d;             //1ステップ増やす or 戻す
00059 
00060     //ステップ数を0~MICRO_STEPの間に収める
00061     while(i >= MICRO_STEP){
00062         i -= MICRO_STEP;
00063     }
00064     while(i < 0){
00065         i+= MICRO_STEP;
00066     }
00067 
00068     //マイクロステップ駆動のための電圧値計算    
00069     x = cos(2.0 * M_PI * i / MICRO_STEP);
00070     y = sin(2.0 * M_PI * i / MICRO_STEP);
00071     
00072     //ステッピングモータA相に電圧印加
00073     if(x > 0){
00074         AIN1 =  1-x;
00075         AIN2 =  1;
00076     }else{
00077         AIN1 =  1;
00078         AIN2 =  1+x;
00079     }
00080     
00081     //ステッピングモータB相に電圧印加
00082     if(y > 0){
00083         BIN1 =  1-y;
00084         BIN2 =  1;
00085     }else{
00086         BIN1 =  1;
00087         BIN2 =  1+y;
00088     }    
00089     
00090 }
00091 
00092 //方位角と仰角を設定して向ける
00093 void Antenna(double Azm_angle, double Elv_angle, bool sync = false){
00094     Azm.rotate(Suteppa::ABSOLUTE,100 * MICRO_STEP * Azm_angle / 360.0,sync);
00095     Elv.move(Elv_angle,sync);
00096 }
00097 
00098 //現在の方位角を取得する
00099 double get_Azm(void){
00100     return 360.0 * Azm.getStep() / 100 / MICRO_STEP;
00101 }
00102 
00103 //現在の仰角を取得する
00104 double get_Elv(void){
00105     return Elv.get_angle();
00106 }
00107 
00108 //ステッピングモータの処理を進める。
00109 bool tick(void){
00110     bool Azm_result, Elv_result;
00111     Azm_result = Azm.tick();
00112     Elv_result = Elv.tick();
00113     
00114     return Azm_result || Elv_result;
00115 }
00116 
00117 int main() {
00118 
00119     // シリアル通信の速度設定
00120     usbSerial.baud(115200);
00121     char str[256];
00122     int i = 0;
00123 
00124     //指令値保存用
00125     double AzmAngle,ElvAngle;
00126 
00127     int cnt = 0;    //通信エラー排除用カウンタ値
00128 
00129     //PWM周期100us
00130     AIN1.period(1e-4);
00131     AIN2.period(1e-4);
00132     BIN1.period(1e-4);
00133     BIN2.period(1e-4);
00134 
00135     //一回転のステップ数, ステップ用関数
00136     Azm.init(100*MICRO_STEP, step);
00137     //10ステップを加減速に使い, 開始速度は10000us
00138     Azm.beginSmooth(10*MICRO_STEP, 10000);
00139     //通常速度は1000us
00140     Azm.setSpeed(1000);
00141 
00142     //方位角を0度にリセットする
00143     Azm_home();
00144     //仰角を0度にセットする。
00145     Elv.go(0);
00146 
00147     //制御ループ
00148     while(1){
00149 
00150         //PCから指令値受信 & 翻訳
00151         if(usbSerial.readable()){
00152             str[i] = usbSerial.getc();
00153             if(str[i] == '\n' || str[i] == '\r'){
00154                 str[i] = '\0';
00155 
00156                 //制御停止要求
00157                 if(strncmp(str,"SA SE ",6) == 0){
00158                     Azm_home(false);
00159                     Elv.move(0,true);
00160 
00161                 //現在角 読み込み要求
00162                 }else if(strncmp(str,"AZ EL ",6) == 0){
00163                     usbSerial.printf("AZ%.1f EL%.1f \n",get_Azm(),get_Elv());
00164 
00165                 //指令値受信
00166                 }else if( sscanf(str,"%lf,%lf",&AzmAngle,&ElvAngle) == 2 || sscanf(str,"AZ%lf EL%lf ",&AzmAngle,&ElvAngle) == 2 ){
00167                     //方位角を0~360度に制限
00168                     while(AzmAngle < 0){
00169                         AzmAngle += 360;
00170                     }
00171                     while(AzmAngle > 360){
00172                         AzmAngle -= 360;
00173                     }
00174 
00175                     //仰角を0~90度に制限
00176                     if(ElvAngle < 0){
00177                         ElvAngle = 0;
00178                     }
00179                     if(ElvAngle > 90){
00180                         ElvAngle = 90;
00181                     }
00182 
00183                     //現在の方位角より、指令値に30度~ の差があるとき（速度早め）
00184                     if(abs(AzmAngle - get_Azm())>30){
00185 
00186                         //同じ値が3回以上きたら適用する（たまに仰角方位角ともに0度のエラー値を送ってくるためその対策）
00187                         if(cnt < 3){
00188                             cnt++;
00189                         }else{
00190                             Azm.endSmooth();
00191                             //通常速度は700us
00192                             Azm.setSpeed(700);
00193                             Antenna(AzmAngle,ElvAngle,false);
00194                         }
00195 
00196                     //現在の方位角より、指令値に5度~30度の差があるとき（速度ふつう）
00197                     }else if(abs(AzmAngle - get_Azm())>5){
00198                         Azm.endSmooth();
00199                         //通常速度は7000us
00200                         Azm.setSpeed(7000);
00201                         Antenna(AzmAngle,ElvAngle,false);
00202                         cnt = 0;
00203 
00204                     //現在の方位角より、指令値に ~5度の差があるとき（速度ゆっくり）
00205                     }else{
00206                         Azm.endSmooth();
00207                         //通常速度は20000us
00208                         Azm.setSpeed(20000);
00209                         Antenna(AzmAngle,ElvAngle,false);
00210                         cnt = 0;
00211                     }
00212                 }                
00213                 i = 0;  //シリアル受信文字数をリセット
00214             }else{
00215                 i++;    //シリアル受信文字数をカウントアップ
00216             }
00217             //文字バッファ満杯時にシリアル受信文字数をリセット
00218             if(i == 256){
00219                 i = 0;
00220             }
00221         }
00222         tick();     //ステッピングモータ処理
00223     }
00224 }