NITICRobotClub 2018Team-B / Mbed 2 deprecated NHK2018_BM

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

Dependencies:   HCSR04 PID PS3viaSBDBT QEI mbed

main.cpp@7:9837f47a2f55, 2018-10-10 (annotated)

Committer:
tektomo
Date:
Wed Oct 10 13:03:16 2018 +0000
Revision:
7:9837f47a2f55
Parent:
6:eaeaaf374263 
??????

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