春ロボ　ロケット団 / Mbed 2 deprecated catch

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed KondoServoLibrary Encoder

main.cpp@0:8ffb3e099b90, 2020-03-11 (annotated)

Committer:
koheim
Date:
Wed Mar 11 05:14:21 2020 +0000
Revision:
0:8ffb3e099b90
cauch;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
koheim 0:8ffb3e099b90 1 #include"mbed.h"
koheim 0:8ffb3e099b90 2 #include"KondoServo.h"
koheim 0:8ffb3e099b90 3 #include "EC.h" //Encoderライブラリをインクルード
koheim 0:8ffb3e099b90 4 #define RESOLUTION 500
koheim 0:8ffb3e099b90 5
koheim 0:8ffb3e099b90 6 //Serial pc(USBTX, USBRX); // tx, rx
koheim 0:8ffb3e099b90 7 PwmOut f(p26);//モーター
koheim 0:8ffb3e099b90 8 PwmOut b(p25);
koheim 0:8ffb3e099b90 9
koheim 0:8ffb3e099b90 10 Ticker ticker;//割込み
koheim 0:8ffb3e099b90 11
koheim 0:8ffb3e099b90 12 Ec1multi EC(p16,p17,RESOLUTION);//エンコーダー
koheim 0:8ffb3e099b90 13 InterruptIn X(p15);
koheim 0:8ffb3e099b90 14
koheim 0:8ffb3e099b90 15 KondoServo servo(p28,p27);//サーボ
koheim 0:8ffb3e099b90 16
koheim 0:8ffb3e099b90 17 DigitalOut out1(p19);//エアシリンダー（つかむ方）
koheim 0:8ffb3e099b90 18 DigitalOut out2(p20);//エアシリンダー（投擲）
koheim 0:8ffb3e099b90 19
koheim 0:8ffb3e099b90 20 DigitalIn button(p14,PullUp);//タッチセンサー
koheim 0:8ffb3e099b90 21
koheim 0:8ffb3e099b90 22 DigitalOut led1(LED1);//LED
koheim 0:8ffb3e099b90 23 DigitalOut led2(LED2);
koheim 0:8ffb3e099b90 24
koheim 0:8ffb3e099b90 25 void cal()
koheim 0:8ffb3e099b90 26 {
koheim 0:8ffb3e099b90 27 EC.calOmega();
koheim 0:8ffb3e099b90 28 }
koheim 0:8ffb3e099b90 29
koheim 0:8ffb3e099b90 30 int X_count=0;
koheim 0:8ffb3e099b90 31
koheim 0:8ffb3e099b90 32 void Xcount()
koheim 0:8ffb3e099b90 33 {
koheim 0:8ffb3e099b90 34 X_count++;
koheim 0:8ffb3e099b90 35 }
koheim 0:8ffb3e099b90 36
koheim 0:8ffb3e099b90 37 int main()
koheim 0:8ffb3e099b90 38 {
koheim 0:8ffb3e099b90 39 X.rise(&Xcount);
koheim 0:8ffb3e099b90 40 f.period_us(50);
koheim 0:8ffb3e099b90 41 b.period_us(50);
koheim 0:8ffb3e099b90 42 out1 = 0;
koheim 0:8ffb3e099b90 43 out2 = 0;
koheim 0:8ffb3e099b90 44 double a=0,r=0.4,v=0;//rで半径指定 a*r=v
koheim 0:8ffb3e099b90 45 int count=0;
koheim 0:8ffb3e099b90 46 ticker.attach(&cal,0.05);
koheim 0:8ffb3e099b90 47
koheim 0:8ffb3e099b90 48 int id = 0;
koheim 0:8ffb3e099b90 49 double SERVO2DEG = 270.0 / (11500 - 3500);
koheim 0:8ffb3e099b90 50 double first = (6800 - 3500) * SERVO2DEG;
koheim 0:8ffb3e099b90 51 double grab = (3800 - 3500) * SERVO2DEG;
koheim 0:8ffb3e099b90 52 double pass = (5000 - 3500) * SERVO2DEG;
koheim 0:8ffb3e099b90 53
koheim 0:8ffb3e099b90 54 servo.set_degree(id, first);
koheim 0:8ffb3e099b90 55 printf("start\r\n");
koheim 0:8ffb3e099b90 56 wait(1);
koheim 0:8ffb3e099b90 57
koheim 0:8ffb3e099b90 58 servo.set_degree(id, grab);
koheim 0:8ffb3e099b90 59 wait(1);
koheim 0:8ffb3e099b90 60 led1 = 1;//掴む
koheim 0:8ffb3e099b90 61 out1 = 1;
koheim 0:8ffb3e099b90 62 printf("grab\r\n");
koheim 0:8ffb3e099b90 63 wait(1);
koheim 0:8ffb3e099b90 64
koheim 0:8ffb3e099b90 65 while(1) {//投擲アームをセット
koheim 0:8ffb3e099b90 66 a = EC.getOmega();
koheim 0:8ffb3e099b90 67 count = EC.getCount();
koheim 0:8ffb3e099b90 68 v = a*r;
koheim 0:8ffb3e099b90 69 f = 0.0;//速度一定
koheim 0:8ffb3e099b90 70 b = 0.2;
koheim 0:8ffb3e099b90 71 printf("%.3f %.3f %d\r\n",a,v,count);
koheim 0:8ffb3e099b90 72
koheim 0:8ffb3e099b90 73 if(X_count == 1) {
koheim 0:8ffb3e099b90 74 f = 0.0;
koheim 0:8ffb3e099b90 75 b = 0.0;
koheim 0:8ffb3e099b90 76 break;
koheim 0:8ffb3e099b90 77 }
koheim 0:8ffb3e099b90 78 }
koheim 0:8ffb3e099b90 79
koheim 0:8ffb3e099b90 80 servo.set_degree(id, pass);
koheim 0:8ffb3e099b90 81 printf("set\r\n");
koheim 0:8ffb3e099b90 82
koheim 0:8ffb3e099b90 83 while(1) {
koheim 0:8ffb3e099b90 84 a = EC.getOmega();
koheim 0:8ffb3e099b90 85 count = EC.getCount();
koheim 0:8ffb3e099b90 86 v = a*r;
koheim 0:8ffb3e099b90 87 f = 0.0;//速度一定
koheim 0:8ffb3e099b90 88 b = 0.05;
koheim 0:8ffb3e099b90 89 printf("%.3f %.3f\r\n",a,v);
koheim 0:8ffb3e099b90 90 if(button.read() == 0) {
koheim 0:8ffb3e099b90 91 wait(0.05);
koheim 0:8ffb3e099b90 92 if(button.read() == 0) {
koheim 0:8ffb3e099b90 93 f = 0.0;
koheim 0:8ffb3e099b90 94 b = 0.0;
koheim 0:8ffb3e099b90 95 wait(0.5);
koheim 0:8ffb3e099b90 96 break;
koheim 0:8ffb3e099b90 97 }
koheim 0:8ffb3e099b90 98 }
koheim 0:8ffb3e099b90 99 }
koheim 0:8ffb3e099b90 100 led2 = 1;//投擲アームが掴む
koheim 0:8ffb3e099b90 101 out2 = 1;
koheim 0:8ffb3e099b90 102 wait (1);
koheim 0:8ffb3e099b90 103
koheim 0:8ffb3e099b90 104 led1 = 0;//離す
koheim 0:8ffb3e099b90 105 out1 = 0;
koheim 0:8ffb3e099b90 106 printf("ok\r\n");
koheim 0:8ffb3e099b90 107 wait(1);
koheim 0:8ffb3e099b90 108 servo.set_degree(id, first);
koheim 0:8ffb3e099b90 109 wait(1);
koheim 0:8ffb3e099b90 110
koheim 0:8ffb3e099b90 111 while(1) {//投擲アームを上に
koheim 0:8ffb3e099b90 112 a = EC.getOmega();
koheim 0:8ffb3e099b90 113 count = EC.getCount();
koheim 0:8ffb3e099b90 114 v = a*r;
koheim 0:8ffb3e099b90 115 f = 0.1;//速度一定
koheim 0:8ffb3e099b90 116 b = 0.0;
koheim 0:8ffb3e099b90 117 printf("%.3f %.3f\r\n",a,v);
koheim 0:8ffb3e099b90 118 if(X_count == 2) {
koheim 0:8ffb3e099b90 119 EC.reset();
koheim 0:8ffb3e099b90 120 X_count =0;
koheim 0:8ffb3e099b90 121 break;
koheim 0:8ffb3e099b90 122 }
koheim 0:8ffb3e099b90 123 }
koheim 0:8ffb3e099b90 124
koheim 0:8ffb3e099b90 125 servo.set_degree(id, grab);//掴むアームをセット
koheim 0:8ffb3e099b90 126 printf("finish\r\n");
koheim 0:8ffb3e099b90 127 return 0;
koheim 0:8ffb3e099b90 128 }