NITICRobotClub 2018Team-B / Mbed 2 deprecated NHK2018_BM

NHK2018Bチームの手動ロボットのプログラムです。（製作中）

Dependencies:   HCSR04 PID PS3viaSBDBT QEI mbed

main.cpp@2:235db39e0eaa, 2018-09-26 (annotated)

Committer:
BIGBOSS04
Date:
Wed Sep 26 10:58:55 2018 +0000
Revision:
2:235db39e0eaa
Parent:
1:5609d9509abf 
NHK2018_lancha_proglam

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tektomo 0:1ebf4907639c 1 /* ------------------------------------------------------------------- */
tektomo 0:1ebf4907639c 2 /* NHKロボコン2018-Bチーム手動ロボット(設計者: 4S 関) */
tektomo 0:1ebf4907639c 3 /* ------------------------------------------------------------------- */
tektomo 0:1ebf4907639c 4 /* このプログラムは上記のロボット専用の制御プログラムである。 */
tektomo 0:1ebf4907639c 5 /* ------------------------------------------------------------------- */
tektomo 0:1ebf4907639c 6 /* 対応機種: NUCLEO-F446RE */
tektomo 0:1ebf4907639c 7 /* ------------------------------------------------------------------- */
tektomo 0:1ebf4907639c 8 /* 製作者: 1-3 武井 智大, mail: taobmcoe@outlook.com */
tektomo 0:1ebf4907639c 9 /* ------------------------------------------------------------------- */
tektomo 0:1ebf4907639c 10 /* 使用センサ:ロータリーエンコーダ: 2個, 超音波センサ: 1個*/
tektomo 0:1ebf4907639c 11 /* ------------------------------------------------------------------- */
tektomo 0:1ebf4907639c 12 #include "mbed.h"
tektomo 0:1ebf4907639c 13 #include "hcsr04.h"
tektomo 0:1ebf4907639c 14 #include "QEI.h"
tektomo 0:1ebf4907639c 15 #include "PID.h"
tektomo 0:1ebf4907639c 16
tektomo 0:1ebf4907639c 17 #define ps3data ps3s.condata
tektomo 0:1ebf4907639c 18 #define PI 3.14159265359
BIGBOSS04 1:5609d9509abf 19 #define roller_Kp 4.0
BIGBOSS04 1:5609d9509abf 20 #define froller_Ki 0.1
BIGBOSS04 1:5609d9509abf 21 #define broller_Ki 0.1
BIGBOSS04 1:5609d9509abf 22 #define roller_Kd 0.05
tektomo 0:1ebf4907639c 23
tektomo 0:1ebf4907639c 24 //定義
tektomo 0:1ebf4907639c 25
tektomo 0:1ebf4907639c 26 /*I2C定義
tektomo 0:1ebf4907639c 27 0前輪左:0x10
tektomo 0:1ebf4907639c 28 1前輪右:0x12
tektomo 0:1ebf4907639c 29 2後輪左:0x14
tektomo 0:1ebf4907639c 30 3後輪右:0x16
tektomo 0:1ebf4907639c 31 4投射前:0x18
tektomo 0:1ebf4907639c 32 5投射後ろ:0x20
tektomo 0:1ebf4907639c 33 6エアシリンダー:0x40
tektomo 0:1ebf4907639c 34 正転:0x84~0xFF
tektomo 0:1ebf4907639c 35 逆転:0x00~0x7C
tektomo 0:1ebf4907639c 36 ショートブレーキ:0x7D~0x83
tektomo 0:1ebf4907639c 37 */
tektomo 0:1ebf4907639c 38 I2C i2c(PB_9, PB_8);
tektomo 0:1ebf4907639c 39 //PS3コン定義
tektomo 0:1ebf4907639c 40 //PC定義
tektomo 0:1ebf4907639c 41 Serial pc(USBTX,USBRX);
tektomo 0:1ebf4907639c 42 //ロリコン定義
tektomo 0:1ebf4907639c 43 QEI front_roller_wheel(PC_8, PC_6, NC, 624);
tektomo 0:1ebf4907639c 44 QEI back_roller_wheel(PC_5, PA_12, NC, 624);
tektomo 0:1ebf4907639c 45 //PID定義
tektomo 0:1ebf4907639c 46 PID front_roller(roller_Kp, froller_Ki, roller_Kd, 0.001);
tektomo 0:1ebf4907639c 47 PID back_roller(roller_Kp, broller_Ki, roller_Kd, 0.001);
tektomo 0:1ebf4907639c 48 //超音波センサ定義
tektomo 0:1ebf4907639c 49 HCSR04 topsonic(PB_2,PB_1);
tektomo 0:1ebf4907639c 50 //HCSR04 undersonic(PB_15,PB_14);
tektomo 0:1ebf4907639c 51 //LED定義
tektomo 0:1ebf4907639c 52 DigitalOut Power(PC_12);//Green
tektomo 0:1ebf4907639c 53 DigitalOut i2ccheck(PB_7);//Blue
tektomo 0:1ebf4907639c 54 DigitalOut sncled(PC_2);//Yellow
tektomo 0:1ebf4907639c 55 DigitalOut Powemer(PC_3);//Red
tektomo 0:1ebf4907639c 56 DigitalOut myled(LED1);
tektomo 0:1ebf4907639c 57 //緊急停止定義
tektomo 0:1ebf4907639c 58 DigitalOut emersig(PC_0);
tektomo 0:1ebf4907639c 59 //タイマー定義
tektomo 0:1ebf4907639c 60 Ticker get_rps;//ロリコンからRPMの読み取り
tektomo 0:1ebf4907639c 61 Timer shootpet;
tektomo 0:1ebf4907639c 62 Timer mainloop;
tektomo 0:1ebf4907639c 63
tektomo 0:1ebf4907639c 64 InterruptIn but(USER_BUTTON);
tektomo 0:1ebf4907639c 65
tektomo 0:1ebf4907639c 66 //変数定義
tektomo 0:1ebf4907639c 67 //データ配列
tektomo 0:1ebf4907639c 68 char send_data[4][1];//足回り用データ配列
tektomo 0:1ebf4907639c 69 char send_data2[1];//エアシリンダー用データ配列
tektomo 0:1ebf4907639c 70 int address[4] = {0x10, 0x12, 0x14, 0x16};//i2cアドレス
tektomo 0:1ebf4907639c 71 int err[4] = {1, 1, 1, 1};//I2Cリザルト
tektomo 0:1ebf4907639c 72
tektomo 0:1ebf4907639c 73 int front_roller_rpm;//RPM変数
tektomo 0:1ebf4907639c 74 int back_roller_rpm;
tektomo 0:1ebf4907639c 75 int front_roller_pulse[10];//ロリコンパルスバッファ
tektomo 0:1ebf4907639c 76 int back_roller_pulse[10];
tektomo 0:1ebf4907639c 77 int sum_front_roller_pulse;//ロリコンパルス
tektomo 0:1ebf4907639c 78 int sum_back_roller_pulse;
tektomo 0:1ebf4907639c 79 int ave_front_roller_pulse;//ロリコンパルス平均
tektomo 0:1ebf4907639c 80 int ave_back_roller_pulse;
tektomo 0:1ebf4907639c 81 char front_roller_data[1];
tektomo 0:1ebf4907639c 82 char back_roller_data[1];
tektomo 0:1ebf4907639c 83 int dmove_val;
tektomo 0:1ebf4907639c 84 static int j = 0;
tektomo 0:1ebf4907639c 85 int sonicval;
tektomo 0:1ebf4907639c 86 int mode = true;
tektomo 0:1ebf4907639c 87
tektomo 0:1ebf4907639c 88 //プロトタイプ宣言
tektomo 0:1ebf4907639c 89 //メイン制御関数
tektomo 0:1ebf4907639c 90 void ctrl();
tektomo 0:1ebf4907639c 91 void semiauto();
tektomo 0:1ebf4907639c 92 //移動関数
tektomo 0:1ebf4907639c 93 void amove_R();
tektomo 0:1ebf4907639c 94 void amave_L();
tektomo 0:1ebf4907639c 95 void dmove();
tektomo 0:1ebf4907639c 96 //void shortb(int i);
tektomo 0:1ebf4907639c 97 void backfif();
tektomo 0:1ebf4907639c 98 void sendi2c();
tektomo 0:1ebf4907639c 99 //回転数取得関数
tektomo 0:1ebf4907639c 100 void flip();
tektomo 0:1ebf4907639c 101 //ローラーPID
tektomo 0:1ebf4907639c 102 int roller_PID(int front, int back);
BIGBOSS04 1:5609d9509abf 103 int sec;
tektomo 0:1ebf4907639c 104 //発射関数
tektomo 0:1ebf4907639c 105 void shoot();
tektomo 0:1ebf4907639c 106 //その他諸関数
tektomo 0:1ebf4907639c 107 void changemode();
tektomo 0:1ebf4907639c 108 void printdata();
tektomo 0:1ebf4907639c 109 void outemergency();
tektomo 0:1ebf4907639c 110 void emergency();
BIGBOSS04 1:5609d9509abf 111 void TimerIsr();
BIGBOSS04 1:5609d9509abf 112 Ticker timer_;
tektomo 0:1ebf4907639c 113
tektomo 0:1ebf4907639c 114 //実装部
tektomo 0:1ebf4907639c 115
tektomo 0:1ebf4907639c 116 //メイン関数
tektomo 0:1ebf4907639c 117 int main()
tektomo 0:1ebf4907639c 118 {
BIGBOSS04 1:5609d9509abf 119 //float looptime;
tektomo 0:1ebf4907639c 120 pc.baud(460800);
tektomo 0:1ebf4907639c 121 emersig = 0;
tektomo 0:1ebf4907639c 122 Power = 1;
tektomo 0:1ebf4907639c 123 i2ccheck = 0;
tektomo 0:1ebf4907639c 124 get_rps.attach_us(&flip, 40000);
tektomo 0:1ebf4907639c 125 but.rise(shoot);
BIGBOSS04 1:5609d9509abf 126 /*
BIGBOSS04 1:5609d9509abf 127 TimerIsr();
BIGBOSS04 1:5609d9509abf 128 timer_.attach(&TimerIsr, 1);
BIGBOSS04 1:5609d9509abf 129 */
BIGBOSS04 1:5609d9509abf 130 while(true){
tektomo 0:1ebf4907639c 131 //pc.printf("\x1b[0m");
BIGBOSS04 2:235db39e0eaa 132 roller_PID(777,655);
BIGBOSS04 2:235db39e0eaa 133 //roller_PID(1600,800);
BIGBOSS04 1:5609d9509abf 134 topsonic.start();
BIGBOSS04 1:5609d9509abf 135 wait(0.01);
BIGBOSS04 1:5609d9509abf 136 sonicval = topsonic.get_dist_cm();
BIGBOSS04 1:5609d9509abf 137
BIGBOSS04 1:5609d9509abf 138 pc.printf("%3d,%3d,%d\n\r", abs(front_roller_rpm), abs(back_roller_rpm),sonicval);
BIGBOSS04 1:5609d9509abf 139
tektomo 0:1ebf4907639c 140 i2c.write(0x18, front_roller_data, 1, false);
tektomo 0:1ebf4907639c 141 i2c.write(0x20, back_roller_data, 1, false);
tektomo 0:1ebf4907639c 142 }
tektomo 0:1ebf4907639c 143 }
BIGBOSS04 1:5609d9509abf 144 /*
BIGBOSS04 1:5609d9509abf 145 void TimerIsr()
BIGBOSS04 1:5609d9509abf 146 {
BIGBOSS04 1:5609d9509abf 147 static int k = 0;
BIGBOSS04 1:5609d9509abf 148 sec = k % 60; // seconds
BIGBOSS04 1:5609d9509abf 149 k++;
BIGBOSS04 1:5609d9509abf 150 }
BIGBOSS04 1:5609d9509abf 151 */
tektomo 0:1ebf4907639c 152
tektomo 0:1ebf4907639c 153 //メイン制御関数
tektomo 0:1ebf4907639c 154 //緊急停止
tektomo 0:1ebf4907639c 155 void emergency()
tektomo 0:1ebf4907639c 156 {
tektomo 0:1ebf4907639c 157 emersig = 1;
tektomo 0:1ebf4907639c 158 Powemer = 1;
tektomo 0:1ebf4907639c 159 }
tektomo 0:1ebf4907639c 160
tektomo 0:1ebf4907639c 161 //緊急停止解除
tektomo 0:1ebf4907639c 162 void outemergency()
tektomo 0:1ebf4907639c 163 {
tektomo 0:1ebf4907639c 164 emersig = 0;
tektomo 0:1ebf4907639c 165 Powemer = 0;
tektomo 0:1ebf4907639c 166 }
tektomo 0:1ebf4907639c 167
tektomo 0:1ebf4907639c 168 //I2C送信
tektomo 0:1ebf4907639c 169 void sendi2c()
tektomo 0:1ebf4907639c 170 {
tektomo 0:1ebf4907639c 171 err[0] = i2c.write(address[0], send_data[0], 1);
tektomo 0:1ebf4907639c 172 err[1] = i2c.write(address[1], send_data[1], 1);
tektomo 0:1ebf4907639c 173 err[2] = i2c.write(address[2], send_data[2], 1);
tektomo 0:1ebf4907639c 174 err[3] = i2c.write(address[3], send_data[3], 1);
tektomo 0:1ebf4907639c 175 if (err[0]!=0 || err[1]!=0 || err[2]!=0 || err[3]!=0) {
tektomo 0:1ebf4907639c 176 while(10) {
tektomo 0:1ebf4907639c 177 i2ccheck = 1;
tektomo 0:1ebf4907639c 178 wait(0.1);
tektomo 0:1ebf4907639c 179
tektomo 0:1ebf4907639c 180 i2ccheck = 0;
tektomo 0:1ebf4907639c 181 }
tektomo 0:1ebf4907639c 182 } else {
tektomo 0:1ebf4907639c 183 i2ccheck = 1;
tektomo 0:1ebf4907639c 184 }
tektomo 0:1ebf4907639c 185 }
tektomo 0:1ebf4907639c 186
tektomo 0:1ebf4907639c 187 //移動平均をPID
tektomo 0:1ebf4907639c 188 void flip()
tektomo 0:1ebf4907639c 189 {
tektomo 0:1ebf4907639c 190 //前回のi番目のデータを消して新たにそこにデータを書き込む(キューのような考え?)
tektomo 0:1ebf4907639c 191 sum_front_roller_pulse -= front_roller_pulse[j];
tektomo 0:1ebf4907639c 192 sum_back_roller_pulse -= back_roller_pulse[j];
tektomo 0:1ebf4907639c 193
tektomo 0:1ebf4907639c 194 //配列のi番目の箱に取得パルスを代入
tektomo 0:1ebf4907639c 195 front_roller_pulse[j] = front_roller_wheel.getPulses();
tektomo 0:1ebf4907639c 196 back_roller_pulse[j] = back_roller_wheel.getPulses();
tektomo 0:1ebf4907639c 197
tektomo 0:1ebf4907639c 198 front_roller_wheel.reset();
tektomo 0:1ebf4907639c 199 back_roller_wheel.reset();
tektomo 0:1ebf4907639c 200
tektomo 0:1ebf4907639c 201 //i[0]~i[9]までの合計値を代入
tektomo 0:1ebf4907639c 202 sum_front_roller_pulse += front_roller_pulse[j];
tektomo 0:1ebf4907639c 203 sum_back_roller_pulse += back_roller_pulse[j];
tektomo 0:1ebf4907639c 204
tektomo 0:1ebf4907639c 205 //インクリメント
tektomo 0:1ebf4907639c 206 j++;
tektomo 0:1ebf4907639c 207 //iが最大値(9)になったらリセット
tektomo 0:1ebf4907639c 208 if(j > 9) {
tektomo 0:1ebf4907639c 209 j = 0;
tektomo 0:1ebf4907639c 210 }
tektomo 0:1ebf4907639c 211 //10回分の合計値を10で割り、取得パルスの平均を出す
tektomo 0:1ebf4907639c 212 ave_front_roller_pulse = sum_front_roller_pulse / 10;
tektomo 0:1ebf4907639c 213 ave_back_roller_pulse = sum_back_roller_pulse / 10;
tektomo 0:1ebf4907639c 214
tektomo 0:1ebf4907639c 215 //平均値をRPMへ変換
tektomo 0:1ebf4907639c 216 front_roller_rpm = ave_front_roller_pulse * 25 * 60 / 1200;
tektomo 0:1ebf4907639c 217 back_roller_rpm = ave_back_roller_pulse * 25 * 60 / 1200;
tektomo 0:1ebf4907639c 218 }
tektomo 0:1ebf4907639c 219
tektomo 0:1ebf4907639c 220 //ローラー
tektomo 0:1ebf4907639c 221 int roller_PID(int front, int back)
tektomo 0:1ebf4907639c 222 {
tektomo 0:1ebf4907639c 223 front_roller.setInputLimits(-2000, 2000);
tektomo 0:1ebf4907639c 224 back_roller.setInputLimits(-2000, 2000);
tektomo 0:1ebf4907639c 225
tektomo 0:1ebf4907639c 226 front_roller.setOutputLimits(0x84, 0xFF);
tektomo 0:1ebf4907639c 227 back_roller.setOutputLimits(0x84, 0xFF);
tektomo 0:1ebf4907639c 228
tektomo 0:1ebf4907639c 229 front_roller.setMode(AUTO_MODE);
tektomo 0:1ebf4907639c 230 back_roller.setMode(AUTO_MODE);
tektomo 0:1ebf4907639c 231
tektomo 0:1ebf4907639c 232 front_roller.setSetPoint(front);
tektomo 0:1ebf4907639c 233 back_roller.setSetPoint(back);
tektomo 0:1ebf4907639c 234
tektomo 0:1ebf4907639c 235 front_roller.setProcessValue(abs(front_roller_rpm));
tektomo 0:1ebf4907639c 236 back_roller.setProcessValue(abs(back_roller_rpm));
tektomo 0:1ebf4907639c 237
tektomo 0:1ebf4907639c 238 front_roller_data[0] = front_roller.compute();
tektomo 0:1ebf4907639c 239 back_roller_data[0] = back_roller.compute();
tektomo 0:1ebf4907639c 240
tektomo 0:1ebf4907639c 241 return 0;
tektomo 0:1ebf4907639c 242 }
tektomo 0:1ebf4907639c 243
tektomo 0:1ebf4907639c 244 //投擲動作
tektomo 0:1ebf4907639c 245 void shoot()
tektomo 0:1ebf4907639c 246 {
tektomo 0:1ebf4907639c 247 int time;
tektomo 0:1ebf4907639c 248 shootpet.start();
tektomo 0:1ebf4907639c 249 while(true) {
tektomo 0:1ebf4907639c 250 time = shootpet.read();
tektomo 0:1ebf4907639c 251 if(time <= 0.5) {
tektomo 0:1ebf4907639c 252 send_data2[0] = 0x0F;
tektomo 0:1ebf4907639c 253 i2c.write(0x40, send_data2, 1);
tektomo 0:1ebf4907639c 254 } else {
tektomo 0:1ebf4907639c 255 send_data2[0] = 0x80;
tektomo 0:1ebf4907639c 256 i2c.write(0x40, send_data2, 1);
tektomo 0:1ebf4907639c 257 shootpet.stop();
tektomo 0:1ebf4907639c 258 shootpet.reset();
tektomo 0:1ebf4907639c 259 break;
tektomo 0:1ebf4907639c 260 }
tektomo 0:1ebf4907639c 261 }
tektomo 0:1ebf4907639c 262 }