Diff: main.cpp

Revision:

0:022e5e32d580

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Mar 08 03:39:18 2020 +0000
@@ -0,0 +1,224 @@
+#include "mbed.h"                           //mbed関数
+#include "Suteppa.h"                        //ステッピングモータ関数
+#include "ServoMotor.h"                     //駆動速度を設定可能なサーボモータ関数
+#include "Serial.h"                         //シリアル通信関数
+
+//マイクロステップ駆動段階(ステッピングモータの1ステップをさらにいくつで分割するか)
+#define MICRO_STEP  128     
+ 
+Serial usbSerial(USBTX, USBRX);             //PCとの接続用
+DigitalIn LimitSW(D2);                      //方位角0セット用リミットスイッチ
+SERVO_MOTOR Elv(D10, 90, 590, 0, 1490);     //仰角用サーボモータ(使用ピン,角度1,パルス幅1,角度2,パルス幅2)
+Suteppa Azm;                                //方位角用ステッピングモータ
+
+//方位角ステッピングモータ マイクロステップ駆動用PWMピン
+PwmOut AIN1(D7);
+PwmOut AIN2(D6);
+PwmOut BIN1(D5);
+PwmOut BIN2(D4);
+
+
+//方位角ゼロ点設定
+void Azm_home(bool init = true){
+    //10ステップを加減速に使い, 開始速度は10000us
+    Azm.beginSmooth(10*MICRO_STEP, 10000);
+    //最終速度は1000us
+    Azm.setSpeed(1000);
+
+    if(init == false){
+        Azm.rotate(Suteppa::ABSOLUTE, 0*MICRO_STEP);
+        wait(0.5);
+    }
+    
+    //リミットスイッチが反応している間は時計回りに回す。
+    Azm.rotate(Suteppa::RELATIVE, 100*MICRO_STEP,false);
+    while(LimitSW == 0 && Azm.tick());
+
+    //リミットスイッチが反応するまで反時計回りに回す。
+    Azm.rotate(Suteppa::RELATIVE, -100*MICRO_STEP,false);
+    while(LimitSW == 1 && Azm.tick());
+
+    //少し時計回りに回す。
+    Azm.rotate(Suteppa::RELATIVE, 1*MICRO_STEP,true);
+
+    //リミットスイッチが反応するまで反時計回りに回す。
+    Azm.rotate(Suteppa::RELATIVE, -100*MICRO_STEP,false);
+    while(LimitSW == 1 && Azm.tick());
+
+    //ゼロ点セット
+    Azm.setHome();
+    Azm.rotate(Suteppa::RELATIVE, 0,true);
+}
+
+//d(= -1 or +1 )ステップ変える関数
+void step(int d){
+
+    static int i = 0;   //現在ステップ数記憶用
+    double x,y;         //マイクロステップ用電圧値計算用
+    i -= d;             //1ステップ増やす or 戻す
+
+    //ステップ数を0~MICRO_STEPの間に収める
+    while(i >= MICRO_STEP){
+        i -= MICRO_STEP;
+    }
+    while(i < 0){
+        i+= MICRO_STEP;
+    }
+
+    //マイクロステップ駆動のための電圧値計算    
+    x = cos(2.0 * M_PI * i / MICRO_STEP);
+    y = sin(2.0 * M_PI * i / MICRO_STEP);
+    
+    //ステッピングモータA相に電圧印加
+    if(x > 0){
+        AIN1 =  1-x;
+        AIN2 =  1;
+    }else{
+        AIN1 =  1;
+        AIN2 =  1+x;
+    }
+    
+    //ステッピングモータB相に電圧印加
+    if(y > 0){
+        BIN1 =  1-y;
+        BIN2 =  1;
+    }else{
+        BIN1 =  1;
+        BIN2 =  1+y;
+    }    
+    
+}
+
+//方位角と仰角を設定して向ける
+void Antenna(double Azm_angle, double Elv_angle, bool sync = false){
+    Azm.rotate(Suteppa::ABSOLUTE,100 * MICRO_STEP * Azm_angle / 360.0,sync);
+    Elv.move(Elv_angle,sync);
+}
+
+//現在の方位角を取得する
+double get_Azm(void){
+    return 360.0 * Azm.getStep() / 100 / MICRO_STEP;
+}
+
+//現在の仰角を取得する
+double get_Elv(void){
+    return Elv.get_angle();
+}
+
+//ステッピングモータの処理を進める。
+bool tick(void){
+    bool Azm_result, Elv_result;
+    Azm_result = Azm.tick();
+    Elv_result = Elv.tick();
+    
+    return Azm_result || Elv_result;
+}
+
+int main() {
+
+    // シリアル通信の速度設定
+    usbSerial.baud(115200);
+    char str[256];
+    int i = 0;
+
+    //指令値保存用
+    double AzmAngle,ElvAngle;
+
+    int cnt = 0;    //通信エラー排除用カウンタ値
+
+    //PWM周期100us
+    AIN1.period(1e-4);
+    AIN2.period(1e-4);
+    BIN1.period(1e-4);
+    BIN2.period(1e-4);
+
+    //一回転のステップ数, ステップ用関数
+    Azm.init(100*MICRO_STEP, step);
+    //10ステップを加減速に使い, 開始速度は10000us
+    Azm.beginSmooth(10*MICRO_STEP, 10000);
+    //通常速度は1000us
+    Azm.setSpeed(1000);
+
+    //方位角を0度にリセットする
+    Azm_home();
+    //仰角を0度にセットする。
+    Elv.go(0);
+
+    //制御ループ
+    while(1){
+
+        //PCから指令値受信 & 翻訳
+        if(usbSerial.readable()){
+            str[i] = usbSerial.getc();
+            if(str[i] == '\n' || str[i] == '\r'){
+                str[i] = '\0';
+
+                //制御停止要求
+                if(strncmp(str,"SA SE ",6) == 0){
+                    Azm_home(false);
+                    Elv.move(0,true);
+
+                //現在角 読み込み要求
+                }else if(strncmp(str,"AZ EL ",6) == 0){
+                    usbSerial.printf("AZ%.1f EL%.1f \n",get_Azm(),get_Elv());
+
+                //指令値受信
+                }else if( sscanf(str,"%lf,%lf",&AzmAngle,&ElvAngle) == 2 || sscanf(str,"AZ%lf EL%lf ",&AzmAngle,&ElvAngle) == 2 ){
+                    //方位角を0~360度に制限
+                    while(AzmAngle < 0){
+                        AzmAngle += 360;
+                    }
+                    while(AzmAngle > 360){
+                        AzmAngle -= 360;
+                    }
+
+                    //仰角を0~90度に制限
+                    if(ElvAngle < 0){
+                        ElvAngle = 0;
+                    }
+                    if(ElvAngle > 90){
+                        ElvAngle = 90;
+                    }
+
+                    //現在の方位角より、指令値に30度~ の差があるとき（速度早め）
+                    if(abs(AzmAngle - get_Azm())>30){
+
+                        //同じ値が3回以上きたら適用する（たまに仰角方位角ともに0度のエラー値を送ってくるためその対策）
+                        if(cnt < 3){
+                            cnt++;
+                        }else{
+                            Azm.endSmooth();
+                            //通常速度は700us
+                            Azm.setSpeed(700);
+                            Antenna(AzmAngle,ElvAngle,false);
+                        }
+
+                    //現在の方位角より、指令値に5度~30度の差があるとき（速度ふつう）
+                    }else if(abs(AzmAngle - get_Azm())>5){
+                        Azm.endSmooth();
+                        //通常速度は7000us
+                        Azm.setSpeed(7000);
+                        Antenna(AzmAngle,ElvAngle,false);
+                        cnt = 0;
+
+                    //現在の方位角より、指令値に ~5度の差があるとき（速度ゆっくり）
+                    }else{
+                        Azm.endSmooth();
+                        //通常速度は20000us
+                        Azm.setSpeed(20000);
+                        Antenna(AzmAngle,ElvAngle,false);
+                        cnt = 0;
+                    }
+                }                
+                i = 0;  //シリアル受信文字数をリセット
+            }else{
+                i++;    //シリアル受信文字数をカウントアップ
+            }
+            //文字バッファ満杯時にシリアル受信文字数をリセット
+            if(i == 256){
+                i = 0;
+            }
+        }
+        tick();     //ステッピングモータ処理
+    }
+}