Daisuke Nakayama / Mbed 2 deprecated yagi_Rotator_Ver2

Antenna rotator for Nano Ground Station

Dependencies:   mbed ServoMotor Suteppa

main.cpp@1:656a0c325626, 2020-03-08 (annotated)

Committer:
j_rocket_boy
Date:
Sun Mar 08 03:41:47 2020 +0000
Revision:
1:656a0c325626
Parent:
0:022e5e32d580 
from Kyutech

Who changed what in which revision?

UserRevisionLine numberNew contents of line
j_rocket_boy 0:022e5e32d580 1 #include "mbed.h" //mbed関数
j_rocket_boy 0:022e5e32d580 2 #include "Suteppa.h" //ステッピングモータ関数
j_rocket_boy 0:022e5e32d580 3 #include "ServoMotor.h" //駆動速度を設定可能なサーボモータ関数
j_rocket_boy 0:022e5e32d580 4 #include "Serial.h" //シリアル通信関数
j_rocket_boy 0:022e5e32d580 5
j_rocket_boy 0:022e5e32d580 6 //マイクロステップ駆動段階(ステッピングモータの1ステップをさらにいくつで分割するか)
j_rocket_boy 0:022e5e32d580 7 #define MICRO_STEP 128
j_rocket_boy 0:022e5e32d580 8
j_rocket_boy 0:022e5e32d580 9 Serial usbSerial(USBTX, USBRX); //PCとの接続用
j_rocket_boy 0:022e5e32d580 10 DigitalIn LimitSW(D2); //方位角0セット用リミットスイッチ
j_rocket_boy 0:022e5e32d580 11 SERVO_MOTOR Elv(D10, 90, 590, 0, 1490); //仰角用サーボモータ(使用ピン,角度1,パルス幅1,角度2,パルス幅2)
j_rocket_boy 0:022e5e32d580 12 Suteppa Azm; //方位角用ステッピングモータ
j_rocket_boy 0:022e5e32d580 13
j_rocket_boy 0:022e5e32d580 14 //方位角ステッピングモータ マイクロステップ駆動用PWMピン
j_rocket_boy 0:022e5e32d580 15 PwmOut AIN1(D7);
j_rocket_boy 0:022e5e32d580 16 PwmOut AIN2(D6);
j_rocket_boy 0:022e5e32d580 17 PwmOut BIN1(D5);
j_rocket_boy 0:022e5e32d580 18 PwmOut BIN2(D4);
j_rocket_boy 0:022e5e32d580 19
j_rocket_boy 0:022e5e32d580 20
j_rocket_boy 0:022e5e32d580 21 //方位角ゼロ点設定
j_rocket_boy 0:022e5e32d580 22 void Azm_home(bool init = true){
j_rocket_boy 0:022e5e32d580 23 //10ステップを加減速に使い, 開始速度は10000us
j_rocket_boy 0:022e5e32d580 24 Azm.beginSmooth(10*MICRO_STEP, 10000);
j_rocket_boy 0:022e5e32d580 25 //最終速度は1000us
j_rocket_boy 0:022e5e32d580 26 Azm.setSpeed(1000);
j_rocket_boy 0:022e5e32d580 27
j_rocket_boy 0:022e5e32d580 28 if(init == false){
j_rocket_boy 0:022e5e32d580 29 Azm.rotate(Suteppa::ABSOLUTE, 0*MICRO_STEP);
j_rocket_boy 0:022e5e32d580 30 wait(0.5);
j_rocket_boy 0:022e5e32d580 31 }
j_rocket_boy 0:022e5e32d580 32
j_rocket_boy 0:022e5e32d580 33 //リミットスイッチが反応している間は時計回りに回す。
j_rocket_boy 0:022e5e32d580 34 Azm.rotate(Suteppa::RELATIVE, 100*MICRO_STEP,false);
j_rocket_boy 0:022e5e32d580 35 while(LimitSW == 0 && Azm.tick());
j_rocket_boy 0:022e5e32d580 36
j_rocket_boy 0:022e5e32d580 37 //リミットスイッチが反応するまで反時計回りに回す。
j_rocket_boy 0:022e5e32d580 38 Azm.rotate(Suteppa::RELATIVE, -100*MICRO_STEP,false);
j_rocket_boy 0:022e5e32d580 39 while(LimitSW == 1 && Azm.tick());
j_rocket_boy 0:022e5e32d580 40
j_rocket_boy 0:022e5e32d580 41 //少し時計回りに回す。
j_rocket_boy 0:022e5e32d580 42 Azm.rotate(Suteppa::RELATIVE, 1*MICRO_STEP,true);
j_rocket_boy 0:022e5e32d580 43
j_rocket_boy 0:022e5e32d580 44 //リミットスイッチが反応するまで反時計回りに回す。
j_rocket_boy 0:022e5e32d580 45 Azm.rotate(Suteppa::RELATIVE, -100*MICRO_STEP,false);
j_rocket_boy 0:022e5e32d580 46 while(LimitSW == 1 && Azm.tick());
j_rocket_boy 0:022e5e32d580 47
j_rocket_boy 0:022e5e32d580 48 //ゼロ点セット
j_rocket_boy 0:022e5e32d580 49 Azm.setHome();
j_rocket_boy 0:022e5e32d580 50 Azm.rotate(Suteppa::RELATIVE, 0,true);
j_rocket_boy 0:022e5e32d580 51 }
j_rocket_boy 0:022e5e32d580 52
j_rocket_boy 0:022e5e32d580 53 //d(= -1 or +1 )ステップ変える関数
j_rocket_boy 0:022e5e32d580 54 void step(int d){
j_rocket_boy 0:022e5e32d580 55
j_rocket_boy 0:022e5e32d580 56 static int i = 0; //現在ステップ数記憶用
j_rocket_boy 0:022e5e32d580 57 double x,y; //マイクロステップ用電圧値計算用
j_rocket_boy 0:022e5e32d580 58 i -= d; //1ステップ増やす or 戻す
j_rocket_boy 0:022e5e32d580 59
j_rocket_boy 0:022e5e32d580 60 //ステップ数を0~MICRO_STEPの間に収める
j_rocket_boy 0:022e5e32d580 61 while(i >= MICRO_STEP){
j_rocket_boy 0:022e5e32d580 62 i -= MICRO_STEP;
j_rocket_boy 0:022e5e32d580 63 }
j_rocket_boy 0:022e5e32d580 64 while(i < 0){
j_rocket_boy 0:022e5e32d580 65 i+= MICRO_STEP;
j_rocket_boy 0:022e5e32d580 66 }
j_rocket_boy 0:022e5e32d580 67
j_rocket_boy 0:022e5e32d580 68 //マイクロステップ駆動のための電圧値計算
j_rocket_boy 0:022e5e32d580 69 x = cos(2.0 * M_PI * i / MICRO_STEP);
j_rocket_boy 0:022e5e32d580 70 y = sin(2.0 * M_PI * i / MICRO_STEP);
j_rocket_boy 0:022e5e32d580 71
j_rocket_boy 0:022e5e32d580 72 //ステッピングモータA相に電圧印加
j_rocket_boy 0:022e5e32d580 73 if(x > 0){
j_rocket_boy 0:022e5e32d580 74 AIN1 = 1-x;
j_rocket_boy 0:022e5e32d580 75 AIN2 = 1;
j_rocket_boy 0:022e5e32d580 76 }else{
j_rocket_boy 0:022e5e32d580 77 AIN1 = 1;
j_rocket_boy 0:022e5e32d580 78 AIN2 = 1+x;
j_rocket_boy 0:022e5e32d580 79 }
j_rocket_boy 0:022e5e32d580 80
j_rocket_boy 0:022e5e32d580 81 //ステッピングモータB相に電圧印加
j_rocket_boy 0:022e5e32d580 82 if(y > 0){
j_rocket_boy 0:022e5e32d580 83 BIN1 = 1-y;
j_rocket_boy 0:022e5e32d580 84 BIN2 = 1;
j_rocket_boy 0:022e5e32d580 85 }else{
j_rocket_boy 0:022e5e32d580 86 BIN1 = 1;
j_rocket_boy 0:022e5e32d580 87 BIN2 = 1+y;
j_rocket_boy 0:022e5e32d580 88 }
j_rocket_boy 0:022e5e32d580 89
j_rocket_boy 0:022e5e32d580 90 }
j_rocket_boy 0:022e5e32d580 91
j_rocket_boy 0:022e5e32d580 92 //方位角と仰角を設定して向ける
j_rocket_boy 0:022e5e32d580 93 void Antenna(double Azm_angle, double Elv_angle, bool sync = false){
j_rocket_boy 0:022e5e32d580 94 Azm.rotate(Suteppa::ABSOLUTE,100 * MICRO_STEP * Azm_angle / 360.0,sync);
j_rocket_boy 0:022e5e32d580 95 Elv.move(Elv_angle,sync);
j_rocket_boy 0:022e5e32d580 96 }
j_rocket_boy 0:022e5e32d580 97
j_rocket_boy 0:022e5e32d580 98 //現在の方位角を取得する
j_rocket_boy 0:022e5e32d580 99 double get_Azm(void){
j_rocket_boy 0:022e5e32d580 100 return 360.0 * Azm.getStep() / 100 / MICRO_STEP;
j_rocket_boy 0:022e5e32d580 101 }
j_rocket_boy 0:022e5e32d580 102
j_rocket_boy 0:022e5e32d580 103 //現在の仰角を取得する
j_rocket_boy 0:022e5e32d580 104 double get_Elv(void){
j_rocket_boy 0:022e5e32d580 105 return Elv.get_angle();
j_rocket_boy 0:022e5e32d580 106 }
j_rocket_boy 0:022e5e32d580 107
j_rocket_boy 0:022e5e32d580 108 //ステッピングモータの処理を進める。
j_rocket_boy 0:022e5e32d580 109 bool tick(void){
j_rocket_boy 0:022e5e32d580 110 bool Azm_result, Elv_result;
j_rocket_boy 0:022e5e32d580 111 Azm_result = Azm.tick();
j_rocket_boy 0:022e5e32d580 112 Elv_result = Elv.tick();
j_rocket_boy 0:022e5e32d580 113
j_rocket_boy 0:022e5e32d580 114 return Azm_result || Elv_result;
j_rocket_boy 0:022e5e32d580 115 }
j_rocket_boy 0:022e5e32d580 116
j_rocket_boy 0:022e5e32d580 117 int main() {
j_rocket_boy 0:022e5e32d580 118
j_rocket_boy 0:022e5e32d580 119 // シリアル通信の速度設定
j_rocket_boy 0:022e5e32d580 120 usbSerial.baud(115200);
j_rocket_boy 0:022e5e32d580 121 char str[256];
j_rocket_boy 0:022e5e32d580 122 int i = 0;
j_rocket_boy 0:022e5e32d580 123
j_rocket_boy 0:022e5e32d580 124 //指令値保存用
j_rocket_boy 0:022e5e32d580 125 double AzmAngle,ElvAngle;
j_rocket_boy 0:022e5e32d580 126
j_rocket_boy 0:022e5e32d580 127 int cnt = 0; //通信エラー排除用カウンタ値
j_rocket_boy 0:022e5e32d580 128
j_rocket_boy 0:022e5e32d580 129 //PWM周期100us
j_rocket_boy 0:022e5e32d580 130 AIN1.period(1e-4);
j_rocket_boy 0:022e5e32d580 131 AIN2.period(1e-4);
j_rocket_boy 0:022e5e32d580 132 BIN1.period(1e-4);
j_rocket_boy 0:022e5e32d580 133 BIN2.period(1e-4);
j_rocket_boy 0:022e5e32d580 134
j_rocket_boy 0:022e5e32d580 135 //一回転のステップ数, ステップ用関数
j_rocket_boy 0:022e5e32d580 136 Azm.init(100*MICRO_STEP, step);
j_rocket_boy 0:022e5e32d580 137 //10ステップを加減速に使い, 開始速度は10000us
j_rocket_boy 0:022e5e32d580 138 Azm.beginSmooth(10*MICRO_STEP, 10000);
j_rocket_boy 0:022e5e32d580 139 //通常速度は1000us
j_rocket_boy 0:022e5e32d580 140 Azm.setSpeed(1000);
j_rocket_boy 0:022e5e32d580 141
j_rocket_boy 0:022e5e32d580 142 //方位角を0度にリセットする
j_rocket_boy 0:022e5e32d580 143 Azm_home();
j_rocket_boy 0:022e5e32d580 144 //仰角を0度にセットする。
j_rocket_boy 0:022e5e32d580 145 Elv.go(0);
j_rocket_boy 0:022e5e32d580 146
j_rocket_boy 0:022e5e32d580 147 //制御ループ
j_rocket_boy 0:022e5e32d580 148 while(1){
j_rocket_boy 0:022e5e32d580 149
j_rocket_boy 0:022e5e32d580 150 //PCから指令値受信 & 翻訳
j_rocket_boy 0:022e5e32d580 151 if(usbSerial.readable()){
j_rocket_boy 0:022e5e32d580 152 str[i] = usbSerial.getc();
j_rocket_boy 0:022e5e32d580 153 if(str[i] == '\n' || str[i] == '\r'){
j_rocket_boy 0:022e5e32d580 154 str[i] = '\0';
j_rocket_boy 0:022e5e32d580 155
j_rocket_boy 0:022e5e32d580 156 //制御停止要求
j_rocket_boy 0:022e5e32d580 157 if(strncmp(str,"SA SE ",6) == 0){
j_rocket_boy 0:022e5e32d580 158 Azm_home(false);
j_rocket_boy 0:022e5e32d580 159 Elv.move(0,true);
j_rocket_boy 0:022e5e32d580 160
j_rocket_boy 0:022e5e32d580 161 //現在角 読み込み要求
j_rocket_boy 0:022e5e32d580 162 }else if(strncmp(str,"AZ EL ",6) == 0){
j_rocket_boy 0:022e5e32d580 163 usbSerial.printf("AZ%.1f EL%.1f \n",get_Azm(),get_Elv());
j_rocket_boy 0:022e5e32d580 164
j_rocket_boy 0:022e5e32d580 165 //指令値受信
j_rocket_boy 0:022e5e32d580 166 }else if( sscanf(str,"%lf,%lf",&AzmAngle,&ElvAngle) == 2 || sscanf(str,"AZ%lf EL%lf ",&AzmAngle,&ElvAngle) == 2 ){
j_rocket_boy 0:022e5e32d580 167 //方位角を0~360度に制限
j_rocket_boy 0:022e5e32d580 168 while(AzmAngle < 0){
j_rocket_boy 0:022e5e32d580 169 AzmAngle += 360;
j_rocket_boy 0:022e5e32d580 170 }
j_rocket_boy 0:022e5e32d580 171 while(AzmAngle > 360){
j_rocket_boy 0:022e5e32d580 172 AzmAngle -= 360;
j_rocket_boy 0:022e5e32d580 173 }
j_rocket_boy 0:022e5e32d580 174
j_rocket_boy 0:022e5e32d580 175 //仰角を0~90度に制限
j_rocket_boy 0:022e5e32d580 176 if(ElvAngle < 0){
j_rocket_boy 0:022e5e32d580 177 ElvAngle = 0;
j_rocket_boy 0:022e5e32d580 178 }
j_rocket_boy 0:022e5e32d580 179 if(ElvAngle > 90){
j_rocket_boy 0:022e5e32d580 180 ElvAngle = 90;
j_rocket_boy 0:022e5e32d580 181 }
j_rocket_boy 0:022e5e32d580 182
j_rocket_boy 0:022e5e32d580 183 //現在の方位角より、指令値に30度~ の差があるとき（速度早め）
j_rocket_boy 0:022e5e32d580 184 if(abs(AzmAngle - get_Azm())>30){
j_rocket_boy 0:022e5e32d580 185
j_rocket_boy 0:022e5e32d580 186 //同じ値が3回以上きたら適用する（たまに仰角方位角ともに0度のエラー値を送ってくるためその対策）
j_rocket_boy 0:022e5e32d580 187 if(cnt < 3){
j_rocket_boy 0:022e5e32d580 188 cnt++;
j_rocket_boy 0:022e5e32d580 189 }else{
j_rocket_boy 0:022e5e32d580 190 Azm.endSmooth();
j_rocket_boy 0:022e5e32d580 191 //通常速度は700us
j_rocket_boy 0:022e5e32d580 192 Azm.setSpeed(700);
j_rocket_boy 0:022e5e32d580 193 Antenna(AzmAngle,ElvAngle,false);
j_rocket_boy 0:022e5e32d580 194 }
j_rocket_boy 0:022e5e32d580 195
j_rocket_boy 0:022e5e32d580 196 //現在の方位角より、指令値に5度~30度の差があるとき（速度ふつう）
j_rocket_boy 0:022e5e32d580 197 }else if(abs(AzmAngle - get_Azm())>5){
j_rocket_boy 0:022e5e32d580 198 Azm.endSmooth();
j_rocket_boy 0:022e5e32d580 199 //通常速度は7000us
j_rocket_boy 0:022e5e32d580 200 Azm.setSpeed(7000);
j_rocket_boy 0:022e5e32d580 201 Antenna(AzmAngle,ElvAngle,false);
j_rocket_boy 0:022e5e32d580 202 cnt = 0;
j_rocket_boy 0:022e5e32d580 203
j_rocket_boy 0:022e5e32d580 204 //現在の方位角より、指令値に ~5度の差があるとき（速度ゆっくり）
j_rocket_boy 0:022e5e32d580 205 }else{
j_rocket_boy 0:022e5e32d580 206 Azm.endSmooth();
j_rocket_boy 0:022e5e32d580 207 //通常速度は20000us
j_rocket_boy 0:022e5e32d580 208 Azm.setSpeed(20000);
j_rocket_boy 0:022e5e32d580 209 Antenna(AzmAngle,ElvAngle,false);
j_rocket_boy 0:022e5e32d580 210 cnt = 0;
j_rocket_boy 0:022e5e32d580 211 }
j_rocket_boy 0:022e5e32d580 212 }
j_rocket_boy 0:022e5e32d580 213 i = 0; //シリアル受信文字数をリセット
j_rocket_boy 0:022e5e32d580 214 }else{
j_rocket_boy 0:022e5e32d580 215 i++; //シリアル受信文字数をカウントアップ
j_rocket_boy 0:022e5e32d580 216 }
j_rocket_boy 0:022e5e32d580 217 //文字バッファ満杯時にシリアル受信文字数をリセット
j_rocket_boy 0:022e5e32d580 218 if(i == 256){
j_rocket_boy 0:022e5e32d580 219 i = 0;
j_rocket_boy 0:022e5e32d580 220 }
j_rocket_boy 0:022e5e32d580 221 }
j_rocket_boy 0:022e5e32d580 222 tick(); //ステッピングモータ処理
j_rocket_boy 0:022e5e32d580 223 }
j_rocket_boy 0:022e5e32d580 224 }