
はじめての手作りマイコンロボット実験セットのプログラムです ご自由に改造して使ってください
main.cpp@0:ebf41685fa06, 2016-10-21 (annotated)
- Committer:
- kohacraft
- Date:
- Fri Oct 21 02:04:30 2016 +0000
- Revision:
- 0:ebf41685fa06
ver1.0
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kohacraft | 0:ebf41685fa06 | 1 | //はじめての手作りマイコンロボット実験セットのプログラムです |
kohacraft | 0:ebf41685fa06 | 2 | #include "mbed.h" |
kohacraft | 0:ebf41685fa06 | 3 | |
kohacraft | 0:ebf41685fa06 | 4 | PwmOut ude1(dp1); //右の腕のサーボモータ制御に使います |
kohacraft | 0:ebf41685fa06 | 5 | PwmOut ude2(dp2); //左の腕のサーボモータ制御に使います |
kohacraft | 0:ebf41685fa06 | 6 | |
kohacraft | 0:ebf41685fa06 | 7 | DigitalOut asi1(dp18); //右のモーターを動かすのに使います |
kohacraft | 0:ebf41685fa06 | 8 | DigitalOut asi2(dp24); //左のモーターを動かすのに使います |
kohacraft | 0:ebf41685fa06 | 9 | |
kohacraft | 0:ebf41685fa06 | 10 | DigitalOut sp(dp4); //モーター動作中の時にONになります |
kohacraft | 0:ebf41685fa06 | 11 | |
kohacraft | 0:ebf41685fa06 | 12 | |
kohacraft | 0:ebf41685fa06 | 13 | DigitalIn mae(dp10); //両方のモータを回すためのスイッチをつなぎます |
kohacraft | 0:ebf41685fa06 | 14 | DigitalIn hidari(dp11); //左のモータを回すためのスイッチをつなぎます |
kohacraft | 0:ebf41685fa06 | 15 | DigitalIn migi(dp9); //右のモータを回すためのスイッチをつなぎます |
kohacraft | 0:ebf41685fa06 | 16 | |
kohacraft | 0:ebf41685fa06 | 17 | DigitalOut me1(dp13); //まばたきする目のLEDをつなぎます |
kohacraft | 0:ebf41685fa06 | 18 | DigitalOut me2(dp14); //まばたきする目のLEDをつなぎます |
kohacraft | 0:ebf41685fa06 | 19 | |
kohacraft | 0:ebf41685fa06 | 20 | |
kohacraft | 0:ebf41685fa06 | 21 | //タイマー割り込みでうでをふる |
kohacraft | 0:ebf41685fa06 | 22 | //sin関数を使い振り子のようになめらかに行ったり来たりするようサーボモーターを動かします |
kohacraft | 0:ebf41685fa06 | 23 | Ticker udehuriTimer; |
kohacraft | 0:ebf41685fa06 | 24 | float kakudo=0.0; |
kohacraft | 0:ebf41685fa06 | 25 | void udehuri() { |
kohacraft | 0:ebf41685fa06 | 26 | kakudo += 360/100; //タイマー割り込みが実行されるごとにsin関数の角度を360/100度ずつ増やします |
kohacraft | 0:ebf41685fa06 | 27 | //100の部分を50などに減らすと腕の振りが速くなり、200などに増やすと振りが遅くなります |
kohacraft | 0:ebf41685fa06 | 28 | if( kakudo > 360 ) //最大角度を超えたら0度に戻す |
kohacraft | 0:ebf41685fa06 | 29 | kakudo = 0.0; |
kohacraft | 0:ebf41685fa06 | 30 | float ude; |
kohacraft | 0:ebf41685fa06 | 31 | ude = ( sin( kakudo *2*3.14/360 ) + 1.0 )/1.5 + 1.0; //1から2くらいになる |
kohacraft | 0:ebf41685fa06 | 32 | //sin( kakudo *2*3.14/360 )+1.0 は0→2→0→2と0~2の範囲でなめらなに行ったり来たりする式です |
kohacraft | 0:ebf41685fa06 | 33 | //その値を1.5で割るので0~1.3の範囲で変化し、最後に1.0を足すので1.0~2.3の範囲の値になります。 |
kohacraft | 0:ebf41685fa06 | 34 | //サーボモーターに設定できる値は0.5~2.5の範囲内です。この時サーボの角度は約180度動きます |
kohacraft | 0:ebf41685fa06 | 35 | //上記のプログラムでは約半分の100度程度腕を振ります |
kohacraft | 0:ebf41685fa06 | 36 | //ude = ( sin( kakudo *2*3.14/360 ) + 1.0 )/1.0 + 0.5; |
kohacraft | 0:ebf41685fa06 | 37 | //とすると0.5~2.5の範囲になり、腕が最大の振り幅の180度になります |
kohacraft | 0:ebf41685fa06 | 38 | ude = ude / 20.0; //サーボのPWMの幅の比率に変換します |
kohacraft | 0:ebf41685fa06 | 39 | |
kohacraft | 0:ebf41685fa06 | 40 | ude1 = ude; |
kohacraft | 0:ebf41685fa06 | 41 | ude2 = ude; |
kohacraft | 0:ebf41685fa06 | 42 | } |
kohacraft | 0:ebf41685fa06 | 43 | |
kohacraft | 0:ebf41685fa06 | 44 | //タイマー割り込みでまばたきさせる |
kohacraft | 0:ebf41685fa06 | 45 | Ticker blink; |
kohacraft | 0:ebf41685fa06 | 46 | void mabataki(){ |
kohacraft | 0:ebf41685fa06 | 47 | me1 = 0; //目のLEDをOFFします |
kohacraft | 0:ebf41685fa06 | 48 | me2 = 0; |
kohacraft | 0:ebf41685fa06 | 49 | wait(0.05); //0.05秒待って |
kohacraft | 0:ebf41685fa06 | 50 | me1 = 1; //目のLEDをONします |
kohacraft | 0:ebf41685fa06 | 51 | me2 = 1; |
kohacraft | 0:ebf41685fa06 | 52 | } |
kohacraft | 0:ebf41685fa06 | 53 | |
kohacraft | 0:ebf41685fa06 | 54 | int main() { |
kohacraft | 0:ebf41685fa06 | 55 | |
kohacraft | 0:ebf41685fa06 | 56 | |
kohacraft | 0:ebf41685fa06 | 57 | asi1 = 0; |
kohacraft | 0:ebf41685fa06 | 58 | asi2 = 0; |
kohacraft | 0:ebf41685fa06 | 59 | ude1.period_ms(20); //サーボモーターの制御信号の周期が20msなのでその設定をします |
kohacraft | 0:ebf41685fa06 | 60 | ude2.period_ms(20); |
kohacraft | 0:ebf41685fa06 | 61 | ude1 = 1.5/20.0; //腕を真ん中の角度にします |
kohacraft | 0:ebf41685fa06 | 62 | ude2 = 1.5/20.0; |
kohacraft | 0:ebf41685fa06 | 63 | |
kohacraft | 0:ebf41685fa06 | 64 | me1 = 1; |
kohacraft | 0:ebf41685fa06 | 65 | me2 = 1; |
kohacraft | 0:ebf41685fa06 | 66 | blink.attach(&mabataki, 4.0); //4秒ごとにまばたきのプログラムが実行されるようタイマー割り込みをセットします |
kohacraft | 0:ebf41685fa06 | 67 | while(1) |
kohacraft | 0:ebf41685fa06 | 68 | { |
kohacraft | 0:ebf41685fa06 | 69 | //前ボタンでで両方の足が回る |
kohacraft | 0:ebf41685fa06 | 70 | if( mae == 1 ) |
kohacraft | 0:ebf41685fa06 | 71 | { |
kohacraft | 0:ebf41685fa06 | 72 | sp = 1; |
kohacraft | 0:ebf41685fa06 | 73 | udehuriTimer.attach(&udehuri, 0.02);//0.02秒ごとに腕振りのプログラムが実行されるようタイマー割り込みをセットします |
kohacraft | 0:ebf41685fa06 | 74 | asi1 = 1; //右のモーターを回転させます |
kohacraft | 0:ebf41685fa06 | 75 | asi2 = 1; //左のモーターを回転させます |
kohacraft | 0:ebf41685fa06 | 76 | wait(0.1); //0.1秒モータを回転させたまま待ちます |
kohacraft | 0:ebf41685fa06 | 77 | udehuriTimer.detach(); //腕振りのタイマー割り込みを終了させます |
kohacraft | 0:ebf41685fa06 | 78 | asi1 = 0; //右のモーターを停止させます |
kohacraft | 0:ebf41685fa06 | 79 | asi2 = 0; //左のモーターを停止させます |
kohacraft | 0:ebf41685fa06 | 80 | } |
kohacraft | 0:ebf41685fa06 | 81 | |
kohacraft | 0:ebf41685fa06 | 82 | //右ボタンで右のモーターが回る |
kohacraft | 0:ebf41685fa06 | 83 | if( migi == 1 ) |
kohacraft | 0:ebf41685fa06 | 84 | { |
kohacraft | 0:ebf41685fa06 | 85 | sp = 1; |
kohacraft | 0:ebf41685fa06 | 86 | udehuriTimer.attach(&udehuri, 0.02); |
kohacraft | 0:ebf41685fa06 | 87 | asi1 = 1; //右のモーターを回転させます |
kohacraft | 0:ebf41685fa06 | 88 | wait(0.1); |
kohacraft | 0:ebf41685fa06 | 89 | udehuriTimer.detach(); |
kohacraft | 0:ebf41685fa06 | 90 | asi1 = 0; //右のモーターを停止させます |
kohacraft | 0:ebf41685fa06 | 91 | } |
kohacraft | 0:ebf41685fa06 | 92 | |
kohacraft | 0:ebf41685fa06 | 93 | //左ボタンで左のモーターが回る |
kohacraft | 0:ebf41685fa06 | 94 | if( hidari == 1 ) |
kohacraft | 0:ebf41685fa06 | 95 | { |
kohacraft | 0:ebf41685fa06 | 96 | sp = 1; |
kohacraft | 0:ebf41685fa06 | 97 | udehuriTimer.attach(&udehuri, 0.02); |
kohacraft | 0:ebf41685fa06 | 98 | asi2 = 1; //左のモーターを回転させます |
kohacraft | 0:ebf41685fa06 | 99 | wait(0.1); |
kohacraft | 0:ebf41685fa06 | 100 | udehuriTimer.detach(); |
kohacraft | 0:ebf41685fa06 | 101 | asi2 = 0; //左のモーターを停止させます |
kohacraft | 0:ebf41685fa06 | 102 | } |
kohacraft | 0:ebf41685fa06 | 103 | |
kohacraft | 0:ebf41685fa06 | 104 | //全てのボタンを押してなければ音楽を止める |
kohacraft | 0:ebf41685fa06 | 105 | if( mae == 0 && migi == 0 && hidari == 0) |
kohacraft | 0:ebf41685fa06 | 106 | { |
kohacraft | 0:ebf41685fa06 | 107 | sp = 0; |
kohacraft | 0:ebf41685fa06 | 108 | } |
kohacraft | 0:ebf41685fa06 | 109 | |
kohacraft | 0:ebf41685fa06 | 110 | } |
kohacraft | 0:ebf41685fa06 | 111 | |
kohacraft | 0:ebf41685fa06 | 112 | |
kohacraft | 0:ebf41685fa06 | 113 | } |