Antenna rotator for Nano Ground Station
Dependencies: mbed ServoMotor Suteppa
main.cpp@0:022e5e32d580, 2020-03-08 (annotated)
- Committer:
- j_rocket_boy
- Date:
- Sun Mar 08 03:39:18 2020 +0000
- Revision:
- 0:022e5e32d580
for Antenna rotator
Who changed what in which revision?
User | Revision | Line number | New 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 | } |