Revision 26:7258d5ad0bff, committed 2017-10-25

Comitter:

number_key

Date:

Wed Oct 25 03:25:06 2017 +0900

Parent:

25:d199d621ecca

Commit message:

no message

Changed in this revision

diff -r d199d621ecca -r 7258d5ad0bff bot/PIDcontroller/PID_controller.cpp
--- a/bot/PIDcontroller/PID_controller.cpp	Sat Sep 30 17:59:10 2017 +0900
+++ b/bot/PIDcontroller/PID_controller.cpp	Wed Oct 25 03:25:06 2017 +0900
@@ -7,15 +7,13 @@
 
 PIDC::PIDC() :
     PID(KC, TI, TD, INTERVAL),
-    JY901(HMCsda, HMCscl),
-    axisOffSetDegree(0),
-    planeOffSetDegree(0),
+    HMC6352(HMCsda, HMCscl),
+    offSetDegree(0),
     turnOverNumber(0),
     beforeDegree(0),
     rawDegree(0),
     calculationResult(0),
-    axisCurrentDegree(0),
-    planeCurrentDegree(0)
+    currentDegree(0)
 {
     PID::setInputLimits(-INPUT_LIMIT, INPUT_LIMIT);
     PID::setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT);
@@ -23,24 +21,23 @@
     PID::setMode(AUTO_MODE);
     PID::setSetPoint(0.0);
 
-    rawDegree = JY901::getZaxisAngle();
+    wait(0.1);
+    HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20);
+    wait(0.1);
+    rawDegree = HMC6352::sample();
     beforeDegree = rawDegree;
-    planeOffSetDegree = rawDegree;
-    axisOffSetDegree = rawDegree;
-//    this -> attach(this, &PIDC::updateOutput, INTERVAL);
+    offSetDegree = rawDegree;
 }
 
 PIDC::PIDC(PinName sda, PinName scl, float kc, float ti, float td, float interval) :
     PID(kc, ti, td, interval),
-    JY901(sda, scl),
-    axisOffSetDegree(0),
-    planeOffSetDegree(0),
+    HMC6352(sda, scl),
+    offSetDegree(0),
     turnOverNumber(0),
     beforeDegree(0),
     rawDegree(0),
     calculationResult(0),
-    axisCurrentDegree(0),
-    planeCurrentDegree(0)
+    currentDegree(0)
 {
     PID::setInputLimits(-INPUT_LIMIT, INPUT_LIMIT);
     PID::setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT);
@@ -48,23 +45,23 @@
     PID::setMode(AUTO_MODE);
     PID::setSetPoint(0.0);
 
-    rawDegree = JY901::getZaxisAngle();
+    wait(0.1);
+    HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20);
+    wait(0.1);
+    rawDegree = HMC6352::sample();
     beforeDegree = rawDegree;
-    planeOffSetDegree = rawDegree;
-    axisOffSetDegree = rawDegree;
-//    this -> attach(this, &PIDC::updateOutput, INTERVAL);
+    offSetDegree = rawDegree;
 }
 
 
 void PIDC::confirm()
 {
-    rawDegree = JY901::getZaxisAngle();
+    rawDegree = HMC6352::sample();
     if(rawDegree - beforeDegree < -SENSED_THRESHOLD) ++turnOverNumber;
     if(rawDegree - beforeDegree > SENSED_THRESHOLD) --turnOverNumber;
-    axisCurrentDegree = rawDegree - axisOffSetDegree + turnOverNumber * 360;
-    planeCurrentDegree = rawDegree - planeOffSetDegree + turnOverNumber * 360;
-    beforeDegree = JY901::getZaxisAngle();
-    PID::setProcessValue(axisCurrentDegree);
+    currentDegree = rawDegree - offSetDegree + turnOverNumber * 3600;
+    beforeDegree = HMC6352::sample();
+    PID::setProcessValue(currentDegree / 10.0);
     calculationResult = PID::compute();
 }
 
@@ -73,31 +70,29 @@
     return calculationResult;
 }
 
-float PIDC::getCurrentDegree() const
+int PIDC::getCurrentDegree() const
 {
-    return planeCurrentDegree;
+    return currentDegree;
+}
+
+int PIDC::getRawDegree()
+{
+    return HMC6352::sample();
 }
 
-void PIDC::resetAxisOffset()
+void PIDC::setPoint(float point)
 {
-    beforeDegree = JY901::getZaxisAngle();
-    axisOffSetDegree = JY901::getZaxisAngle();
-    turnOverNumber = 0;
+    PID::setSetPoint(point);
 }
 
-void PIDC::resetPlaneOffset()
+void PIDC::resetOffset()
 {
-    beforeDegree = JY901::getZaxisAngle();
-    planeOffSetDegree = JY901::getZaxisAngle();
+    beforeDegree = HMC6352::sample();
+    offSetDegree = HMC6352::sample();
     turnOverNumber = 0;
 }
 
-void PIDC::renewAngle()
-{
-  angle = JY901::getZaxisAngle();
-}
-
 void PIDC::calibration(int mode)
 {
-
+    setCalibrationMode(mode);
 }

diff -r d199d621ecca -r 7258d5ad0bff bot/PIDcontroller/PID_controller.h
--- a/bot/PIDcontroller/PID_controller.h	Sat Sep 30 17:59:10 2017 +0900
+++ b/bot/PIDcontroller/PID_controller.h	Wed Oct 25 03:25:06 2017 +0900
@@ -1,6 +1,22 @@
 /**
 * @file PID_controller.h
 * @brief コンパスセンサを使ったPIDコントローラ
+*
+* Example :
+* @code
+* #include "mbed.h"
+* #include "PID_controller.h"
+*
+* PIDC pidc;
+*
+* int main()
+* {
+*     while(1) {
+*         pidc.confirm();
+*         pc.printf("Hi, %f\r\n", pid.getCo());
+*     }
+* }
+* @endcode
 */
 #ifndef PID_CONTROLLER_H
 #define PID_CONTROLLER_H
@@ -9,22 +25,19 @@
 #include "pin_config.h"
 
 #include "PID.h"
-#include "jy901.h"
-
-// const double M_PI = 3.141592653589793;
-const double KC = 5.2;
+#include "HMC6352.h"
+const double KC = 9.0;
 const double TI = 0.0;
-const double TD = 0.00;
+const double TD = 0.00008;
 const float INTERVAL  = 0.01;
-const float INPUT_LIMIT = 180.0;
+const float INPUT_LIMIT = 360.0;
 const float OUTPUT_LIMIT = 0.4;
 const float BIAS = 0.0;
-const int SENSED_THRESHOLD = 180;
-
+const int SENSED_THRESHOLD = 1800;
 /**
 * @brief コンパスセンサを使ったPIDコントローラ
 */
-class PIDC : public PID, JY901, Ticker
+class PIDC : public PID, HMC6352, Ticker
 {
 public :
     /**
@@ -51,12 +64,7 @@
     /**
      * @brief 回転用座標系リセット
      */
-    void resetAxisOffset();
-
-    /**
-     * 平行移動用座標系リセット
-     */
-    void resetPlaneOffset();
+    void resetOffset();
 
     /**
      * @brief PIDの計算結果を取得
@@ -68,31 +76,33 @@
      * 現在の角度を取得
      * @return 現在の角度(degree)
      */
-    float getCurrentDegree() const;
+    int getCurrentDegree() const;
+
+    /**
+     * センサの生値を取得
+     * @return コンパスセンサの生値
+     */
+    int getRawDegree();
+
+    void setPoint(float point);
 
     /**
      * @brief キャリブレーションする
      * @param mode ENTER OR EXIT
      */
-
-     void renewAngle();
-
     void calibration(int mode);
 private :
 
     void updateOutput();
 
-    float axisOffSetDegree;
-    float planeOffSetDegree;
+    int offSetDegree;
     int turnOverNumber;
-    float beforeDegree;
+    int beforeDegree;
 
 protected :
-    float rawDegree;
+    int rawDegree;
     float calculationResult;
-    float axisCurrentDegree;
-    float planeCurrentDegree;
-    float angle;
+    int currentDegree;
 };
 
 #endif//PID_CONTROLLER_H

diff -r d199d621ecca -r 7258d5ad0bff bot/bot.cpp
--- a/bot/bot.cpp	Sat Sep 30 17:59:10 2017 +0900
+++ b/bot/bot.cpp	Wed Oct 25 03:25:06 2017 +0900
@@ -1,11 +1,10 @@
 #include "bot.h"
 
 Bot::Bot() :
-    PIDC(),
     pad1(XBee1TX, XBee1RX, ADDR1),
     pad2(XBee2TX, XBee2RX, ADDR2),
     RS485(MDTX, MDRX, 38400),
-    RS485Controller(PWM2),
+    RS485Controller(PWM4),
     powerSwitch(MDstop),
     quadOmni(&RS485Controller, &RS485),
     tentacle(&RS485Controller, &RS485),
@@ -13,61 +12,72 @@
     nishijoSword(&RS485Controller, &RS485),
     receiveSuccessed1(0),
     receiveSuccessed2(0),
-    led({DebugLED1, DebugLED2, DebugLED3, DebugLED4,
-        DebugLED5, DebugLED6, DebugLED7, DebugLED8}),
-    test(PWM1),
+    frontDegree(0),
+    led({DebugLED3, DebugLED4, DebugLED5}),
     debugSerial(USBTX, USBRX, 230400)
 {
-
-    test = 0;
     quadOmni.moveXY(0, 0, 0);
 
-    powerSwitch = 1;
+    powerSwitch = true;
 
-    for(int i = 0; i < 8; i++) {
-        led[i] = 1;
+    for(int i = 0; i < 3; i++) {
+        led[i] = true;
         wait(0.1);
-        led[i] = 0;
+        led[i] = false;
     }
-    for(int i = 0; i < 8; i++) {
-        led[i] = 1;
-        wait(0.1);
-        led[i] = 0;
+}
+
+void Bot::confirmPad1()
+{
+    receiveSuccessed1 = pad1.receiveState();
+    if(!receiveSuccessed1) {
+        quadOmni.moveXY(0, 0, 0);
     }
 }
 
-void Bot::confirmAll()
+
+void Bot::confirmPad2()
 {
-    receiveSuccessed1 = pad1.receiveState();
     receiveSuccessed2 = pad2.receiveState();
-    if(!receiveSuccessed1) {
+    if(!receiveSuccessed2){
+      tentacle.stop();
+      nishijo.stop();
+      nishijoSword.stop();
+    }
+}
+
+void Bot::drive()
+{
+    if(receiveSuccessed1) {
+        quadOmni.moveXY(
+            pad1.getStick(2),
+            -pad1.getStick(3),
+            0,
+            0,
+            - pad1.getStick(0) / 3
+        );
+    } else {
         quadOmni.moveXY(0, 0, 0);
-        tentacle.stop();
     }
 }
 
 void Bot::controllDrive()
 {
-    test = !test;
     if(receiveSuccessed1) {
-        if(pad1.getNorm(0) > 0.5) {
-            PIDC::PID::setSetPoint((pad1.getRadian(0) - M_PI / 2) * (180.0 / M_PI));
-            PIDC::confirm();
-        }
-        //PIDC::confirm();
-        PIDC::renewAngle();
-        //test = !test;
+        /*if(pad1.getNorm(0) > 0.5) {
+            plane.setSetPoint((pad1.getRadian(0) - M_PI / 2) * (180.0 / M_PI));
+            //plane.confirm();
+        }*/
+        plane.confirm();
+        if(pad1.getStick(0))  plane.resetOffset();
         quadOmni.moveXY(
-            pad1.getStick(0) * 0.8,
-            -pad1.getStick(1) * 0.8,
-            pad1.getStick(2) * 0.4
-             //PIDC::calculationResult
+            pad1.getStick(2),
+            -pad1.getStick(3),
+            0,
+            0,
+            - pad1.getStick(0) / 2.0
+            - plane.getCalculationResult()
         );
-        /*quadOmni.moveCircular(
-          0.2,
-          0,
-          0
-        );*/
     } else {
         quadOmni.moveXY(0, 0, 0);
     }
@@ -75,32 +85,34 @@
 
 void Bot::controllDrive2()
 {
+    plane.confirm();
+    axis.confirm();
     float moment = 0;
     static float beforestick = pad1.getStick(2);
 
     if(!pad1.getButton2(2)) {
-        PIDC::resetPlaneOffset();
+        axis.resetOffset();
     }
 
     if((beforestick >= 0.5 && pad1.getStick(2) < 0.5) || (beforestick <= -0.5 && pad1.getStick(2) > -0.5)) {
-        PIDC::PID::setSetPoint(0.0);
-        PIDC::resetAxisOffset();
+        plane.setSetPoint(0.0);
+        plane.resetOffset();
     }
 
     if(pad1.getStick(2) > 0.5 || pad1.getStick(2) < -0.5) {
-        moment = pad1.getStick(2) / 4.0;
-        PIDC::confirm();
-    }
-    if(fabs(pad1.getStick(2)) < 0.5) {
-        PIDC::confirm();
-        moment = PIDC::calculationResult;
+        moment = pad1.getStick(2) / 2.0;
+    } else  {
+        moment = plane.getCalculationResult();
     }
 
     if(receiveSuccessed1) {
+        led[1] = !led[1];
         quadOmni.moveCircular(
-            pad1.getNorm(0) / 2.0,
-            pad1.getRadian(0) - PIDC::planeCurrentDegree / 10.0 * (M_PI / 180.0) + M_PI,
-            moment
+            pad1.getNorm(0),
+            pad1.getRadian(0) - axis.getCurrentDegree() / 10.0 * (M_PI / 180.0) + M_PI,
+            0.5,
+            0.5,
+            -moment
         );
     } else {
         quadOmni.moveXY(0, 0, 0);
@@ -108,67 +120,125 @@
     beforestick = pad1.getStick(2);
 }
 
+void Bot::controllDrive3()
+{
+    static int rollR = 0;
+    static int rollL = 0;
+    static int mode = 1;
+    if(receiveSuccessed1) {
+        if(rollR && !pad1.getButton2(2)) {
+            frontDegree += ADJUST_DEGREE;
+        }
+        rollR = pad1.getButton2(2);
+
+        if(rollL && !pad1.getButton2(0)) {
+            frontDegree -= ADJUST_DEGREE;
+        }
+        rollL = pad1.getButton2(0);
+
+        if(!pad1.getButton2(4)) {
+            mode = 1;
+        }
+
+        if(!pad1.getButton2(5)) {
+            mode = 2;
+        }
+
+        if(mode == 1) {
+            plane.setPoint(frontDegree);
+            plane.confirm();
+
+            quadOmni.moveXY(
+                pad1.getStick(0),
+                -pad1.getStick(1),
+                0.5,
+                0.5,
+                -plane.getCalculationResult()
+            );
+        }
+        if(mode == 2) {
+            plane.setPoint(90.0 + frontDegree);
+            plane.confirm();
+
+            quadOmni.moveXY(
+                -pad1.getStick(1),
+                -pad1.getStick(0),
+                0.5,
+                0.5,
+                -plane.getCalculationResult()
+            );
+        }
+    } else {
+        quadOmni.moveXY(0, 0, 0);
+    }
+}
+
 void Bot::controllMech()
 {
-    //nishijoSword.move(1.0);
+
     if(receiveSuccessed2) {
-        /*if(!pad1.getButton2(2)) tentacle.rightMove(TENTACLE_SPEED);
-        if(!pad1.getButton2(3)) tentacle.rightMove(-TENTACLE_SPEED);
-        if(pad1.getButton2(2) && pad1.getButton2(3))  tentacle.rightMove(0);
 
-        if(!pad1.getButton2(0)) tentacle.leftMove(TENTACLE_SPEED);
-        if(!pad1.getButton2(1)) tentacle.leftMove(-TENTACLE_SPEED);
-        if(pad1.getButton2(0) && pad1.getButton2(1))  tentacle.leftMove(0);*/
-
-        if(pad2.getStick(1) > 0){
-            tentacle.leftMove(pad2.getStick(1));
-        } else {
-            tentacle.leftMove(-TENTACLE_SPEED);
-        }
+        tentacle.leftMove(pad2.getStick(3));
+        tentacle.rightMove(pad2.getStick(1));
 
-        if(pad2.getStick(3) > 0){
-            tentacle.leftMove(pad2.getStick(3));
-        } else {
-            tentacle.leftMove(-TENTACLE_SPEED);
-        }
+        if (!pad2.getButton2(0)) nishijoSword.move(-WIND_UP_SPEED);
+        if (!pad2.getButton2(1)) nishijoSword.move(WIND_UP_SPEED);
+        if(pad2.getButton2(0) && pad2.getButton2(1))  nishijoSword.move(0);
 
-        if (!pad1.getButton2(3)) nishijoSword.move(WIND_UP_SPEED);
-        if (!pad1.getButton2(5)) nishijoSword.move(-WIND_UP_SPEED);
-        if(pad2.getButton2(3) && pad2.getButton2(5))  nishijoSword.move(0);
+        if (!pad2.getButton2(2)) nishijo.move(-SWORD_SPEED);
+        if (!pad2.getButton2(3)) nishijo.move(SWORD_SPEED);
+        if(pad2.getButton2(2) && pad2.getButton2(3))  nishijo.move(0);
 
-        //test = !pad1.getButton1(5);
-
-        //if(!pad1.getButton1(6)) powerSwitch = 0;
-        //if(!pad1.getButton1(5)&&!pad1.getButton1(4)) powerSwitch = 1;
-
-        //else powerSwitch = 1;
     } else {
       tentacle.stop();
       nishijoSword.stop();
-      //test =0;
-      //powerSwitch = 0;
+      nishijo.stop();
     }
-    /*if(receiveSuccessed2){
-
-    }*/
 }
 
 
 void Bot::calibrate()
 {
-
+    if(receiveSuccessed1 &&
+        !pad1.getButton2(0) &&
+        !pad1.getButton2(1) &&
+        !pad1.getButton2(2) &&
+        !pad1.getButton2(3)
+      ) {
+        t.start();
+        t.reset();
+        plane.calibration(HMC6352_ENTER_CALIB);
+        while(t.read() < 5.0) {
+            quadOmni.moveXY(0, 0, 0.4);
+            nishijo.stop();
+            nishijoSword.stop();
+            tentacle.stop();
+        }
+        t.stop();
+        quadOmni.moveXY(0, 0, 0);
+        plane.calibration(HMC6352_EXIT_CALIB);
+    }
 }
 
-void Bot::debugPrint()
+
+void Bot::checkConnection()
 {
   if(receiveSuccessed1) {
-    debugSerial.printf("%f %f %f %f \r\n",pad1.getStick(0),pad1.getStick(1),pad1.getStick(2),pad1.getStick(3));
+    debugSerial.printf("ON ");
   } else {
-    debugSerial.printf("\r\n");
+    debugSerial.printf("OFF ");
   }
   if(receiveSuccessed2) {
-    debugSerial.printf("%f %f %f %f \r\n",pad2.getStick(0),pad2.getStick(1),pad2.getStick(2),pad2.getStick(3));
+    debugSerial.printf("ON\r\n");
+  } else {
+    debugSerial.printf("OFF\r\n");
+  }
+}
 
+void Bot::debugCompass()
+{
+  if(receiveSuccessed1) {
+    debugSerial.printf("%f %f %f\r\n",plane.getRawDegree()/10.0 ,-plane.getCalculationResult(),-pad1.getStick(0));
   } else {
     debugSerial.printf("\r\n");
   }

diff -r d199d621ecca -r 7258d5ad0bff bot/bot.h
--- a/bot/bot.h	Sat Sep 30 17:59:10 2017 +0900
+++ b/bot/bot.h	Wed Oct 25 03:25:06 2017 +0900
@@ -15,11 +15,12 @@
 #include "elevator.h"
 #include "def.h"
 
+const float ADJUST_DEGREE = 5.0;
 
 /**
 * @brief ロボットのクラス
 */
-class Bot : public PIDC
+class Bot
 {
 public :
     /**
@@ -30,22 +31,30 @@
     /**
     * @brief センサなどの値を更新
     */
-    void confirmAll();
+    void confirmPad1();
+    void confirmPad2();
 
     /**
     * @brief 足回りの制御
     */
+    void drive();
     void controllDrive();
     void controllDrive2();
+    void controllDrive3();
 
     /**
     * @brief 機構部の制御
     */
     void controllMech();
 
+    /**
+     * センサのキャリブレーション
+     */
     void calibrate();
 
-    void debugPrint();
+    void checkConnection();
+
+    void debugCompass();
 
 private :
     Controller pad1;
@@ -57,11 +66,14 @@
     Tentacle tentacle;
     Sword nishijo;
     Elevator nishijoSword;
+    PIDC plane;
+    PIDC axis;
     bool receiveSuccessed1;
     bool receiveSuccessed2;
-    DigitalOut led[8];
+    float frontDegree;
+    DigitalOut led[3];
     Serial debugSerial;
-    DigitalOut test;
+    Timer t;
 };
 
 #endif//BOT_H

diff -r d199d621ecca -r 7258d5ad0bff bot/elevator/elevator.cpp
--- a/bot/elevator/elevator.cpp	Sat Sep 30 17:59:10 2017 +0900
+++ b/bot/elevator/elevator.cpp	Wed Oct 25 03:25:06 2017 +0900
@@ -1,15 +1,17 @@
 #include "elevator.h"
 
 Elevator::Elevator(DigitalOut* RS485Controller, Serial* RS485) :
-  elevatorMotor(RS485Controller, 1, 2, SM, RS485)
+  elevatorMotor(RS485Controller, 1, 2, SM, RS485),
+  limit(PWM2, PWM1),
+  debugSerial(USBTX, USBRX, 115200)
 {
 	elevatorMotor.braking = true;
 }
 
 void Elevator::move(float speed)
 {
-  //if(speed*right.getPosition() < 0)    speed = 0;
-  //if(speed*right.getPosition() > 0)    right.resetPosition();
+  if(speed*limit.getPosition() < 0)    speed = 0;
+  if(speed*limit.getPosition() > 0)    limit.resetPosition();
 	elevatorMotor.setSpeed(speed);
 }
 
@@ -17,3 +19,8 @@
 {
   elevatorMotor.setSpeed(0);
 }
+
+void Elevator::debugPrint()
+{
+  debugSerial.printf("elevator:%d\r\f",limit.getPosition());
+}

diff -r d199d621ecca -r 7258d5ad0bff bot/elevator/elevator.h
--- a/bot/elevator/elevator.h	Sat Sep 30 17:59:10 2017 +0900
+++ b/bot/elevator/elevator.h	Wed Oct 25 03:25:06 2017 +0900
@@ -13,10 +13,12 @@
 
 	void move(float speed);
 	void stop();
+	void debugPrint();
 private:
 
 	ikarashiMDC elevatorMotor;
-	//Limit ;
+	Limit limit;
+	Serial debugSerial;
 };
 
 #endif

diff -r d199d621ecca -r 7258d5ad0bff bot/limitSwitch/limitSwitch.cpp
--- a/bot/limitSwitch/limitSwitch.cpp	Sat Sep 30 17:59:10 2017 +0900
+++ b/bot/limitSwitch/limitSwitch.cpp	Wed Oct 25 03:25:06 2017 +0900
@@ -10,12 +10,12 @@
 
 void Limit::frontLimit()
 {
-    position = 1;
+    position = -1;
 }
 
 void Limit::backLimit()
 {
-    position = -1;
+    position = 1;
 }
 
 void Limit::resetPosition()

diff -r d199d621ecca -r 7258d5ad0bff bot/limitSwitch/limitSwitch.h
--- a/bot/limitSwitch/limitSwitch.h	Sat Sep 30 17:59:10 2017 +0900
+++ b/bot/limitSwitch/limitSwitch.h	Wed Oct 25 03:25:06 2017 +0900
@@ -1,7 +1,3 @@
-/**
-* @file arm_unit.h
-* @brief 触手機構のStateクラス
-*/
 #ifndef LIMITSWITCH_H
 #define LIMITSWITCH_H
 
@@ -9,15 +5,10 @@
 #include "pin_config.h"
 
 
-/**
-* @brief 触手機構のStateクラス
-*/
+
 class Limit {
 public :
 
-    /**
-    * @brief コンストラクタ
-    */
     Limit(PinName limitSwitch1, PinName limitSwitch2);
 
 
@@ -37,4 +28,4 @@
     int position;
 };
 
-#endif//ARM_UNIT_H
+#endif

diff -r d199d621ecca -r 7258d5ad0bff bot/sword_unit/sword_unit.cpp
--- a/bot/sword_unit/sword_unit.cpp	Sat Sep 30 17:59:10 2017 +0900
+++ b/bot/sword_unit/sword_unit.cpp	Wed Oct 25 03:25:06 2017 +0900
@@ -8,8 +8,6 @@
 
 void Sword::move(float speed)
 {
-  //if(speed*right.getPosition() < 0)    speed = 0;
-  //if(speed*right.getPosition() > 0)    right.resetPosition();
 	swordMotor.setSpeed(speed);
 }
 

diff -r d199d621ecca -r 7258d5ad0bff bot/sword_unit/sword_unit.h
--- a/bot/sword_unit/sword_unit.h	Sat Sep 30 17:59:10 2017 +0900
+++ b/bot/sword_unit/sword_unit.h	Wed Oct 25 03:25:06 2017 +0900
@@ -16,7 +16,6 @@
 private:
 
 	ikarashiMDC swordMotor;
-	//Limit ;
 };
 
 #endif

diff -r d199d621ecca -r 7258d5ad0bff bot/tentacle_unit/tentacle_unit.cpp
--- a/bot/tentacle_unit/tentacle_unit.cpp	Sat Sep 30 17:59:10 2017 +0900
+++ b/bot/tentacle_unit/tentacle_unit.cpp	Wed Oct 25 03:25:06 2017 +0900
@@ -2,10 +2,11 @@
 
 Tentacle::Tentacle(DigitalOut* RS485Controller, Serial* RS485) :
 	tentacleMotor({
-    {RS485Controller, 1, 0, SM, RS485},
+    {RS485Controller, 1, 3, SM, RS485},
     {RS485Controller, 1, 1, SM, RS485},
   }),
-  right(Sensor3pin3a, Sensor3pin3b), left(Sensor4pin2a, Sensor4pin2b)
+  right(PWM3, PWM6),left(PWM8,PWM10),
+	debugSerial(USBTX, USBRX, 115200)
 {
 	tentacleMotor[0].braking = true;
   tentacleMotor[1].braking = true;
@@ -30,3 +31,8 @@
   tentacleMotor[0].setSpeed(0);
   tentacleMotor[1].setSpeed(0);
 }
+
+void Tentacle::debugPrint()
+{
+	debugSerial.printf("right:%d left:%d\r\n",right.getPosition(),left.getPosition());
+}

diff -r d199d621ecca -r 7258d5ad0bff bot/tentacle_unit/tentacle_unit.h
--- a/bot/tentacle_unit/tentacle_unit.h	Sat Sep 30 17:59:10 2017 +0900
+++ b/bot/tentacle_unit/tentacle_unit.h	Wed Oct 25 03:25:06 2017 +0900
@@ -14,11 +14,13 @@
 	void rightMove(float speed);
 	void leftMove(float speed);
 	void stop();
+	void debugPrint();
 private:
 
 	ikarashiMDC tentacleMotor[2];
 	Limit right;
 	Limit left;
+	Serial debugSerial;
 };
 
 #endif

diff -r d199d621ecca -r 7258d5ad0bff bot/wheel_unit/wheel_unit.cpp
--- a/bot/wheel_unit/wheel_unit.cpp	Sat Sep 30 17:59:10 2017 +0900
+++ b/bot/wheel_unit/wheel_unit.cpp	Wed Oct 25 03:25:06 2017 +0900
@@ -1,16 +1,18 @@
 #include "wheel_unit.h"
 
 WheelUnit::WheelUnit(DigitalOut* RS485Controller, Serial* RS485) :
-	Omni(4),
+	omni(4),
 	wheels({
-		{RS485Controller, 0, 1, SM, RS485},
-		{RS485Controller, 0, 2, SM, RS485},
-		{RS485Controller, 0, 3, SM, RS485},
-		{RS485Controller, 0, 0, SM, RS485}
+		ikarashiMDC({RS485Controller, 0, 1, SM, RS485}),
+		ikarashiMDC({RS485Controller, 0, 2, SM, RS485}),
+		ikarashiMDC({RS485Controller, 0, 3, SM, RS485}),
+		ikarashiMDC({RS485Controller, 0, 0, SM, RS485})
 	})
 {
-	Omni::setWheelRadian(PI / 4.0, 3 * PI / 4.0, 5 * PI / 4.0, 7 * PI / 4.0);
-	//Omni::setWheelRadian(PI / 4.0, PI / 4.0, PI / 4.0, PI / 4.0);
+	omni.wheel[0].setRadian(PI / 4.0 * 5.0);
+	omni.wheel[1].setRadian(PI / 4.0 * 7.0);
+	omni.wheel[2].setRadian(PI / 4.0 * 1.0);
+	omni.wheel[3].setRadian(PI / 4.0 * 3.0);
 	for(int i = 0; i < 4; i++) {
 		wheels[i].braking = true;
 	}
@@ -18,16 +20,32 @@
 
 void WheelUnit::moveXY(float X, float Y, float moment)
 {
-	Omni::computeXY(X, Y, moment);
+	omni.computeXY(X, Y, moment);
 	for(int i = 0; i < 4; i++) {
-		wheels[i].setSpeed(Omni::getOutput(i));
+		wheels[i].setSpeed(omni.wheel[i]);
+	}
+}
+
+void WheelUnit::moveXY(float X, float Y, float gX, float gY, float moment)
+{
+	omni.computeXY(X, Y, gX, gY, moment);
+	for(int i = 0; i < 4; i++) {
+		wheels[i].setSpeed(omni.wheel[i]);
 	}
 }
 
 void WheelUnit::moveCircular(float r, float radian, float moment)
 {
-	Omni::computeCircular(r, radian, moment);
+	omni.computeCircular(r, radian, moment);
 	for(int i = 0; i < 4; i++) {
-		wheels[i].setSpeed(Omni::getOutput(i));
+		wheels[i].setSpeed(omni.wheel[i]);
 	}
 }
+
+void WheelUnit::moveCircular(float r, float radian, float gX, float gY, float moment)
+{
+	omni.computeCircular(r, radian, gX, gY, moment);
+	for(int i = 0; i < 4; i++) {
+		wheels[i].setSpeed(omni.wheel[i]);
+	}
+}

diff -r d199d621ecca -r 7258d5ad0bff bot/wheel_unit/wheel_unit.h
--- a/bot/wheel_unit/wheel_unit.h	Sat Sep 30 17:59:10 2017 +0900
+++ b/bot/wheel_unit/wheel_unit.h	Wed Oct 25 03:25:06 2017 +0900
@@ -3,19 +3,21 @@
 
 #include "mbed.h"
 #include "pin_config.h"
-#include "omni.h"
+#include "omni_wheel.h"
 #include "ikarashiMDC.h"
 
 const int RS485_BAUD = 38400;
 
-class WheelUnit : Omni {
+class WheelUnit {
 public:
 	WheelUnit(DigitalOut* RS485Controller, Serial* RS485);
 	void moveXY(float X, float Y, float moment);
+	void moveXY(float X, float Y, float gX, float gY, float moment);
 	void moveCircular(float r, float radian, float moment);
+	void moveCircular(float r, float radian, float gX, float gY, float moment);
 
 private:
-	//ikarashiMDC wheels[4];
+	OmniWheel omni;
 	ikarashiMDC wheels[4];
 protected:
 };

diff -r d199d621ecca -r 7258d5ad0bff def.h
--- a/def.h	Sat Sep 30 17:59:10 2017 +0900
+++ b/def.h	Wed Oct 25 03:25:06 2017 +0900
@@ -3,6 +3,6 @@
 
 #define SWORD_SPEED 1.0
 #define TENTACLE_SPEED 0.2
-#define WIND_UP_SPEED 0.3
+#define WIND_UP_SPEED 1.0
 
 #endif

diff -r d199d621ecca -r 7258d5ad0bff libs/HMC6352.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/libs/HMC6352.lib	Wed Oct 25 03:25:06 2017 +0900
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/aberk/code/HMC6352/#83c0cb554099

diff -r d199d621ecca -r 7258d5ad0bff libs/omni.lib
--- a/libs/omni.lib	Sat Sep 30 17:59:10 2017 +0900
+++ b/libs/omni.lib	Wed Oct 25 03:25:06 2017 +0900
@@ -1,1 +1,1 @@
-https://developer.mbed.org/teams/NHK-Robocon2016_Nagaoka_B_Team/code/omni/#4f0b55344c73
+https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/omni/#4f0b55344c73

diff -r d199d621ecca -r 7258d5ad0bff libs/omni_wheel.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/libs/omni_wheel.lib	Wed Oct 25 03:25:06 2017 +0900
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/omni_wheel/#9d972b2dad0f

diff -r d199d621ecca -r 7258d5ad0bff main.cpp
--- a/main.cpp	Sat Sep 30 17:59:10 2017 +0900
+++ b/main.cpp	Wed Oct 25 03:25:06 2017 +0900
@@ -9,10 +9,16 @@
 {
   //printf("start\r\n");
     while(1) {
-        OCTOPUS.confirmAll();
+        OCTOPUS.confirmPad1();
+        OCTOPUS.confirmPad2();
+        //OCTOPUS.drive();
         OCTOPUS.controllDrive();
+        //OCTOPUS.controllDrive2();
+        //OCTOPUS.controllDrive3();
         OCTOPUS.controllMech();
-        OCTOPUS.debugPrint();
-        //wait(0.05);
+        //OCTOPUS.checkConnection();
+        //OCTOPUS.debugCompass();
+        OCTOPUS.calibrate();
+        wait(INTERVAL);
     }
 }

diff -r d199d621ecca -r 7258d5ad0bff mbed-os.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-os.lib	Wed Oct 25 03:25:06 2017 +0900
@@ -0,0 +1,1 @@
+https://github.com/ARMmbed/mbed-os/#316c875136175ce15b5e745d47ad4b9d81c9c50b