NITICRobotClub 2018Team-B / Mbed 2 deprecated NHK2018_BM

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

Dependencies:   HCSR04 PID PS3viaSBDBT QEI mbed

main.cpp@3:f462b1244d77, 2018-09-27 (annotated)

Committer:
tektomo
Date:
Thu Sep 27 09:25:21 2018 +0000
Revision:
3:f462b1244d77
Parent:
0:1ebf4907639c
Child:
4:58a2afff53a4
PID???????????;

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 "PS3_S.h"
tektomo 0:1ebf4907639c 14 #include "hcsr04.h"
tektomo 0:1ebf4907639c 15 #include "QEI.h"
tektomo 0:1ebf4907639c 16 #include "PID.h"
tektomo 0:1ebf4907639c 17
tektomo 0:1ebf4907639c 18 #define ps3data ps3s.condata
tektomo 0:1ebf4907639c 19 #define PI 3.14159265359
tektomo 3:f462b1244d77 20 #define roller_Kp 1.5
tektomo 3:f462b1244d77 21 #define froller_Ki 0.1
tektomo 3:f462b1244d77 22 #define broller_Ki 0.1
tektomo 3:f462b1244d77 23 #define roller_Kd 0.05
tektomo 0:1ebf4907639c 24
tektomo 0:1ebf4907639c 25 //定義
tektomo 0:1ebf4907639c 26
tektomo 0:1ebf4907639c 27 /*I2C定義
tektomo 0:1ebf4907639c 28 0前輪左:0x10
tektomo 0:1ebf4907639c 29 1前輪右:0x12
tektomo 0:1ebf4907639c 30 2後輪左:0x14
tektomo 0:1ebf4907639c 31 3後輪右:0x16
tektomo 0:1ebf4907639c 32 4投射前:0x18
tektomo 0:1ebf4907639c 33 5投射後ろ:0x20
tektomo 0:1ebf4907639c 34 6エアシリンダー:0x40
tektomo 0:1ebf4907639c 35 正転:0x84~0xFF
tektomo 0:1ebf4907639c 36 逆転:0x00~0x7C
tektomo 0:1ebf4907639c 37 ショートブレーキ:0x7D~0x83
tektomo 0:1ebf4907639c 38 */
tektomo 0:1ebf4907639c 39 I2C i2c(PB_9, PB_8);
tektomo 0:1ebf4907639c 40 //PS3コン定義
tektomo 0:1ebf4907639c 41 PS3s ps3s(PC_10, PC_11);
tektomo 0:1ebf4907639c 42 //PC定義
tektomo 0:1ebf4907639c 43 Serial pc(USBTX,USBRX);
tektomo 0:1ebf4907639c 44 //ロリコン定義
tektomo 0:1ebf4907639c 45 QEI front_roller_wheel(PC_8, PC_6, NC, 624);
tektomo 0:1ebf4907639c 46 QEI back_roller_wheel(PC_5, PA_12, NC, 624);
tektomo 0:1ebf4907639c 47 //PID定義
tektomo 0:1ebf4907639c 48 PID front_roller(roller_Kp, froller_Ki, roller_Kd, 0.001);
tektomo 0:1ebf4907639c 49 PID back_roller(roller_Kp, broller_Ki, roller_Kd, 0.001);
tektomo 0:1ebf4907639c 50 //超音波センサ定義
tektomo 0:1ebf4907639c 51 HCSR04 topsonic(PB_2,PB_1);
tektomo 0:1ebf4907639c 52 //HCSR04 undersonic(PB_15,PB_14);
tektomo 0:1ebf4907639c 53 //LED定義
tektomo 0:1ebf4907639c 54 DigitalOut Power(PC_12);//Green
tektomo 0:1ebf4907639c 55 DigitalOut i2ccheck(PB_7);//Blue
tektomo 0:1ebf4907639c 56 DigitalOut sncled(PC_2);//Yellow
tektomo 0:1ebf4907639c 57 DigitalOut Powemer(PC_3);//Red
tektomo 0:1ebf4907639c 58 DigitalOut myled(LED1);
tektomo 0:1ebf4907639c 59 //緊急停止定義
tektomo 0:1ebf4907639c 60 DigitalOut emersig(PC_0);
tektomo 0:1ebf4907639c 61 //タイマー定義
tektomo 0:1ebf4907639c 62 Ticker get_rps;//ロリコンからRPMの読み取り
tektomo 0:1ebf4907639c 63 Timer shootpet;
tektomo 0:1ebf4907639c 64 Timer mainloop;
tektomo 0:1ebf4907639c 65
tektomo 0:1ebf4907639c 66 InterruptIn but(USER_BUTTON);
tektomo 0:1ebf4907639c 67
tektomo 0:1ebf4907639c 68 //変数定義
tektomo 0:1ebf4907639c 69 //データ配列
tektomo 0:1ebf4907639c 70 char send_data[4][1];//足回り用データ配列
tektomo 0:1ebf4907639c 71 char send_data2[1];//エアシリンダー用データ配列
tektomo 0:1ebf4907639c 72 int address[4] = {0x10, 0x12, 0x14, 0x16};//i2cアドレス
tektomo 0:1ebf4907639c 73 int err[4] = {1, 1, 1, 1};//I2Cリザルト
tektomo 0:1ebf4907639c 74
tektomo 0:1ebf4907639c 75 int front_roller_rpm;//RPM変数
tektomo 0:1ebf4907639c 76 int back_roller_rpm;
tektomo 0:1ebf4907639c 77 int front_roller_pulse[10];//ロリコンパルスバッファ
tektomo 0:1ebf4907639c 78 int back_roller_pulse[10];
tektomo 0:1ebf4907639c 79 int sum_front_roller_pulse;//ロリコンパルス
tektomo 0:1ebf4907639c 80 int sum_back_roller_pulse;
tektomo 0:1ebf4907639c 81 int ave_front_roller_pulse;//ロリコンパルス平均
tektomo 0:1ebf4907639c 82 int ave_back_roller_pulse;
tektomo 0:1ebf4907639c 83 char front_roller_data[1];
tektomo 0:1ebf4907639c 84 char back_roller_data[1];
tektomo 0:1ebf4907639c 85 int dmove_val;
tektomo 0:1ebf4907639c 86 static int j = 0;
tektomo 0:1ebf4907639c 87 int sonicval;
tektomo 0:1ebf4907639c 88 int mode = true;
tektomo 3:f462b1244d77 89 int table;
tektomo 3:f462b1244d77 90 double looptime;
tektomo 0:1ebf4907639c 91
tektomo 0:1ebf4907639c 92 //プロトタイプ宣言
tektomo 0:1ebf4907639c 93 //メイン制御関数
tektomo 0:1ebf4907639c 94 void ctrl();
tektomo 0:1ebf4907639c 95 void semiauto();
tektomo 0:1ebf4907639c 96 //移動関数
tektomo 0:1ebf4907639c 97 void amove_R();
tektomo 0:1ebf4907639c 98 void amave_L();
tektomo 0:1ebf4907639c 99 void dmove();
tektomo 0:1ebf4907639c 100 //void shortb(int i);
tektomo 0:1ebf4907639c 101 void backfif();
tektomo 0:1ebf4907639c 102 void sendi2c();
tektomo 0:1ebf4907639c 103 //回転数取得関数
tektomo 0:1ebf4907639c 104 void flip();
tektomo 0:1ebf4907639c 105 //ローラーPID
tektomo 0:1ebf4907639c 106 int roller_PID(int front, int back);
tektomo 0:1ebf4907639c 107 //発射関数
tektomo 0:1ebf4907639c 108 void shoot();
tektomo 0:1ebf4907639c 109 //その他諸関数
tektomo 0:1ebf4907639c 110 void changemode();
tektomo 0:1ebf4907639c 111 void printdata();
tektomo 0:1ebf4907639c 112 void outemergency();
tektomo 0:1ebf4907639c 113 void emergency();
tektomo 0:1ebf4907639c 114
tektomo 0:1ebf4907639c 115 //実装部
tektomo 0:1ebf4907639c 116
tektomo 0:1ebf4907639c 117 //メイン関数
tektomo 0:1ebf4907639c 118 int main()
tektomo 0:1ebf4907639c 119 {
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);
tektomo 3:f462b1244d77 126 pc.printf("\x1b[0m");
tektomo 0:1ebf4907639c 127 while(true) {
tektomo 0:1ebf4907639c 128 looptime = mainloop.read();
tektomo 3:f462b1244d77 129 pc.printf("%3d,%3d,%f,%d\n\r",front_roller_rpm, back_roller_rpm, looptime, sonicval);
tektomo 0:1ebf4907639c 130 mainloop.reset();
tektomo 0:1ebf4907639c 131 mainloop.start();
tektomo 3:f462b1244d77 132 roller_PID(777,655);
tektomo 3:f462b1244d77 133 //front_roller_data[0] = 0x80;
tektomo 3:f462b1244d77 134 //back_roller_data[0] = 0x80;
tektomo 0:1ebf4907639c 135 i2c.write(0x18, front_roller_data, 1, false);
tektomo 0:1ebf4907639c 136 i2c.write(0x20, back_roller_data, 1, false);
tektomo 0:1ebf4907639c 137 topsonic.start();
tektomo 0:1ebf4907639c 138 wait(0.01);
tektomo 0:1ebf4907639c 139 sonicval = topsonic.get_dist_cm();
tektomo 3:f462b1244d77 140 //ps3s.getdata();
tektomo 3:f462b1244d77 141 //ps3s.checkdata();
tektomo 3:f462b1244d77 142 //ctrl();
tektomo 3:f462b1244d77 143 //printdata();
tektomo 0:1ebf4907639c 144 mainloop.stop();
tektomo 0:1ebf4907639c 145 }
tektomo 0:1ebf4907639c 146 }
tektomo 0:1ebf4907639c 147
tektomo 0:1ebf4907639c 148 //メイン制御関数
tektomo 0:1ebf4907639c 149 void ctrl()
tektomo 0:1ebf4907639c 150 {
tektomo 0:1ebf4907639c 151
tektomo 0:1ebf4907639c 152 if(ps3data[12]!=0) {//Select
tektomo 0:1ebf4907639c 153 emergency();//緊急停止
tektomo 0:1ebf4907639c 154 } else if(ps3data[13]!=0) {//Start
tektomo 0:1ebf4907639c 155 outemergency();//緊急停止解除
tektomo 0:1ebf4907639c 156 }
tektomo 0:1ebf4907639c 157
tektomo 0:1ebf4907639c 158 if(emersig == 0) {
tektomo 0:1ebf4907639c 159 for(int i = 0; i < 4; i++) {
tektomo 0:1ebf4907639c 160 send_data[i][0] = 0x83;//ショートブレーキ
tektomo 0:1ebf4907639c 161 }
tektomo 0:1ebf4907639c 162
tektomo 0:1ebf4907639c 163 if(ps3data[0]!=0) {
tektomo 0:1ebf4907639c 164 dmove_val += 1;//上
tektomo 0:1ebf4907639c 165 } else if(ps3data[1]!=0) {
tektomo 0:1ebf4907639c 166 dmove_val += 2;//下
tektomo 0:1ebf4907639c 167 } else if(ps3data[2]!=0) {
tektomo 0:1ebf4907639c 168 dmove_val += 4;//右
tektomo 0:1ebf4907639c 169 } else if(ps3data[3]!=0) {
tektomo 0:1ebf4907639c 170 dmove_val += 8;//左
tektomo 0:1ebf4907639c 171 } else if(ps3data[8]!=0) {//L1
tektomo 0:1ebf4907639c 172 dmove_val += 32;//左回転
tektomo 0:1ebf4907639c 173 } else if(ps3data[10]!=0) {//R1
tektomo 0:1ebf4907639c 174 dmove_val += 16;//右回転
tektomo 0:1ebf4907639c 175 }
tektomo 0:1ebf4907639c 176 if (dmove_val != 0) {
tektomo 0:1ebf4907639c 177 dmove();//方向キー+L1R1キー
tektomo 0:1ebf4907639c 178 }
tektomo 0:1ebf4907639c 179
tektomo 0:1ebf4907639c 180 if(((ps3data[16] <= 51) || (ps3data[16] >= 77)) || ((ps3data[17] <= 51) || (ps3data[17] >= 77))) {
tektomo 0:1ebf4907639c 181 amove_R();//アナログパッド
tektomo 0:1ebf4907639c 182 }
tektomo 0:1ebf4907639c 183
tektomo 0:1ebf4907639c 184 if(ps3data[7]!=0) {//しかく
tektomo 3:f462b1244d77 185 //backfif();50cm後退
tektomo 0:1ebf4907639c 186 } else if(ps3data[6]!=0) {//まる
tektomo 0:1ebf4907639c 187 shoot();//投擲
tektomo 0:1ebf4907639c 188 } else if(ps3data[4]!=0) { //さんかく
tektomo 0:1ebf4907639c 189 semiauto();
tektomo 0:1ebf4907639c 190 } else if(ps3data[5]!=0) { //しかく
tektomo 0:1ebf4907639c 191 changemode();
tektomo 0:1ebf4907639c 192 }
tektomo 0:1ebf4907639c 193 sendi2c();
tektomo 0:1ebf4907639c 194 } else {
tektomo 0:1ebf4907639c 195 }
tektomo 0:1ebf4907639c 196 }
tektomo 0:1ebf4907639c 197
tektomo 0:1ebf4907639c 198 //緊急停止
tektomo 0:1ebf4907639c 199 void emergency()
tektomo 0:1ebf4907639c 200 {
tektomo 0:1ebf4907639c 201 emersig = 1;
tektomo 0:1ebf4907639c 202 Powemer = 1;
tektomo 0:1ebf4907639c 203 }
tektomo 0:1ebf4907639c 204
tektomo 0:1ebf4907639c 205 //緊急停止解除
tektomo 0:1ebf4907639c 206 void outemergency()
tektomo 0:1ebf4907639c 207 {
tektomo 0:1ebf4907639c 208 emersig = 0;
tektomo 0:1ebf4907639c 209 Powemer = 0;
tektomo 0:1ebf4907639c 210 }
tektomo 0:1ebf4907639c 211
tektomo 0:1ebf4907639c 212 //右アナログパッド
tektomo 0:1ebf4907639c 213 void amove_R()
tektomo 0:1ebf4907639c 214 {
tektomo 0:1ebf4907639c 215 int RXdata = ps3data[16];
tektomo 0:1ebf4907639c 216 int RYdata = ps3data[17];
tektomo 0:1ebf4907639c 217
tektomo 0:1ebf4907639c 218 if(RXdata == 64 && RYdata < 64) {
tektomo 0:1ebf4907639c 219 //垂直上
tektomo 0:1ebf4907639c 220 send_data[0][0] = abs(RYdata - 53) + 202 - RYdata - 8;
tektomo 0:1ebf4907639c 221 send_data[1][0] = abs(RYdata - 53) + 202 - RYdata;
tektomo 0:1ebf4907639c 222 send_data[2][0] = abs(RYdata - 53) + 202 - RYdata - 8;
tektomo 0:1ebf4907639c 223 send_data[3][0] = abs(RYdata - 53) + 202 - RYdata;
tektomo 0:1ebf4907639c 224
tektomo 0:1ebf4907639c 225 } else if(RXdata == 64 && RYdata > 64) {
tektomo 0:1ebf4907639c 226 //垂直下
tektomo 0:1ebf4907639c 227 send_data[0][0] = abs(RYdata - 127) * 2;
tektomo 0:1ebf4907639c 228 send_data[1][0] = abs(RYdata - 127) * 2 + 8;
tektomo 0:1ebf4907639c 229 send_data[2][0] = abs(RYdata - 127) * 2;
tektomo 0:1ebf4907639c 230 send_data[3][0] = abs(RYdata - 127) * 2 + 8;
tektomo 0:1ebf4907639c 231
tektomo 0:1ebf4907639c 232 } else if(RXdata > 64 && RYdata == 64) {
tektomo 0:1ebf4907639c 233 //垂直右
tektomo 0:1ebf4907639c 234 send_data[0][0] = (RXdata - 75) + 203 - abs(RXdata - 127) - 8;
tektomo 0:1ebf4907639c 235 send_data[3][0] = (RXdata - 75) + 203 - abs(RXdata - 127) - 8;
tektomo 0:1ebf4907639c 236 send_data[1][0] = abs(RXdata - 127) * 2;
tektomo 0:1ebf4907639c 237 send_data[2][0] = abs(RXdata - 127) * 2;
tektomo 0:1ebf4907639c 238
tektomo 0:1ebf4907639c 239 } else if(RXdata < 64 && RYdata == 64) {
tektomo 0:1ebf4907639c 240 //垂直左
tektomo 0:1ebf4907639c 241 send_data[0][0] = RXdata * 2;
tektomo 0:1ebf4907639c 242 send_data[3][0] = RXdata * 2;
tektomo 0:1ebf4907639c 243 send_data[1][0] = abs(RXdata - 53) + 202 - RXdata - 8;
tektomo 0:1ebf4907639c 244 send_data[2][0] = abs(RXdata - 53) + 202 - RXdata - 8;
tektomo 0:1ebf4907639c 245
tektomo 0:1ebf4907639c 246 } else if (RXdata > 64 && RYdata < 64) {
tektomo 0:1ebf4907639c 247 //右上
tektomo 0:1ebf4907639c 248 send_data[0][0] = (RXdata - 74) + 202 - abs(RXdata - 127);
tektomo 0:1ebf4907639c 249 send_data[3][0] = (RXdata - 74) + 202 - abs(RXdata - 127);
tektomo 0:1ebf4907639c 250 } else if(RXdata < 64 && RYdata < 64) {
tektomo 0:1ebf4907639c 251 //左上
tektomo 0:1ebf4907639c 252 send_data[1][0] = abs(RXdata - 54) + 201 - RXdata;
tektomo 0:1ebf4907639c 253 send_data[2][0] = abs(RXdata - 54) + 201 - RXdata;
tektomo 0:1ebf4907639c 254 } else if(RXdata < 64 && RYdata > 64) {
tektomo 0:1ebf4907639c 255 //左下
tektomo 0:1ebf4907639c 256 send_data[0][0] = RXdata * 2;
tektomo 0:1ebf4907639c 257 send_data[3][0] = RXdata * 2;
tektomo 0:1ebf4907639c 258 } else if(RXdata > 64 && RYdata > 64) {
tektomo 0:1ebf4907639c 259 //右下
tektomo 0:1ebf4907639c 260 send_data[1][0] = abs(RXdata - 127) * 2;
tektomo 0:1ebf4907639c 261 send_data[2][0] = abs(RXdata - 127) * 2;
tektomo 0:1ebf4907639c 262 }
tektomo 0:1ebf4907639c 263 }
tektomo 0:1ebf4907639c 264
tektomo 0:1ebf4907639c 265 void dmove()
tektomo 0:1ebf4907639c 266 {
tektomo 0:1ebf4907639c 267 int k = 32;
tektomo 0:1ebf4907639c 268 while(true) {
tektomo 0:1ebf4907639c 269 if (dmove_val == 0) {
tektomo 0:1ebf4907639c 270 break;
tektomo 0:1ebf4907639c 271 } else {
tektomo 0:1ebf4907639c 272 if (dmove_val - k >= 0) {
tektomo 0:1ebf4907639c 273 sncled = 1;
tektomo 0:1ebf4907639c 274 switch (k) {
tektomo 0:1ebf4907639c 275 case 1:
tektomo 0:1ebf4907639c 276 send_data[0][0] = 0xF7;
tektomo 0:1ebf4907639c 277 send_data[1][0] = 0xFF;
tektomo 0:1ebf4907639c 278 send_data[2][0] = 0xF7;
tektomo 0:1ebf4907639c 279 send_data[3][0] = 0xFF;
tektomo 0:1ebf4907639c 280 break;
tektomo 0:1ebf4907639c 281 case 2:
tektomo 0:1ebf4907639c 282 send_data[0][0] = 0x00;
tektomo 0:1ebf4907639c 283 send_data[1][0] = 0x0A;
tektomo 0:1ebf4907639c 284 send_data[2][0] = 0x00;
tektomo 0:1ebf4907639c 285 send_data[3][0] = 0x0A;
tektomo 0:1ebf4907639c 286 break;
tektomo 0:1ebf4907639c 287 case 4:
tektomo 0:1ebf4907639c 288 send_data[0][0] = 0xF5;
tektomo 0:1ebf4907639c 289 send_data[3][0] = 0xFF;
tektomo 0:1ebf4907639c 290 send_data[1][0] = 0x09;
tektomo 0:1ebf4907639c 291 send_data[2][0] = 0x00;
tektomo 0:1ebf4907639c 292 break;
tektomo 0:1ebf4907639c 293 case 8:
tektomo 0:1ebf4907639c 294 send_data[0][0] = 0x00;
tektomo 0:1ebf4907639c 295 send_data[3][0] = 0x09;
tektomo 0:1ebf4907639c 296 send_data[1][0] = 0xFF;
tektomo 0:1ebf4907639c 297 send_data[2][0] = 0xF5;
tektomo 0:1ebf4907639c 298 break;
tektomo 0:1ebf4907639c 299 case 16:
tektomo 0:1ebf4907639c 300 send_data[1][0] = 0x00;
tektomo 0:1ebf4907639c 301 send_data[3][0] = 0x00;
tektomo 0:1ebf4907639c 302 send_data[0][0] = 0xFF;
tektomo 0:1ebf4907639c 303 send_data[2][0] = 0xFF;
tektomo 0:1ebf4907639c 304 break;
tektomo 0:1ebf4907639c 305 case 32:
tektomo 0:1ebf4907639c 306 send_data[0][0] = 0x00;
tektomo 0:1ebf4907639c 307 send_data[2][0] = 0x00;
tektomo 0:1ebf4907639c 308 send_data[1][0] = 0xFF;
tektomo 0:1ebf4907639c 309 send_data[3][0] = 0xFF;
tektomo 0:1ebf4907639c 310 break;
tektomo 0:1ebf4907639c 311 default:
tektomo 0:1ebf4907639c 312 break;
tektomo 0:1ebf4907639c 313 }
tektomo 0:1ebf4907639c 314 sncled = 0;
tektomo 0:1ebf4907639c 315 dmove_val -= k;
tektomo 0:1ebf4907639c 316 } else {
tektomo 0:1ebf4907639c 317 k /= 2;
tektomo 0:1ebf4907639c 318 }
tektomo 0:1ebf4907639c 319 }
tektomo 0:1ebf4907639c 320 }
tektomo 0:1ebf4907639c 321 }
tektomo 0:1ebf4907639c 322
tektomo 0:1ebf4907639c 323 //ショートブレーキ
tektomo 0:1ebf4907639c 324 void shortb(int i)
tektomo 0:1ebf4907639c 325 {
tektomo 0:1ebf4907639c 326 i2ccheck = 0;
tektomo 0:1ebf4907639c 327 send_data[i][0] = 0x83;
tektomo 0:1ebf4907639c 328 }
tektomo 0:1ebf4907639c 329
tektomo 3:f462b1244d77 330 /*50cm後退
tektomo 0:1ebf4907639c 331 void backfif()
tektomo 0:1ebf4907639c 332 {
tektomo 0:1ebf4907639c 333 int snc1;
tektomo 0:1ebf4907639c 334 //int snc2;
tektomo 0:1ebf4907639c 335 while(true) {
tektomo 0:1ebf4907639c 336 topsonic.start();
tektomo 0:1ebf4907639c 337 //undersonic.start();
tektomo 0:1ebf4907639c 338 snc1 = topsonic.get_dist_cm();
tektomo 0:1ebf4907639c 339 //snc2 = undersonic.get_dist_cm();
tektomo 0:1ebf4907639c 340 if (snc1 != 0) { // && (snc2 != 0)) {
tektomo 0:1ebf4907639c 341 break;
tektomo 0:1ebf4907639c 342 }
tektomo 0:1ebf4907639c 343 }
tektomo 0:1ebf4907639c 344
tektomo 0:1ebf4907639c 345 int snc1buffer;
tektomo 0:1ebf4907639c 346 //int snc2buffer;
tektomo 0:1ebf4907639c 347 while(true) {
tektomo 0:1ebf4907639c 348 topsonic.start();
tektomo 0:1ebf4907639c 349 //undersonic.start();
tektomo 0:1ebf4907639c 350 snc1buffer = topsonic.get_dist_cm();
tektomo 0:1ebf4907639c 351 //snc2buffer = undersonic.get_dist_cm();
tektomo 0:1ebf4907639c 352 if (snc1buffer == 0) { // || snc2buffer == 0) {
tektomo 0:1ebf4907639c 353 continue;
tektomo 0:1ebf4907639c 354 }
tektomo 0:1ebf4907639c 355 if(snc1buffer >= snc1 + 50) {//|| (snc2buffer >= snc2 + 50)) {
tektomo 0:1ebf4907639c 356 sncled = 1;
tektomo 0:1ebf4907639c 357 for(int i = 0; i < 4; i++) {
tektomo 0:1ebf4907639c 358 send_data[i][0] = 0x83;
tektomo 0:1ebf4907639c 359 }
tektomo 0:1ebf4907639c 360 break;
tektomo 0:1ebf4907639c 361 } else {
tektomo 0:1ebf4907639c 362 for(int i = 0; i < 4; i++) {
tektomo 0:1ebf4907639c 363 send_data[i][0] = 0x10;
tektomo 0:1ebf4907639c 364 i2c.write(address[i], send_data[i], 1);
tektomo 0:1ebf4907639c 365 pc.printf("Goal:%dcm,Now:%dcm\n\r", snc1 + 50, snc1buffer);
tektomo 0:1ebf4907639c 366 }
tektomo 0:1ebf4907639c 367 }
tektomo 0:1ebf4907639c 368 }
tektomo 3:f462b1244d77 369 }*/
tektomo 0:1ebf4907639c 370
tektomo 0:1ebf4907639c 371 //I2C送信
tektomo 0:1ebf4907639c 372 void sendi2c()
tektomo 0:1ebf4907639c 373 {
tektomo 0:1ebf4907639c 374 err[0] = i2c.write(address[0], send_data[0], 1);
tektomo 0:1ebf4907639c 375 err[1] = i2c.write(address[1], send_data[1], 1);
tektomo 0:1ebf4907639c 376 err[2] = i2c.write(address[2], send_data[2], 1);
tektomo 0:1ebf4907639c 377 err[3] = i2c.write(address[3], send_data[3], 1);
tektomo 0:1ebf4907639c 378 if (err[0]!=0 || err[1]!=0 || err[2]!=0 || err[3]!=0) {
tektomo 0:1ebf4907639c 379 while(10) {
tektomo 0:1ebf4907639c 380 i2ccheck = 1;
tektomo 0:1ebf4907639c 381 wait(0.1);
tektomo 0:1ebf4907639c 382
tektomo 0:1ebf4907639c 383 i2ccheck = 0;
tektomo 0:1ebf4907639c 384 }
tektomo 0:1ebf4907639c 385 } else {
tektomo 0:1ebf4907639c 386 i2ccheck = 1;
tektomo 0:1ebf4907639c 387 }
tektomo 0:1ebf4907639c 388 }
tektomo 0:1ebf4907639c 389
tektomo 0:1ebf4907639c 390 //移動平均をPID
tektomo 0:1ebf4907639c 391 void flip()
tektomo 0:1ebf4907639c 392 {
tektomo 0:1ebf4907639c 393 //前回のi番目のデータを消して新たにそこにデータを書き込む(キューのような考え?)
tektomo 0:1ebf4907639c 394 sum_front_roller_pulse -= front_roller_pulse[j];
tektomo 0:1ebf4907639c 395 sum_back_roller_pulse -= back_roller_pulse[j];
tektomo 0:1ebf4907639c 396
tektomo 0:1ebf4907639c 397 //配列のi番目の箱に取得パルスを代入
tektomo 0:1ebf4907639c 398 front_roller_pulse[j] = front_roller_wheel.getPulses();
tektomo 0:1ebf4907639c 399 back_roller_pulse[j] = back_roller_wheel.getPulses();
tektomo 0:1ebf4907639c 400
tektomo 0:1ebf4907639c 401 front_roller_wheel.reset();
tektomo 0:1ebf4907639c 402 back_roller_wheel.reset();
tektomo 0:1ebf4907639c 403
tektomo 0:1ebf4907639c 404 //i[0]~i[9]までの合計値を代入
tektomo 0:1ebf4907639c 405 sum_front_roller_pulse += front_roller_pulse[j];
tektomo 0:1ebf4907639c 406 sum_back_roller_pulse += back_roller_pulse[j];
tektomo 0:1ebf4907639c 407
tektomo 0:1ebf4907639c 408 //インクリメント
tektomo 0:1ebf4907639c 409 j++;
tektomo 0:1ebf4907639c 410 //iが最大値(9)になったらリセット
tektomo 0:1ebf4907639c 411 if(j > 9) {
tektomo 0:1ebf4907639c 412 j = 0;
tektomo 0:1ebf4907639c 413 }
tektomo 0:1ebf4907639c 414 //10回分の合計値を10で割り、取得パルスの平均を出す
tektomo 0:1ebf4907639c 415 ave_front_roller_pulse = sum_front_roller_pulse / 10;
tektomo 0:1ebf4907639c 416 ave_back_roller_pulse = sum_back_roller_pulse / 10;
tektomo 0:1ebf4907639c 417
tektomo 0:1ebf4907639c 418 //平均値をRPMへ変換
tektomo 0:1ebf4907639c 419 front_roller_rpm = ave_front_roller_pulse * 25 * 60 / 1200;
tektomo 0:1ebf4907639c 420 back_roller_rpm = ave_back_roller_pulse * 25 * 60 / 1200;
tektomo 0:1ebf4907639c 421 }
tektomo 0:1ebf4907639c 422
tektomo 0:1ebf4907639c 423 //ローラー
tektomo 0:1ebf4907639c 424 int roller_PID(int front, int back)
tektomo 0:1ebf4907639c 425 {
tektomo 0:1ebf4907639c 426 front_roller.setInputLimits(-2000, 2000);
tektomo 0:1ebf4907639c 427 back_roller.setInputLimits(-2000, 2000);
tektomo 0:1ebf4907639c 428
tektomo 0:1ebf4907639c 429 front_roller.setOutputLimits(0x84, 0xFF);
tektomo 0:1ebf4907639c 430 back_roller.setOutputLimits(0x84, 0xFF);
tektomo 0:1ebf4907639c 431
tektomo 0:1ebf4907639c 432 front_roller.setMode(AUTO_MODE);
tektomo 0:1ebf4907639c 433 back_roller.setMode(AUTO_MODE);
tektomo 0:1ebf4907639c 434
tektomo 0:1ebf4907639c 435 front_roller.setSetPoint(front);
tektomo 0:1ebf4907639c 436 back_roller.setSetPoint(back);
tektomo 0:1ebf4907639c 437
tektomo 0:1ebf4907639c 438 front_roller.setProcessValue(abs(front_roller_rpm));
tektomo 0:1ebf4907639c 439 back_roller.setProcessValue(abs(back_roller_rpm));
tektomo 0:1ebf4907639c 440
tektomo 0:1ebf4907639c 441 front_roller_data[0] = front_roller.compute();
tektomo 0:1ebf4907639c 442 back_roller_data[0] = back_roller.compute();
tektomo 0:1ebf4907639c 443
tektomo 0:1ebf4907639c 444 return 0;
tektomo 0:1ebf4907639c 445 }
tektomo 0:1ebf4907639c 446
tektomo 0:1ebf4907639c 447 //投擲動作
tektomo 0:1ebf4907639c 448 void shoot()
tektomo 0:1ebf4907639c 449 {
tektomo 0:1ebf4907639c 450 int time;
tektomo 3:f462b1244d77 451 int front;
tektomo 3:f462b1244d77 452 int back;
tektomo 3:f462b1244d77 453 if(table == 0) {
tektomo 3:f462b1244d77 454 front = 666;
tektomo 3:f462b1244d77 455 back = 666;
tektomo 3:f462b1244d77 456 } else if(table == 1) {
tektomo 3:f462b1244d77 457 front = 777;
tektomo 3:f462b1244d77 458 back = 777;
tektomo 3:f462b1244d77 459 }
tektomo 3:f462b1244d77 460 while(true) {
tektomo 3:f462b1244d77 461 shootpet.start();
tektomo 3:f462b1244d77 462 roller_PID(front,back);
tektomo 3:f462b1244d77 463 i2c.write(0x18, front_roller_data, 1, false);
tektomo 3:f462b1244d77 464 i2c.write(0x20, back_roller_data, 1, false);
tektomo 3:f462b1244d77 465 if(shootpet.read() == 10) {
tektomo 3:f462b1244d77 466 break;
tektomo 3:f462b1244d77 467 }
tektomo 3:f462b1244d77 468 }
tektomo 3:f462b1244d77 469 shootpet.stop();
tektomo 3:f462b1244d77 470 shootpet.reset();
tektomo 0:1ebf4907639c 471 shootpet.start();
tektomo 0:1ebf4907639c 472 while(true) {
tektomo 0:1ebf4907639c 473 time = shootpet.read();
tektomo 0:1ebf4907639c 474 if(time <= 0.5) {
tektomo 0:1ebf4907639c 475 send_data2[0] = 0x0F;
tektomo 0:1ebf4907639c 476 i2c.write(0x40, send_data2, 1);
tektomo 0:1ebf4907639c 477 } else {
tektomo 0:1ebf4907639c 478 send_data2[0] = 0x80;
tektomo 0:1ebf4907639c 479 i2c.write(0x40, send_data2, 1);
tektomo 3:f462b1244d77 480 i2c.write(0x18, send_data2, 1, false);
tektomo 3:f462b1244d77 481 i2c.write(0x20, send_data2, 1, false);
tektomo 0:1ebf4907639c 482 shootpet.stop();
tektomo 0:1ebf4907639c 483 shootpet.reset();
tektomo 0:1ebf4907639c 484 break;
tektomo 0:1ebf4907639c 485 }
tektomo 0:1ebf4907639c 486 }
tektomo 0:1ebf4907639c 487 }
tektomo 0:1ebf4907639c 488
tektomo 0:1ebf4907639c 489
tektomo 0:1ebf4907639c 490 //セミオートプレイテスト
tektomo 0:1ebf4907639c 491
tektomo 0:1ebf4907639c 492 void semiauto()
tektomo 0:1ebf4907639c 493 {
tektomo 0:1ebf4907639c 494 while(true) {
tektomo 3:f462b1244d77 495 looptime = mainloop.read();
tektomo 3:f462b1244d77 496 pc.printf("%f,%d\n\r", looptime, sonicval);
tektomo 3:f462b1244d77 497 mainloop.reset();
tektomo 3:f462b1244d77 498 mainloop.start();
tektomo 3:f462b1244d77 499 send_data[0][0] = 0xE1;
tektomo 3:f462b1244d77 500 send_data[3][0] = 0xEC;
tektomo 3:f462b1244d77 501 send_data[1][0] = 0x1D;
tektomo 3:f462b1244d77 502 send_data[2][0] = 0x14;
tektomo 0:1ebf4907639c 503 topsonic.start();
tektomo 0:1ebf4907639c 504 wait(0.01);
tektomo 0:1ebf4907639c 505 sonicval = topsonic.get_dist_cm();
tektomo 3:f462b1244d77 506 if(sonicval < 100) {
tektomo 3:f462b1244d77 507 send_data[0][0] = 0xF7;
tektomo 3:f462b1244d77 508 send_data[1][0] = 0xFF;
tektomo 3:f462b1244d77 509 send_data[2][0] = 0xF7;
tektomo 3:f462b1244d77 510 send_data[3][0] = 0xFF;
tektomo 3:f462b1244d77 511 if(sonicval == 75) {
tektomo 3:f462b1244d77 512 send_data[0][0] = 0x80;
tektomo 3:f462b1244d77 513 send_data[1][0] = 0x80;
tektomo 3:f462b1244d77 514 send_data[2][0] = 0x80;
tektomo 3:f462b1244d77 515 send_data[3][0] = 0x80;
tektomo 3:f462b1244d77 516 }
tektomo 0:1ebf4907639c 517 }
tektomo 3:f462b1244d77 518 sendi2c();
tektomo 3:f462b1244d77 519 if(sonicval == 75) {
tektomo 3:f462b1244d77 520 table = 0;
tektomo 3:f462b1244d77 521 shoot();
tektomo 0:1ebf4907639c 522 }
tektomo 3:f462b1244d77 523 mainloop.stop();
tektomo 0:1ebf4907639c 524 }
tektomo 0:1ebf4907639c 525 }
tektomo 0:1ebf4907639c 526
tektomo 3:f462b1244d77 527 void changemode()
tektomo 3:f462b1244d77 528 {
tektomo 3:f462b1244d77 529 mode = !mode;
tektomo 3:f462b1244d77 530 if(mode) {
tektomo 3:f462b1244d77 531 pc.printf("\x1b[41m");
tektomo 3:f462b1244d77 532 } else {
tektomo 3:f462b1244d77 533 pc.printf("\x1b[44m");
tektomo 0:1ebf4907639c 534 }
tektomo 3:f462b1244d77 535 }
tektomo 0:1ebf4907639c 536
tektomo 0:1ebf4907639c 537
tektomo 0:1ebf4907639c 538 //デバック用データ出力
tektomo 3:f462b1244d77 539 void printdata()
tektomo 3:f462b1244d77 540 {
tektomo 3:f462b1244d77 541 for (int i = 0; i < 7; i++) {
tektomo 3:f462b1244d77 542 pc.printf("%d,",ps3s.Rdata[i]);
tektomo 3:f462b1244d77 543 }
tektomo 3:f462b1244d77 544 pc.printf("%d\n\r",ps3s.Rdata[7]);
tektomo 3:f462b1244d77 545 }