Revision 52:320f910ca6ca, committed 2017-11-23

Comitter:

number_key

Date:

Thu Nov 23 11:49:39 2017 +0900

Parent:

50:3a7c858aa0f9

Commit message:

dirty

Changed in this revision

--- a/bot/PIDcontroller/PID_controller.h	Mon Nov 13 17:33:30 2017 +0900
+++ b/bot/PIDcontroller/PID_controller.h	Thu Nov 23 11:49:39 2017 +0900
@@ -27,9 +27,9 @@
 #include "PID.h"
 #include "R1307.h"
 
-const double KC = 8.8;
-const double TI = 5.0;
-const double TD = 0.000002;
+const double KC = 9.7;
+const double TI = 4.0;
+const double TD = 0.0000022;
 const float INTERVAL  = 0.001;
 const float INPUT_LIMIT = 360.0;
 const float OUTPUT_LIMIT = 0.4;

--- a/bot/bot.cpp	Mon Nov 13 17:33:30 2017 +0900
+++ b/bot/bot.cpp	Thu Nov 23 11:49:39 2017 +0900
@@ -94,7 +94,7 @@
 
   if(!pad1.getButton2(5)) {
       axis.resetOffset();
-      debugSerial.printf("Force RESeT\n\r");
+      //debugSerial.printf("Force RESeT\n\r");
   }
 
   if((beforestick >= 0.2 && pad1.getStick(0) < 0.2) || (beforestick <= -0.2 && pad1.getStick(0) > -0.2)) {
@@ -212,8 +212,8 @@
     if (!pad2.getButton2(1)) nishijoSword.move(WIND_UP_SPEED);
     if(pad2.getButton2(0) && pad2.getButton2(1))  nishijoSword.move(0);
 
-    if (!pad2.getButton2(2)) nishijo.move(-SWORD_SPEED);
-    if (!pad2.getButton2(3)) nishijo.move(SWORD_SPEED);
+    if (!pad2.getButton2(3)) nishijo.move(-SWORD_SPEED);
+    if (!pad2.getButton2(2)) nishijo.move(SWORD_SPEED);
     if(pad2.getButton2(2) && pad2.getButton2(3))  nishijo.move(0);
   } else {
     tentacle.stop();
@@ -228,14 +228,29 @@
 
 void Bot::checkConnection()
 {
-  if(receiveSuccessed1) debugSerial.printf("ON ");
-  else  debugSerial.printf("OFF ");
+  if(receiveSuccessed1 == 1) debugSerial.printf("ON ");
+  else  debugSerial.printf("%d ",receiveSuccessed1);
 
-  if(receiveSuccessed2) debugSerial.printf("ON\r\n");
-  else  debugSerial.printf("OFF\r\n");
+  if(receiveSuccessed2 == 1) debugSerial.printf("ON\r\n");
+  else  debugSerial.printf("%d\r\n",receiveSuccessed2);
 }
 
 void Bot::checkDegree()
 {
     debugSerial.printf("%f %f\r\n",plane.getRawDegree(), plane.getCalculationResult());
 }
+
+void Bot::checkPacket()
+{
+    char buf=0;
+    while(1) {
+      buf =  pad1.getc();
+      debugSerial.printf("%d %c\r\n",buf,buf);
+    }
+}
+
+void Bot::checkReceiveData()
+{
+  debugSerial.printf("%f %f %f %f\r\n",pad1.getStick(0),pad1.getStick(1),pad1.getStick(2),pad1.getStick(3));
+  debugSerial.printf("%f %f %f %f\r\n",pad2.getStick(0),pad2.getStick(1),pad2.getStick(2),pad2.getStick(3));
+}

--- a/bot/bot.h	Mon Nov 13 17:33:30 2017 +0900
+++ b/bot/bot.h	Thu Nov 23 11:49:39 2017 +0900
@@ -56,6 +56,10 @@
 
     void checkDegree();
 
+    void checkPacket();
+
+    void checkReceiveData();
+
 private :
     Controller pad1;
     Controller pad2;
@@ -68,8 +72,8 @@
     Sword nishijo;
     PIDC plane;
     PIDC axis;
-    bool receiveSuccessed1;
-    bool receiveSuccessed2;
+    int receiveSuccessed1;
+    int receiveSuccessed2;
     float frontDegree;
     Serial debugSerial;
     Timer t;

--- a/bot/controller/controller.cpp	Mon Nov 13 17:33:30 2017 +0900
+++ b/bot/controller/controller.cpp	Thu Nov 23 11:49:39 2017 +0900
@@ -24,7 +24,7 @@
 {
 }
 
-bool Controller::receiveState()
+int Controller::receiveState()
 {
     fepTemp = FEP::read(data, DATA_SIZE);
     if(fepTemp == FEP_SUCCESS) {
@@ -42,10 +42,14 @@
             if(stick[i] < -1.0) stick[i] = -1.0;
         }
         setStick();
+    } else if(fepTemp == FEP_DT_BIN) {
+        return 2;
+    } else if(fepTemp == FEP_DT_BIN) {
+        return 3;
     } else if(fepTemp == FEP_NO_RESPONSE) {
         return 0;
     } else {
-        return 0;
+        return 4;
     }
     return 1;
 }

--- a/bot/controller/controller.h	Mon Nov 13 17:33:30 2017 +0900
+++ b/bot/controller/controller.h	Thu Nov 23 11:49:39 2017 +0900
@@ -65,7 +65,7 @@
     /**
     * @brief メンバ変数にボタンのステートを格納
     */
-    bool receiveState();
+    int receiveState();
 
     /**
      * ボタン1の状態を取得

--- a/bot/tentacle_unit/tentacle_unit.cpp	Mon Nov 13 17:33:30 2017 +0900
+++ b/bot/tentacle_unit/tentacle_unit.cpp	Thu Nov 23 11:49:39 2017 +0900
@@ -14,15 +14,15 @@
 
 void Tentacle::rightMove(float speed)
 {
-  if(speed*right.getPosition() < 0)    speed = 0;
-  if(speed*right.getPosition() > 0)    right.resetPosition();
+  /*if(speed*right.getPosition() < 0)    speed = 0;
+  if(speed*right.getPosition() > 0)    right.resetPosition();*/
 	tentacleMotor[0].setSpeed(speed);
 }
 
 void Tentacle::leftMove(float speed)
 {
-  if(speed*left.getPosition() < 0)    speed = 0;
-  if(speed*left.getPosition() > 0)    left.resetPosition();
+  /*if(speed*left.getPosition() < 0)    speed = 0;
+  if(speed*left.getPosition() > 0)    left.resetPosition();*/
   tentacleMotor[1].setSpeed(speed);
 }
 

--- a/main.cpp	Mon Nov 13 17:33:30 2017 +0900
+++ b/main.cpp	Thu Nov 23 11:49:39 2017 +0900
@@ -4,22 +4,47 @@
 
 Bot OCTOPUS;
 Serial pc(USBTX, USBRX, 115200);
+Timer t;
+Timer tc;
 
 int main()
 {
-    pc.printf("const\n\r");
+    //pc.printf("const\n\r");
+    t.reset();
+    tc.reset();
     while(1) {
-        printf("loop\n\r");
+        //printf("loop\n\r");
+        //tc.start();
+        //t.start();
+        OCTOPUS.confirmPad2();
+        //t.stop();
+        //pc.printf("%f ",t.read());
+        //t.start();
+        //wait(0.01);
         OCTOPUS.confirmPad1();
-        OCTOPUS.confirmPad2();
+        //pc.printf("\r\n");
+        //t.stop();
+        //pc.printf("%f ",t.read());
+        //t.start();
         //OCTOPUS.controllDrive();
         OCTOPUS.controllDrive2();
         // OCTOPUS.controllDrive3();
         //OCTOPUS.controllDrive4();
+        //t.stop();
+        //t.start();
         OCTOPUS.controllMech();
         //OCTOPUS.calibrate();
+        //t.stop();
+        //pc.printf("%f ",t.read());
+        //wait(fabsf(0.12-t.read()));
+        //t.reset();
+        //tc.stop();
         //OCTOPUS.checkConnection();
         //OCTOPUS.checkDegree();
-        wait(0.01);
+        //OCTOPUS.checkPacket();
+        //OCTOPUS.checkReceiveData();
+        //pc.printf("%f ",tc.read());
+        //tc.reset();
+        wait(INTERVAL);
     }
 }