Diff: main.cpp

Revision:

5:79cc2a126504

Parent:

4:58a2afff53a4

Child:

6:eaeaaf374263

--- a/main.cpp	Mon Oct 01 08:21:32 2018 +0000
+++ b/main.cpp	Tue Oct 02 13:17:01 2018 +0000
@@ -18,9 +18,9 @@
 #define ps3data ps3s.condata
 #define PI 3.14159265359
 #define roller_Kp 1.5
-#define froller_Ki 0.1
-#define broller_Ki 0.1
-#define roller_Kd 0.05
+#define froller_Ki 0.5
+#define broller_Ki 0.5
+#define roller_Kd 0.0
 
 //定義
 
@@ -60,9 +60,6 @@
 DigitalOut emersig(PC_0);
 //タイマー定義
 Ticker get_rps;//ロリコンからRPMの読み取り
-Timer shootpet;
-Timer mainloop;
-
 InterruptIn but(USER_BUTTON);
 
 //変数定義
@@ -87,8 +84,6 @@
 int sonicval;
 int mode = true;
 int table;
-double looptime;
-float timer1 = 0;
 
 //プロトタイプ宣言
 //メイン制御関数
@@ -125,21 +120,18 @@
     get_rps.attach_us(&flip, 40000);
     but.rise(shoot);
     pc.printf("\x1b[0m");
+    send_data2[0] = 0x80;
+    i2c.write(0x18, send_data2, 1);
+    i2c.write(0x20, send_data2, 1);
     while(true) {
-        looptime = mainloop.read();
-        mainloop.reset();
-        mainloop.start();
-        //roller_PID(777,655);
-        //i2c.write(0x18, front_roller_data, 1, false);
-        //i2c.write(0x20, back_roller_data,  1, false);
         topsonic.start();
-        wait(0.01);
         sonicval = topsonic.get_dist_cm();
         ps3s.getdata();
         ps3s.checkdata();
+        if(ps3data[6] == 1){
+            shoot();
+        }
         ctrl();
-        //printdata();
-        mainloop.stop();
     }
 }
 
@@ -147,9 +139,9 @@
 void ctrl()
 {
 
-    if(ps3data[12]!=0) {//Select
+    if(ps3data[12]==1) {//Select
         emergency();//緊急停止
-    } else if(ps3data[13]!=0) {//Start
+    } else if(ps3data[13]==1) {//Start
         outemergency();//緊急停止解除
     }
 
@@ -158,20 +150,20 @@
             send_data[i][0] = 0x83;//ショートブレーキ
         }
 
-        if(ps3data[0]!=0) {
+        if(ps3data[0]==1) {
             dmove_val += 1;//上
-        } else if(ps3data[1]!=0) {
+        } else if(ps3data[1]==1) {
             dmove_val += 2;//下
-        } else if(ps3data[2]!=0) {
+        } else if(ps3data[2]==1) {
             dmove_val += 4;//右
-        } else if(ps3data[3]!=0) {
+        } else if(ps3data[3]==1) {
             dmove_val += 8;//左
-        } else if(ps3data[8]!=0) {//L1
+        } else if(ps3data[8]==1) {//L1
             dmove_val += 32;//左回転
-        } else if(ps3data[10]!=0) {//R1
+        } else if(ps3data[10]==1) {//R1
             dmove_val += 16;//右回転
         }
-        if (dmove_val != 0) {
+        if (dmove_val == 1) {
             dmove();//方向キー+L1R1キー
         }
 
@@ -179,16 +171,14 @@
             amove_R();//アナログパッド
         }
 
-        if(ps3data[7]!=0) {//しかく
+        if(ps3data[7]==1) {//しかく
             //backfif();50cm後退
-        } else if(ps3data[6]!=0) {//まる
-            shoot();//投擲
-        } else if(ps3data[4]!=0) { //さんかく
+        } else if(ps3data[4]==1) { //さんかく
             semiauto();
-        } else if(ps3data[5]!=0) { //しかく
+        } else if(ps3data[5]==1) { //しかく
             changemode();
         }
-        sendi2c();
+        //sendi2c();
     } else {
     }
 }
@@ -416,7 +406,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,%f\n\r",abs(front_roller_rpm), abs(back_roller_rpm), sonicval);
+    pc.printf("%3d,%3d,%d\n\r",abs(front_roller_rpm), abs(back_roller_rpm),sonicval);
 }
 
 //ローラー
@@ -446,20 +436,11 @@
 //投擲動作
 void shoot()
 {
-    shootpet.start();
     while(true) {
-        timer1 = shootpet.read();
-        if(timer1 > 10) {
-            break;
-        }
         roller_PID(1000,1000);
-        //i2c.write(0x18, front_roller_data, 1, false);
-        i2c.write(0x20, back_roller_data,  1, false);
+        i2c.write(0x18, front_roller_data, 1, false);
+        i2c.write(0x20, back_roller_data, 1, false);
     }
-    back_roller_data[0] = 0x80;
-    i2c.write(0x20, back_roller_data,1,false);
-    shootpet.stop();
-    shootpet.reset();
 }