NITICRobotClub 2018Team-B / Mbed 2 deprecated Lets_move_Seki2018_null

自動投射のみです。（手動用にチューニングしました）

Dependencies:   HCSR04 PID QEI mbed

Fork of Lets_move_Seki2018_ver2 by INCTRC Information Sharing Test

main.cpp@5:b3bbf96356cf, 2018-09-27 (annotated)

Committer:
tektomo
Date:
Thu Sep 27 09:03:04 2018 +0000
Revision:
5:b3bbf96356cf
Parent:
4:df334779a69e 
???????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yuron 4:df334779a69e 1 /* ------------------------------------------------------------------- */
yuron 4:df334779a69e 2 /* NHKロボコン2018-Bチーム自動ロボット(設計者: 4S 関) */
yuron 4:df334779a69e 3 /* ------------------------------------------------------------------- */
yuron 4:df334779a69e 4 /* このプログラムは上記のロボット専用の制御プログラムである。 */
yuron 4:df334779a69e 5 /* ------------------------------------------------------------------- */
yuron 4:df334779a69e 6 /* 対応機種: NUCLEO-F446RE */
yuron 4:df334779a69e 7 /* ------------------------------------------------------------------- */
yuron 4:df334779a69e 8 /* 製作者: 4D 高久 雄飛, mail: rab1sy23@gmail.com */
yuron 4:df334779a69e 9 /* ------------------------------------------------------------------- */
yuron 4:df334779a69e 10 /* 使用センサ: リミットスイッチ1個, ロータリーエンコーダ: 7個, 超音波センサ: 1個, フォトインタラプタ: 1個 */
yuron 4:df334779a69e 11 /* 他にラインセンサ基板のPIC(16F1938)と通信をしてライントレースをさせている */
yuron 4:df334779a69e 12 /* ------------------------------------------------------------------- */
yuron 0:f73c1b076ae4 13 #include "mbed.h"
yuron 0:f73c1b076ae4 14 #include "math.h"
yuron 0:f73c1b076ae4 15 #include "QEI.h"
yuron 0:f73c1b076ae4 16 #include "PID.h"
yuron 4:df334779a69e 17 #include "hcsr04.h"
tektomo 5:b3bbf96356cf 18 //Pi
yuron 0:f73c1b076ae4 19 #define PI 3.14159265359
tektomo 5:b3bbf96356cf 20 //PIDGain of rollers
tektomo 5:b3bbf96356cf 21 #define roller_Kp 1.5
tektomo 5:b3bbf96356cf 22 #define roller_Ki 0.1
yuron 4:df334779a69e 23
tektomo 5:b3bbf96356cf 24 InterruptIn shoot(PC_13);
tektomo 5:b3bbf96356cf 25 HCSR04 sonic(PB_3, PB_10);
tektomo 5:b3bbf96356cf 26
yuron 4:df334779a69e 27 //前ローラー
yuron 4:df334779a69e 28 PID front_roller(roller_Kp, roller_Ki, 0.0, 0.001);
yuron 4:df334779a69e 29 //後ローラー
yuron 4:df334779a69e 30 PID back_roller(roller_Kp, roller_Ki, 0.0, 0.001);
yuron 4:df334779a69e 31 //MDとの通信ポート
yuron 4:df334779a69e 32 I2C i2c(PB_9, PB_8); //SDA, SCL
yuron 4:df334779a69e 33 //PCとの通信ポート
yuron 4:df334779a69e 34 Serial pc(USBTX, USBRX); //TX, RX
yuron 4:df334779a69e 35 //12V停止信号ピン
yuron 0:f73c1b076ae4 36 DigitalOut emergency(PA_13);
yuron 4:df334779a69e 37 //前ローラー
yuron 4:df334779a69e 38 QEI front_roller_wheel(PA_0, PA_1, NC, 624);
yuron 4:df334779a69e 39 //後ローラー
yuron 4:df334779a69e 40 QEI back_roller_wheel(PB_5, PB_4, NC, 624);
tektomo 5:b3bbf96356cf 41 Ticker get_rps;
yuron 0:f73c1b076ae4 42
yuron 4:df334779a69e 43 int front_roller_rpm;
yuron 4:df334779a69e 44 int back_roller_rpm;
yuron 0:f73c1b076ae4 45
tektomo 5:b3bbf96356cf 46 int front_roller_pulse[10];
tektomo 5:b3bbf96356cf 47 int back_roller_pulse[10];
yuron 0:f73c1b076ae4 48
tektomo 5:b3bbf96356cf 49 int sum_front_roller_pulse;
tektomo 5:b3bbf96356cf 50 int sum_back_roller_pulse;
yuron 0:f73c1b076ae4 51
tektomo 5:b3bbf96356cf 52 int ave_front_roller_pulse;
tektomo 5:b3bbf96356cf 53 int ave_back_roller_pulse;
yuron 0:f73c1b076ae4 54
yuron 0:f73c1b076ae4 55 char send_data[1];
yuron 0:f73c1b076ae4 56
yuron 4:df334779a69e 57 char front_roller_data[1];
yuron 4:df334779a69e 58 char back_roller_data[1];
tektomo 5:b3bbf96356cf 59 char cylinder_data[0];
yuron 4:df334779a69e 60
tektomo 5:b3bbf96356cf 61 static int i = 0;
tektomo 5:b3bbf96356cf 62 //各関数のプロトタイプ宣言
tektomo 5:b3bbf96356cf 63 void shot();
tektomo 5:b3bbf96356cf 64 void ultrasonic();
tektomo 5:b3bbf96356cf 65 //回転数取得関数
yuron 0:f73c1b076ae4 66 void flip();
tektomo 5:b3bbf96356cf 67 //ローラーPID
yuron 4:df334779a69e 68 int roller_PID(int front, int back);
yuron 0:f73c1b076ae4 69
tektomo 5:b3bbf96356cf 70 //移動平均をPID
yuron 4:df334779a69e 71 void flip() {
tektomo 5:b3bbf96356cf 72 //前回のi番目のデータを消して新たにそこにデータを書き込む(キューのような考え?)
tektomo 5:b3bbf96356cf 73 sum_front_roller_pulse -= front_roller_pulse[i];
tektomo 5:b3bbf96356cf 74 sum_back_roller_pulse -= back_roller_pulse[i];
yuron 4:df334779a69e 75
tektomo 5:b3bbf96356cf 76 //配列のi番目の箱に取得パルスを代入
tektomo 5:b3bbf96356cf 77 front_roller_pulse[i] = front_roller_wheel.getPulses();
tektomo 5:b3bbf96356cf 78 back_roller_pulse[i] = back_roller_wheel.getPulses();
yuron 4:df334779a69e 79
yuron 4:df334779a69e 80 front_roller_wheel.reset();
yuron 4:df334779a69e 81 back_roller_wheel.reset();
yuron 4:df334779a69e 82
tektomo 5:b3bbf96356cf 83 //i[0]~i[9]までの合計値を代入
tektomo 5:b3bbf96356cf 84 sum_front_roller_pulse += front_roller_pulse[i];
tektomo 5:b3bbf96356cf 85 sum_back_roller_pulse += back_roller_pulse[i];
yuron 4:df334779a69e 86
tektomo 5:b3bbf96356cf 87 //インクリメント
tektomo 5:b3bbf96356cf 88 i++;
tektomo 5:b3bbf96356cf 89 //iが最大値(9)になったらリセット
tektomo 5:b3bbf96356cf 90 if(i > 9) {
tektomo 5:b3bbf96356cf 91 i = 0;
tektomo 5:b3bbf96356cf 92 }
tektomo 5:b3bbf96356cf 93 //10回分の合計値を10で割り、取得パルスの平均を出す
tektomo 5:b3bbf96356cf 94 ave_front_roller_pulse = sum_front_roller_pulse / 10;
tektomo 5:b3bbf96356cf 95 ave_back_roller_pulse = sum_back_roller_pulse / 10;
yuron 4:df334779a69e 96
tektomo 5:b3bbf96356cf 97 //平均値をRPMへ変換
tektomo 5:b3bbf96356cf 98 front_roller_rpm = ave_front_roller_pulse * 25 * 60 / 50;
tektomo 5:b3bbf96356cf 99 back_roller_rpm = ave_back_roller_pulse * 25 * 60 / 1200;
tektomo 5:b3bbf96356cf 100 pc.printf("%d %d\n\r", abs(front_roller_rpm), abs(back_roller_rpm));
yuron 0:f73c1b076ae4 101 }
tektomo 5:b3bbf96356cf 102 //ローラー
yuron 4:df334779a69e 103 int roller_PID(int front, int back) {
yuron 4:df334779a69e 104 front_roller.setInputLimits(-2000, 2000);
yuron 4:df334779a69e 105 back_roller.setInputLimits(-2000, 2000);
tektomo 5:b3bbf96356cf 106
yuron 4:df334779a69e 107 front_roller.setOutputLimits(0x84, 0xFF);
yuron 4:df334779a69e 108 back_roller.setOutputLimits(0x84, 0xFF);
tektomo 5:b3bbf96356cf 109
yuron 4:df334779a69e 110 front_roller.setMode(AUTO_MODE);
yuron 4:df334779a69e 111 back_roller.setMode(AUTO_MODE);
tektomo 5:b3bbf96356cf 112
yuron 4:df334779a69e 113 front_roller.setSetPoint(front);
yuron 4:df334779a69e 114 back_roller.setSetPoint(back);
tektomo 5:b3bbf96356cf 115
yuron 4:df334779a69e 116 front_roller.setProcessValue(abs(front_roller_rpm));
yuron 4:df334779a69e 117 back_roller.setProcessValue(abs(back_roller_rpm));
tektomo 5:b3bbf96356cf 118
yuron 4:df334779a69e 119 front_roller_data[0] = front_roller.compute();
yuron 4:df334779a69e 120 back_roller_data[0] = back_roller.compute();
tektomo 5:b3bbf96356cf 121
yuron 4:df334779a69e 122 return 0;
yuron 4:df334779a69e 123 }
yuron 4:df334779a69e 124 int main(void) {
tektomo 5:b3bbf96356cf 125 //緊急停止用信号ピンをLow
yuron 0:f73c1b076ae4 126 emergency = 0;
tektomo 5:b3bbf96356cf 127 //回転数取得関数のタイマ割り込み(40ms)
yuron 4:df334779a69e 128 get_rps.attach_us(&flip, 40000);
tektomo 5:b3bbf96356cf 129
tektomo 5:b3bbf96356cf 130 //各MD, エアシリ制御基板へ起動後0.1秒間0x80を送りつける(通信切断防止用)
yuron 2:c32991ba628f 131 send_data[0] = 0x80;
yuron 4:df334779a69e 132 i2c.write(0x16, send_data, 1);
yuron 3:1223cab367d9 133 i2c.write(0x22, send_data, 1);
yuron 4:df334779a69e 134 i2c.write(0x40, send_data, 1);
tektomo 5:b3bbf96356cf 135
tektomo 5:b3bbf96356cf 136 shoot.fall(shot);
yuron 0:f73c1b076ae4 137 wait(0.1);
tektomo 5:b3bbf96356cf 138
yuron 4:df334779a69e 139 while(1) {
yuron 4:df334779a69e 140 ultrasonic();
tektomo 5:b3bbf96356cf 141 //ローラーぐるぐる
tektomo 5:b3bbf96356cf 142 roller_PID(200, 200);
yuron 4:df334779a69e 143 i2c.write(0x20, front_roller_data, 1, false);
yuron 4:df334779a69e 144 i2c.write(0x22, back_roller_data, 1, false);
tektomo 5:b3bbf96356cf 145 wait_us(20);
yuron 0:f73c1b076ae4 146 }
yuron 0:f73c1b076ae4 147 }
tektomo 5:b3bbf96356cf 148
tektomo 5:b3bbf96356cf 149 void shot(){
tektomo 5:b3bbf96356cf 150 cylinder_data[0] = 0x0F;
tektomo 5:b3bbf96356cf 151 i2c.write(0x40, cylinder_data, 1);
tektomo 5:b3bbf96356cf 152 wait(1);
tektomo 5:b3bbf96356cf 153
tektomo 5:b3bbf96356cf 154 cylinder_data[0] = 0x80;
tektomo 5:b3bbf96356cf 155 i2c.write(0x40, cylinder_data, 1);
tektomo 5:b3bbf96356cf 156 wait(0.5);
tektomo 5:b3bbf96356cf 157
tektomo 5:b3bbf96356cf 158 }
tektomo 5:b3bbf96356cf 159
tektomo 5:b3bbf96356cf 160 //超音波センサ用関数
tektomo 5:b3bbf96356cf 161 void ultrasonic() {
tektomo 5:b3bbf96356cf 162 //超音波発射
tektomo 5:b3bbf96356cf 163 sonic.start();
tektomo 5:b3bbf96356cf 164 //10ms待機
tektomo 5:b3bbf96356cf 165 wait(0.01);
tektomo 5:b3bbf96356cf 166 //get_dist_cm関数は、計測から得られた距離(cm)を返します。
tektomo 5:b3bbf96356cf 167 int distance = sonic.get_dist_cm();
tektomo 5:b3bbf96356cf 168 //pc.printf("%d[cm]\r\n", distance);
tektomo 5:b3bbf96356cf 169 }