NITICRobotClub 2018Team-B
NHK2018Bチームの手動ロボットのプログラムです。（製作中）
Dependencies: HCSR04 PID PS3viaSBDBT QEI mbed
Diff: main.cpp
- Revision:
- 6:eaeaaf374263
- Parent:
- 5:79cc2a126504
- Child:
- 7:9837f47a2f55
--- a/main.cpp Tue Oct 02 13:17:01 2018 +0000
+++ b/main.cpp Sat Oct 06 13:11:46 2018 +0000
@@ -17,9 +17,9 @@
#define ps3data ps3s.condata
#define PI 3.14159265359
-#define roller_Kp 1.5
-#define froller_Ki 0.5
-#define broller_Ki 0.5
+#define roller_Kp 2.5
+#define froller_Ki 1.0
+#define broller_Ki 0.8
#define roller_Kd 0.0
//定義
@@ -60,7 +60,7 @@
DigitalOut emersig(PC_0);
//タイマー定義
Ticker get_rps;//ロリコンからRPMの読み取り
-InterruptIn but(USER_BUTTON);
+Timer shooot;
//変数定義
//データ配列
@@ -79,11 +79,13 @@
int ave_back_roller_pulse;
char front_roller_data[1];
char back_roller_data[1];
-int dmove_val;
static int j = 0;
int sonicval;
int mode = true;
-int table;
+int table[3];
+int times = 0;
+int letshoot = 0;
+int endshoot = 0;
//プロトタイプ宣言
//メイン制御関数
@@ -113,31 +115,45 @@
//メイン関数
int main()
{
- pc.baud(460800);
+ Power = 1;
emersig = 0;
- Power = 1;
i2ccheck = 0;
+ pc.baud(460800);
get_rps.attach_us(&flip, 40000);
- but.rise(shoot);
pc.printf("\x1b[0m");
+
send_data2[0] = 0x80;
+ i2c.write(0x10, send_data2, 1);
+ i2c.write(0x12, send_data2, 1);
+ i2c.write(0x14, send_data2, 1);
+ i2c.write(0x16, send_data2, 1);
i2c.write(0x18, send_data2, 1);
i2c.write(0x20, send_data2, 1);
+ i2c.write(0x40, send_data2, 1);
+
while(true) {
topsonic.start();
sonicval = topsonic.get_dist_cm();
ps3s.getdata();
ps3s.checkdata();
- if(ps3data[6] == 1){
- shoot();
- }
ctrl();
+ sendi2c();
}
}
//メイン制御関数
void ctrl()
{
+ bool dmove_val = false;
+ front_roller_data[0] = 0x80;
+ back_roller_data[0] = 0x80;
+ send_data2[0] = 0x80;
+ i2c.write(0x18, front_roller_data, 1, false);
+ i2c.write(0x20, back_roller_data, 1, false);
+ i2c.write(0x40, send_data2, 1, false);
+ for(int i = 0; i < 4; i++) {
+ send_data[i][0] = 0x80;//ショートブレーキ
+ }
if(ps3data[12]==1) {//Select
emergency();//緊急停止
@@ -145,41 +161,39 @@
outemergency();//緊急停止解除
}
+ if(ps3data[6] == 1) { //まる
+ shoot();
+ } else if(ps3data[4]==1) { //さんかく
+ semiauto();
+ } else if(ps3data[5]==1) { //ばつ
+ changemode();
+ } else if(ps3data[7]==1){//しかく
+ endshoot = 0;
+ shooot.reset();
+ times = 0;
+ }
if(emersig == 0) {
- for(int i = 0; i < 4; i++) {
- send_data[i][0] = 0x83;//ショートブレーキ
- }
if(ps3data[0]==1) {
- dmove_val += 1;//上
+ dmove_val = true;//上
} else if(ps3data[1]==1) {
- dmove_val += 2;//下
+ dmove_val = true;//下
} else if(ps3data[2]==1) {
- dmove_val += 4;//右
+ dmove_val = true;//右
} else if(ps3data[3]==1) {
- dmove_val += 8;//左
+ dmove_val = true;//左
} else if(ps3data[8]==1) {//L1
- dmove_val += 32;//左回転
+ dmove_val = true;//左回転
} else if(ps3data[10]==1) {//R1
- dmove_val += 16;//右回転
+ dmove_val = true;//右回転
}
- if (dmove_val == 1) {
+ if (dmove_val) {
dmove();//方向キー+L1R1キー
}
if(((ps3data[16] <= 51) || (ps3data[16] >= 77)) || ((ps3data[17] <= 51) || (ps3data[17] >= 77))) {
amove_R();//アナログパッド
}
-
- if(ps3data[7]==1) {//しかく
- //backfif();50cm後退
- } else if(ps3data[4]==1) { //さんかく
- semiauto();
- } else if(ps3data[5]==1) { //しかく
- changemode();
- }
- //sendi2c();
- } else {
}
}
@@ -252,124 +266,78 @@
void dmove()
{
- int k = 32;
- while(true) {
- if (dmove_val == 0) {
- break;
- } else {
- if (dmove_val - k >= 0) {
- sncled = 1;
- switch (k) {
- case 1:
- send_data[0][0] = 0xF7;
- send_data[1][0] = 0xFF;
- send_data[2][0] = 0xF7;
- send_data[3][0] = 0xFF;
- break;
- case 2:
- send_data[0][0] = 0x00;
- send_data[1][0] = 0x0A;
- send_data[2][0] = 0x00;
- send_data[3][0] = 0x0A;
- break;
- case 4:
- send_data[0][0] = 0xF5;
- send_data[3][0] = 0xFF;
- send_data[1][0] = 0x09;
- send_data[2][0] = 0x00;
- break;
- case 8:
- send_data[0][0] = 0x00;
- send_data[3][0] = 0x09;
- send_data[1][0] = 0xFF;
- send_data[2][0] = 0xF5;
- break;
- case 16:
- send_data[1][0] = 0x00;
- send_data[3][0] = 0x00;
- send_data[0][0] = 0xFF;
- send_data[2][0] = 0xFF;
- break;
- case 32:
- send_data[0][0] = 0x00;
- send_data[2][0] = 0x00;
- send_data[1][0] = 0xFF;
- send_data[3][0] = 0xFF;
- break;
- default:
- break;
- }
- sncled = 0;
- dmove_val -= k;
- } else {
- k /= 2;
- }
- }
+
+ if(ps3data[0] == 1) {
+ send_data[0][0] = 0xF7;
+ send_data[1][0] = 0xFF;
+ send_data[2][0] = 0xF7;
+ send_data[3][0] = 0xFF;
+ } else if(ps3data[1] == 1) {
+ send_data[0][0] = 0x00;
+ send_data[1][0] = 0x0A;
+ send_data[2][0] = 0x00;
+ send_data[3][0] = 0x0A;
+ } else if(ps3data[2] == 1) {
+ send_data[0][0] = 0xF5;
+ send_data[3][0] = 0xFF;
+ send_data[1][0] = 0x09;
+ send_data[2][0] = 0x00;
+ } else if(ps3data[3] == 1) {
+ send_data[0][0] = 0x00;
+ send_data[3][0] = 0x09;
+ send_data[1][0] = 0xFF;
+ send_data[2][0] = 0xF5;
+ } else if(ps3data[10] == 1) {
+ send_data[1][0] = 0x00;
+ send_data[3][0] = 0x00;
+ send_data[0][0] = 0xFF;
+ send_data[2][0] = 0xFF;
+ } else if(ps3data[8] == 1) {
+ send_data[0][0] = 0x00;
+ send_data[2][0] = 0x00;
+ send_data[1][0] = 0xFF;
+ send_data[3][0] = 0xFF;
}
+
}
//ショートブレーキ
void shortb(int i)
{
i2ccheck = 0;
- send_data[i][0] = 0x83;
+ send_data[i][0] = 0x80;
}
-/*50cm後退
-void backfif()
-{
- int snc1;
- //int snc2;
- while(true) {
- topsonic.start();
- //undersonic.start();
- snc1 = topsonic.get_dist_cm();
- //snc2 = undersonic.get_dist_cm();
- if (snc1 != 0) { // && (snc2 != 0)) {
- break;
- }
- }
-
- int snc1buffer;
- //int snc2buffer;
- while(true) {
- topsonic.start();
- //undersonic.start();
- snc1buffer = topsonic.get_dist_cm();
- //snc2buffer = undersonic.get_dist_cm();
- if (snc1buffer == 0) { // || snc2buffer == 0) {
- continue;
- }
- if(snc1buffer >= snc1 + 50) {//|| (snc2buffer >= snc2 + 50)) {
- sncled = 1;
- for(int i = 0; i < 4; i++) {
- send_data[i][0] = 0x83;
- }
- break;
- } else {
- for(int i = 0; i < 4; i++) {
- send_data[i][0] = 0x10;
- i2c.write(address[i], send_data[i], 1);
- pc.printf("Goal:%dcm,Now:%dcm\n\r", snc1 + 50, snc1buffer);
- }
- }
- }
-}*/
//I2C送信
void sendi2c()
{
+ if(send_data[0][0] < 0x80) {
+ send_data[0][0] += 50;
+ } else if(send_data[0][0] > 0x80) {
+ send_data[0][0] -= 50;
+ }
+ if(send_data[1][0] < 0x80) {
+ send_data[1][0] += 50;
+ } else if(send_data[1][0] > 0x80) {
+ send_data[1][0] -= 50;
+ }
+ if(send_data[2][0] < 0x80) {
+ send_data[2][0] += 50;
+ } else if(send_data[2][0] > 0x80) {
+ send_data[2][0] -= 50;
+ }
+ if(send_data[3][0] < 0x80) {
+ send_data[3][0] += 50;
+ } else if(send_data[3][0] > 0x80) {
+ send_data[3][0] -= 50;
+ }
err[0] = i2c.write(address[0], send_data[0], 1);
err[1] = i2c.write(address[1], send_data[1], 1);
err[2] = i2c.write(address[2], send_data[2], 1);
err[3] = i2c.write(address[3], send_data[3], 1);
if (err[0]!=0 || err[1]!=0 || err[2]!=0 || err[3]!=0) {
- while(10) {
- i2ccheck = 1;
- wait(0.1);
-
- i2ccheck = 0;
- }
+ i2ccheck = 0;
} else {
i2ccheck = 1;
}
@@ -406,7 +374,7 @@
//平均値をRPMへ変換
front_roller_rpm = ave_front_roller_pulse * 25 * 60 / 1200;
back_roller_rpm = ave_back_roller_pulse * 25 * 60 / 1200;
- pc.printf("%3d,%3d,%d\n\r",abs(front_roller_rpm), abs(back_roller_rpm),sonicval);
+ pc.printf("%3d,%3d,%d,%d\n\r",abs(front_roller_rpm), abs(back_roller_rpm),sonicval,times);
}
//ローラー
@@ -436,37 +404,55 @@
//投擲動作
void shoot()
{
- while(true) {
- roller_PID(1000,1000);
+ letshoot = 0;
+ int front = 1650;
+ int back = 600;
+ while(endshoot == 0 || times < 2) {
+ roller_PID(front, back);
i2c.write(0x18, front_roller_data, 1, false);
i2c.write(0x20, back_roller_data, 1, false);
+ 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) ) ) {
+ letshoot++;
+ } else {
+ letshoot = 0;
+ }
+ if(letshoot == 30 && endshoot == 0) {
+ send_data2[0] = 0x0F;
+ i2c.write(0x40, send_data2, 1, false);
+ endshoot = 1;
+ shooot.start();
+ }
+ if(endshoot == 1){
+ times = shooot.read();
+ }
}
+ shooot.stop();
}
-
-
//セミオートプレイテスト
void semiauto()
{
- while(true) {
- looptime = mainloop.read();
- pc.printf("%f,%d\n\r", looptime, sonicval);
- mainloop.reset();
- mainloop.start();
- send_data[0][0] = 0xE1;
- send_data[3][0] = 0xEC;
- send_data[1][0] = 0x1D;
- send_data[2][0] = 0x14;
+ while(true) {//一回目低固定
+ if(mode) {
+ send_data[0][0] = 0xE1;
+ send_data[3][0] = 0xEC;
+ send_data[1][0] = 0x1D;
+ send_data[2][0] = 0x14;
+ } else if(!mode) {
+ send_data[0][0] = 0x00;
+ send_data[3][0] = 0x09;
+ send_data[1][0] = 0xFF;
+ send_data[2][0] = 0xF5;
+ }
topsonic.start();
- wait(0.01);
sonicval = topsonic.get_dist_cm();
if(sonicval < 100) {
send_data[0][0] = 0xF7;
send_data[1][0] = 0xFF;
send_data[2][0] = 0xF7;
send_data[3][0] = 0xFF;
- if(sonicval == 75) {
+ if(sonicval <= 74) {
send_data[0][0] = 0x80;
send_data[1][0] = 0x80;
send_data[2][0] = 0x80;
@@ -474,14 +460,68 @@
}
}
sendi2c();
- if(sonicval == 75) {
- table = 0;
+ if(sonicval <= 74) {
shoot();
+ for(int i = 0; i < 100; i++) {
+ pc.printf("Press Box button if pet stood");
+ ps3s.getdata();
+ ps3s.checkdata();
+ if(ps3data[7] == 1) {
+ table[0] = true;
+ }
+ }
}
- mainloop.stop();
+ if(table[0]) {
+ break;
+ }
+ }
+
+ while(true) { //高固定
+ if(mode) {
+ send_data[0][0] = 0xE1;
+ send_data[3][0] = 0xEC;
+ send_data[1][0] = 0x1D;
+ send_data[2][0] = 0x14;
+ } else if(!mode) {
+ send_data[0][0] = 0x00;
+ send_data[3][0] = 0x09;
+ send_data[1][0] = 0xFF;
+ send_data[2][0] = 0xF5;
+ }
+ topsonic.start();
+ sonicval = topsonic.get_dist_cm();
+ if(sonicval < 150) {
+ send_data[0][0] = 0xF7;
+ send_data[1][0] = 0xFF;
+ send_data[2][0] = 0xF7;
+ send_data[3][0] = 0xFF;
+ if(sonicval <= 124) {
+ send_data[0][0] = 0x80;
+ send_data[1][0] = 0x80;
+ send_data[2][0] = 0x80;
+ send_data[3][0] = 0x80;
+ }
+ }
+ sendi2c();
+ if(sonicval <= 124) {
+ shoot();
+ for(int i = 0; i < 100; i++) {
+ pc.printf("Press Box button if pet stood");
+
+ ps3s.getdata();
+ ps3s.checkdata();
+ if(ps3data[7] == 1) {
+ table[1] = true;
+ }
+ }
+ }
+ if(table[1]) {
+ break;
+ }
}
}
+
void changemode()
{
mode = !mode;
@@ -496,8 +536,12 @@
//デバック用データ出力
void printdata()
{
- for (int i = 0; i < 7; i++) {
- pc.printf("%d,",ps3s.Rdata[i]);
- }
- pc.printf("%d\n\r",ps3s.Rdata[7]);
+ //for (int i = 0; i < 7; i++) {
+ // pc.printf("%d,",ps3s.Rdata[i]);
+ //}
+ //pc.printf("%d\n\r",ps3s.Rdata[7]);
+ pc.printf("%d,",int(send_data[0][0]));
+ pc.printf("%d,",int(send_data[0][1]));
+ pc.printf("%d.",int(send_data[0][2]));
+ pc.printf("%d\n\r",int(send_data[0][3]));
}
\ No newline at end of file