File content as of revision 1:656a0c325626:

#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();     //ステッピングモータ処理
    }
}