Revision 19:3a62cbc6fee9, committed 2017-09-04

Comitter:

UCHITAKE

Date:

Mon Sep 04 01:01:13 2017 +0000

Parent:

18:41f7dd1a5ed1

Child:

20:1c52f8719445

Commit message:

add calibration

Changed in this revision

--- a/2017NHKpin_config.lib	Thu Aug 31 10:34:55 2017 +0900
+++ b/2017NHKpin_config.lib	Mon Sep 04 01:01:13 2017 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/teams/NHK-Robocon2016_Nagaoka_B_Team/code/2017NHKpin_config/#8775d5a808a6
+https://developer.mbed.org/teams/NHK-Robocon2016_Nagaoka_B_Team/code/2017NHKpin_config/#ee450f68d539

--- a/bot/PIDcontroller/PID_controller.cpp	Thu Aug 31 10:34:55 2017 +0900
+++ b/bot/PIDcontroller/PID_controller.cpp	Mon Sep 04 01:01:13 2017 +0000
@@ -23,22 +23,22 @@
 }
 
 PIDC::PIDC(PinName sda, PinName scl, float kc, float ti, float td, float interval) :
-PID(kc, ti, td, interval), HMC6352(sda, scl)
-{
-    PID::setInputLimits(-INPUT_LIMIT, INPUT_LIMIT);
-    PID::setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT);
-    PID::setBias(BIAS);
-    PID::setMode(AUTO_MODE);
-    PID::setSetPoint(0.0);
-
-    HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20);
-    rawDegree = HMC6352::sample();
-    beforeDegree = HMC6352::sample();
-    offSetDegree = HMC6352::sample();
-    initDegree = 0;
-    turnOverNumber = 0;
-//    this -> attach(this, &PIDC::updateOutput, INTERVAL);
-}
+PID(kc, ti, td, interval), HMC6352(sda, scl)
+{
+    PID::setInputLimits(-INPUT_LIMIT, INPUT_LIMIT);
+    PID::setOutputLimits(-OUTPUT_LIMIT, OUTPUT_LIMIT);
+    PID::setBias(BIAS);
+    PID::setMode(AUTO_MODE);
+    PID::setSetPoint(0.0);
+
+    HMC6352::setOpMode(HMC6352_CONTINUOUS, 1, 20);
+    rawDegree = HMC6352::sample();
+    beforeDegree = HMC6352::sample();
+    offSetDegree = HMC6352::sample();
+    initDegree = 0;
+    turnOverNumber = 0;
+//    this -> attach(this, &PIDC::updateOutput, INTERVAL);
+}
 
 
 void PIDC::confirm()
@@ -56,3 +56,9 @@
 {
     return co;
 }
+
+
+void PIDC::calibration(int mode)
+{
+    setCalibrationMode(mode);
+}

--- a/bot/PIDcontroller/PID_controller.h	Thu Aug 31 10:34:55 2017 +0900
+++ b/bot/PIDcontroller/PID_controller.h	Mon Sep 04 01:01:13 2017 +0000
@@ -12,12 +12,12 @@
 #include "HMC6352.h"
 
 #define M_PI 3.141592653589793
-#define KC 1.0
+#define KC 5.0
 #define TI 0.0
 #define TD 0.0
 #define INTERVAL  0.05
 #define INPUT_LIMIT 180
-#define OUTPUT_LIMIT 1.0
+#define OUTPUT_LIMIT 0.4
 #define BIAS 0.0
 
 /**
@@ -44,6 +44,7 @@
 
     void confirm();
     float getCo();
+    void calibration(int mode);
 private :
 
     void updateOutput();

--- a/bot/bot.cpp	Thu Aug 31 10:34:55 2017 +0900
+++ b/bot/bot.cpp	Mon Sep 04 01:01:13 2017 +0000
@@ -1,41 +1,68 @@
 #include "bot.h"
 
 Bot::Bot() :
-PIDC(), pad(XBee1TX, XBee1RX, ADDR), motor(MDSDA, MDSCL, solenoidPin)
+    PIDC(), pad(XBee1TX, XBee1RX, ADDR), motor(MDSDA, MDSCL, solenoidPin)
 {
+    motor.goXY(0, 0, 0);
+    motor.moveSlider(0);
+    motor.destroy(0);
+    motor.swing(0);
+    motor.shakeHead(0);
 }
 
 void Bot::confirmAll()
 {
-    pad.receiveState();
+    suc = pad.receiveState();
     PIDC::confirm();
-    if(pad.getNorm(1) > 0.5) PIDC::setSetPoint(pad.getRadian(1) * (180.0 / M_PI));
+    if(pad.getNorm(1) > 0.5) PIDC::setSetPoint(pad.getRadian(1) * (180.0 / M_PI) - M_PI / 2.0);
+    if(!suc) {
+        motor.goXY(0, 0, 0);
+        motor.moveSlider(0);
+        motor.destroy(0);
+        motor.swing(0);
+        motor.shakeHead(0);
+    }
 }
 
 void Bot::controllDrive()
 {
-    motor.goXY(pad.getStick(0),pad.getStick(1), PIDC::co);
+    if(suc) motor.goXY(pad.getStick(0) / 2.0,-pad.getStick(1) / 2.0, PIDC::co);
 }
 
 void Bot::controllMech()
 {
-    if(!pad.getButton1(0)) motor.moveSlider(ARM_MAX_SPEED);
-    if(!pad.getButton1(1)) motor.moveSlider(-ARM_MAX_SPEED);
-    if(pad.getButton1(0) && pad.getButton1(1)) motor.moveSlider(0);
-
-    if(!pad.getButton1(3)) motor.shakeHead(ARM_MAX_SPEED);
-    if(!pad.getButton1(4)) motor.shakeHead(-ARM_MAX_SPEED);
-    if(pad.getButton1(3) && pad.getButton1(4)) motor.shakeHead(0);
+    if(suc) {
+//        if(!pad.getButton1(0)) motor.moveSlider(ARM_MAX_SPEED);
+//        if(!pad.getButton1(1)) motor.moveSlider(-ARM_MAX_SPEED);
+//        if(pad.getButton1(0) && pad.getButton1(1)) motor.moveSlider(0);
+//
+//        if(!pad.getButton1(3)) motor.shakeHead(ARM_MAX_SPEED);
+//        if(!pad.getButton1(4)) motor.shakeHead(-ARM_MAX_SPEED);
+//        if(pad.getButton1(3) && pad.getButton1(4)) motor.shakeHead(0);
+//
+        if(!pad.getButton1(5)) motor.swing(0.2);
+        if(!pad.getButton1(6)) motor.swing(-0.2);
+        if(pad.getButton1(5) && pad.getButton1(6)) motor.swing(0);
 
-    if(!pad.getButton1(5)) motor.swing(ARM_MAX_SPEED);
-    if(!pad.getButton1(6)) motor.swing(-ARM_MAX_SPEED);
-    if(pad.getButton1(5) && pad.getButton1(6)) motor.swing(0);
+//
+//        if(!pad.getButton1(2)) {
+//            motor.destroy(DESTROY_MAX_SPEED);
+//        } else {
+//            motor.destroy(0);
+//        }
+//
+//        if(!pad.getButton2(0)) motor.release();
+    }
+}
 
-    if(!pad.getButton1(2)) {
-        motor.destroy(DESTROY_MAX_SPEED);
-    } else {
-        motor.destroy(0);
+
+void Bot::calibrate()
+{
+    if(suc && !pad.getButton2(0) && !pad.getButton2(1) && !pad.getButton2(2) && !pad.getButton2(3)) {
+        PIDC::calibration(HMC6352_ENTER_CALIB);
+        motor.goXY(0, 0, 0.3);
+        wait(2.0);
+        motor.goXY(0, 0, 0);
+        PIDC::calibration(HMC6352_EXIT_CALIB);
     }
-
-    if(!pad.getButton2(0)) motor.release();
-}
+}
\ No newline at end of file

--- a/bot/bot.h	Thu Aug 31 10:34:55 2017 +0900
+++ b/bot/bot.h	Mon Sep 04 01:01:13 2017 +0000
@@ -39,10 +39,13 @@
     * @brief 機構部の制御
     */
     void controllMech();
+    
+    void calibrate();
 
 private :
     Controller pad;
     MotorDriver motor;
+    bool suc;
 };
 
 #endif//BOT_H

--- a/bot/controller/controller.cpp	Thu Aug 31 10:34:55 2017 +0900
+++ b/bot/controller/controller.cpp	Mon Sep 04 01:01:13 2017 +0000
@@ -23,7 +23,7 @@
     fepTemp = 0;
 }
 
-void Controller::receiveState()
+bool Controller::receiveState()
 {
     fepTemp = FEP::read(data, DATA_SIZE);
     if(fepTemp == FEP_SUCCESS) {
@@ -40,8 +40,11 @@
         }
         setStick();
     } else if(fepTemp == FEP_NO_RESPONSE) {
+        return 0;
     } else {
+        return 0;
     }
+    return 1;
 }
 
 void Controller::setStick()

--- a/bot/controller/controller.h	Thu Aug 31 10:34:55 2017 +0900
+++ b/bot/controller/controller.h	Mon Sep 04 01:01:13 2017 +0000
@@ -42,7 +42,7 @@
     /**
     * @brief メンバ変数にボタンのステートを格納
     */
-    void receiveState();
+    bool receiveState();
 
     /**
      * ボタン1の状態を取得

--- a/bot/motor_driver/arm_unit/arm_unit.cpp	Thu Aug 31 10:34:55 2017 +0900
+++ b/bot/motor_driver/arm_unit/arm_unit.cpp	Mon Sep 04 01:01:13 2017 +0000
@@ -1,5 +1,11 @@
 #include "arm_unit.h"
 
+void Arm::resetHeight(void)
+{
+    QEI::reset();
+    heightResetFlag = 1;
+}
+
 Arm::Arm() : QEI(Sensor4pin1a, Sensor4pin1b, NC, PULSES_PER_REV, X2_ENCODING),
              limitSwitch(Sensor3pin3a)
 {
@@ -19,8 +25,7 @@
     return heightResetFlag;
 }
 
-void Arm::resetHeight(void)
+int Arm::getHeight()
 {
-    QEI::reset();
-    heightResetFlag = 1;
-}
+    return QEI::getCurrentState();
+}
\ No newline at end of file

--- a/bot/motor_driver/arm_unit/arm_unit.h	Thu Aug 31 10:34:55 2017 +0900
+++ b/bot/motor_driver/arm_unit/arm_unit.h	Mon Sep 04 01:01:13 2017 +0000
@@ -35,6 +35,12 @@
      * @return heightResetFlag(bool)
      */
     bool isResetted();
+    
+    /**
+     * @brief 高さを取得
+     * @return height
+     */
+    int getHeight();
 
 private :
     void resetHeight();

--- a/bot/motor_driver/motor_driver.cpp	Thu Aug 31 10:34:55 2017 +0900
+++ b/bot/motor_driver/motor_driver.cpp	Mon Sep 04 01:01:13 2017 +0000
@@ -12,6 +12,7 @@
     MDC::MotorSTOP();
     solenoid = 0;
 }
+
 void MotorDriver::moveSlider(float speed)
 {
     if(!arm.isResetted() && speed < 0) speed = 0;

--- a/main.cpp	Thu Aug 31 10:34:55 2017 +0900
+++ b/main.cpp	Mon Sep 04 01:01:13 2017 +0000
@@ -3,13 +3,17 @@
 #include "bot.h"
 
 Bot KANI;
+Serial pc(USBTX, USBRX, 115200);
 
 int main()
 {
+    int i = 0;
+    pc.printf("const\n\r");
     while(1) {
         KANI.confirmAll();
         KANI.controllDrive();
         KANI.controllMech();
         wait(0.05);
+        pc.printf("%d\n\r", i++);
     }
 }