Revision 7:1c1b782263cf, committed 2018-10-22

Comitter:

394

Date:

Mon Oct 22 01:28:54 2018 +0000

Parent:

5:f630dbbae44e

Commit message:

10/22; 10:29

Changed in this revision

--- a/HMC5883L.cpp	Sat Oct 20 03:10:04 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,106 +0,0 @@
-/*This program is uncompleted.*/
-#include "HMC5883L.h"
-#include <new>
-
-HMC5883L::HMC5883L(PinName p28, PinName p27) : i2c_(*reinterpret_cast<I2C*>(i2cRaw))
-{
-    new(i2cRaw) I2C(p28, p27);
-    init();
-}
-
-HMC5883L::~HMC5883L()
-{
-    if(&i2c_ == reinterpret_cast<I2C*>(&i2cRaw))
-        reinterpret_cast<I2C*>(&i2cRaw)->~I2C();
-}
-void HMC5883L::init()
-{
-    // init - configure your setup here
-    setConfigurationA(AVG8_SAMPLES | OUTPUT_RATE_15); // 8 sample average, 15Hz, normal mode
-    setConfigurationB(0x20); // default 
-    setMode(CONTINUOUS_MODE); // continuous sample mode
-}
-
-void HMC5883L::setConfigurationA(char config)
-{
-    char cmd[2];
-    cmd[0] = CONFIG_A_REG; // register a address
-    cmd[1] = config;
-    i2c_.write(I2C_ADDRESS, cmd, 2);
-}
-
-void HMC5883L::setConfigurationB(char config)
-{
-    char cmd[2];
-    cmd[0] = CONFIG_B_REG; // register b address
-    cmd[1] = config;
-    i2c_.write(I2C_ADDRESS, cmd, 2);
-}
-
-char HMC5883L::getConfigurationA()
-{
-    char cmd[2];
-    cmd[0] = CONFIG_A_REG; // register a address
-    i2c_.write(I2C_ADDRESS, cmd, 1, true);
-    i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
-    return cmd[1];
-}
-
-char HMC5883L::getConfigurationB()
-{
-    char cmd[2];
-    cmd[0] = CONFIG_B_REG; // register b address
-    i2c_.write(I2C_ADDRESS, cmd, 1, true);
-    i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
-    return cmd[1];
-}
-
-void HMC5883L::setMode(char mode = SINGLE_MODE)
-{
-    char cmd[2];
-    cmd[0] = MODE_REG; // mode register address
-    cmd[1] = mode;
-    i2c_.write(I2C_ADDRESS,cmd,2);
-}
-
-char HMC5883L::getMode()
-{
-    char cmd[2];
-    cmd[0] = MODE_REG; // mode register
-    i2c_.write(I2C_ADDRESS, cmd, 1, true);
-    i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
-    return cmd[1];
-}
-
-char HMC5883L::getStatus()
-{
-    char cmd[2];
-    cmd[0] = STATUS_REG; // status register
-    i2c_.write(I2C_ADDRESS, cmd, 1, true);
-    i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
-    return cmd[1];
-}
-
-void HMC5883L::getXYZ(int16_t output[3])//データの取得
-{
-    char cmd[2];
-    char data[6];
-    cmd[0] = 0x03; // starting point for reading
-    i2c_.write(I2C_ADDRESS, cmd, 1, true); // set the pointer to the start of x
-    i2c_.read(I2C_ADDRESS, data, 6, false);
-    for(int i = 0; i < 3; i++) // fill the output variables
-        output[i] = int16_t(((unsigned char)data[i*2] << 8) | (unsigned char)data[i*2+1]);
-}
-
-double HMC5883L::getHeadingXY()
-{
-    int16_t raw_data[3];
-    getXYZ(raw_data);
-    double heading = atan2(static_cast<double>(raw_data[2]), static_cast<double>(raw_data[0])); // heading = arctan(Y/X) (Y/X)の逆正接
-    // 角度範囲の補正
-    if(heading < 0.0) // fix sign
-        heading += PI2;
-    if(heading > PI2) // fix overflow
-        heading -= PI2;
-    return heading;
-}
\ No newline at end of file

--- a/HMC5883L.h	Sat Oct 20 03:10:04 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,75 +0,0 @@
-#ifndef HMC5883L_H
-#define HMC5883L_H
-
-#include "mbed.h"
-
-#define CONFIG_A_REG    0x00
-#define CONFIG_B_REG    0x01
-#define MODE_REG        0x02
-#define OUTPUT_REG      0x03
-#define STATUS_REG      0x09
-// configuration register a
-#define AVG1_SAMPLES    0x00
-#define AVG2_SAMPLES    0x20
-#define AVG4_SAMPLES    0x80
-#define AVG8_SAMPLES    0xC0
-
-#define OUTPUT_RATE_0_75    0x00
-#define OUTPUT_RATE_1_5     0x04
-#define OUTPUT_RATE_3       0x08
-#define OUTPUT_RATE_7_5     0x0C
-#define OUTPUT_RATE_15      0x10
-#define OUTPUT_RATE_30      0x14
-#define OUTPUT_RATE_75      0x18
-
-#define NORMAL_MEASUREMENT  0x00
-#define POSITIVE_BIAS       0x01
-#define NEGATIVE_BIAS       0x02
-// mode register
-#define CONTINUOUS_MODE     0x00
-#define SINGLE_MODE         0x01
-#define IDLE_MODE           0x02
-// status register
-#define STATUS_LOCK         0x02
-#define STATUS_READY        0x01
-// Utility
-#ifndef M_PI
-#define M_PI 3.1415926535897932384626433832795
-#endif
-
-#define PI2         (2*M_PI)
-#define RAD_TO_DEG  (180.0/M_PI)
-#define DEG_TO_RAD  (M_PI/180.0)
-
-class HMC5883L
-{
-
-public:
-    static const int I2C_ADDRESS = 0x3D;//地磁気センサのアドレス
-    HMC5883L(PinName p28, PinName p27);
-    HMC5883L(I2C &i2c) : i2c_(i2c) {
-        init();
-    }
-    ~HMC5883L();
-    void init();
-    void setConfigurationA(char);
-    char getConfigurationA();
-    void setConfigurationB(char);
-    char getConfigurationB();
-    void setMode(char);
-    char getMode();
-    void getXYZ(int16_t raw[3]);
-    char getStatus();
-    double getHeadingXY();
-    double getHeadingXYDeg() {
-        return (getHeadingXY() * RAD_TO_DEG);//radianからdegreeへ変換
-    }
-
-private:
-    I2C &i2c_;
-    /**
-     * The raw buffer for allocating I2C object in its own without heap memory.
-     */
-    char i2cRaw[sizeof(I2C)];
-};
-#endif // HMC5883L
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/compass.cpp	Mon Oct 22 01:28:54 2018 +0000
@@ -0,0 +1,106 @@
+/*This program is uncompleted.*/
+#include "HMC5883L.h"
+#include <new>
+
+HMC5883L::HMC5883L(PinName p28, PinName p27) : i2c_(*reinterpret_cast<I2C*>(i2cRaw))
+{
+    new(i2cRaw) I2C(p28, p27);
+    init();
+}
+
+HMC5883L::~HMC5883L()
+{
+    if(&i2c_ == reinterpret_cast<I2C*>(&i2cRaw))
+        reinterpret_cast<I2C*>(&i2cRaw)->~I2C();
+}
+void HMC5883L::init()
+{
+    // init - configure your setup here
+    setConfigurationA(AVG8_SAMPLES | OUTPUT_RATE_15); // 8 sample average, 15Hz, normal mode
+    setConfigurationB(0x20); // default 
+    setMode(CONTINUOUS_MODE); // continuous sample mode
+}
+
+void HMC5883L::setConfigurationA(char config)
+{
+    char cmd[2];
+    cmd[0] = CONFIG_A_REG; // register a address
+    cmd[1] = config;
+    i2c_.write(I2C_ADDRESS, cmd, 2);
+}
+
+void HMC5883L::setConfigurationB(char config)
+{
+    char cmd[2];
+    cmd[0] = CONFIG_B_REG; // register b address
+    cmd[1] = config;
+    i2c_.write(I2C_ADDRESS, cmd, 2);
+}
+
+char HMC5883L::getConfigurationA()
+{
+    char cmd[2];
+    cmd[0] = CONFIG_A_REG; // register a address
+    i2c_.write(I2C_ADDRESS, cmd, 1, true);
+    i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
+    return cmd[1];
+}
+
+char HMC5883L::getConfigurationB()
+{
+    char cmd[2];
+    cmd[0] = CONFIG_B_REG; // register b address
+    i2c_.write(I2C_ADDRESS, cmd, 1, true);
+    i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
+    return cmd[1];
+}
+
+void HMC5883L::setMode(char mode = SINGLE_MODE)
+{
+    char cmd[2];
+    cmd[0] = MODE_REG; // mode register address
+    cmd[1] = mode;
+    i2c_.write(I2C_ADDRESS,cmd,2);
+}
+
+char HMC5883L::getMode()
+{
+    char cmd[2];
+    cmd[0] = MODE_REG; // mode register
+    i2c_.write(I2C_ADDRESS, cmd, 1, true);
+    i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
+    return cmd[1];
+}
+
+char HMC5883L::getStatus()
+{
+    char cmd[2];
+    cmd[0] = STATUS_REG; // status register
+    i2c_.write(I2C_ADDRESS, cmd, 1, true);
+    i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
+    return cmd[1];
+}
+
+void HMC5883L::getXYZ(int16_t output[3])//データの取得
+{
+    char cmd[2];
+    char data[6];
+    cmd[0] = 0x03; // starting point for reading
+    i2c_.write(I2C_ADDRESS, cmd, 1, true); // set the pointer to the start of x
+    i2c_.read(I2C_ADDRESS, data, 6, false);
+    for(int i = 0; i < 3; i++) // fill the output variables
+        output[i] = int16_t(((unsigned char)data[i*2] << 8) | (unsigned char)data[i*2+1]);
+}
+
+double HMC5883L::getHeadingXY()
+{
+    int16_t raw_data[3];
+    getXYZ(raw_data);
+    double heading = atan2(static_cast<double>(raw_data[2]), static_cast<double>(raw_data[0])); // heading = arctan(Y/X) (Y/X)の逆正接
+    // 角度範囲の補正
+    if(heading < 0.0) // fix sign
+        heading += PI2;
+    if(heading > PI2) // fix overflow
+        heading -= PI2;
+    return heading;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/compass.h	Mon Oct 22 01:28:54 2018 +0000
@@ -0,0 +1,75 @@
+#ifndef HMC5883L_H
+#define HMC5883L_H
+
+#include "mbed.h"
+
+#define CONFIG_A_REG    0x00
+#define CONFIG_B_REG    0x01
+#define MODE_REG        0x02
+#define OUTPUT_REG      0x03
+#define STATUS_REG      0x09
+// configuration register a
+#define AVG1_SAMPLES    0x00
+#define AVG2_SAMPLES    0x20
+#define AVG4_SAMPLES    0x80
+#define AVG8_SAMPLES    0xC0
+
+#define OUTPUT_RATE_0_75    0x00
+#define OUTPUT_RATE_1_5     0x04
+#define OUTPUT_RATE_3       0x08
+#define OUTPUT_RATE_7_5     0x0C
+#define OUTPUT_RATE_15      0x10
+#define OUTPUT_RATE_30      0x14
+#define OUTPUT_RATE_75      0x18
+
+#define NORMAL_MEASUREMENT  0x00
+#define POSITIVE_BIAS       0x01
+#define NEGATIVE_BIAS       0x02
+// mode register
+#define CONTINUOUS_MODE     0x00
+#define SINGLE_MODE         0x01
+#define IDLE_MODE           0x02
+// status register
+#define STATUS_LOCK         0x02
+#define STATUS_READY        0x01
+// Utility
+#ifndef M_PI
+#define M_PI 3.1415926535897932384626433832795
+#endif
+
+#define PI2         (2*M_PI)
+#define RAD_TO_DEG  (180.0/M_PI)
+#define DEG_TO_RAD  (M_PI/180.0)
+
+class HMC5883L
+{
+
+public:
+    static const int I2C_ADDRESS = 0x3D;//地磁気センサのアドレス
+    HMC5883L(PinName p28, PinName p27);
+    HMC5883L(I2C &i2c) : i2c_(i2c) {
+        init();
+    }
+    ~HMC5883L();
+    void init();
+    void setConfigurationA(char);
+    char getConfigurationA();
+    void setConfigurationB(char);
+    char getConfigurationB();
+    void setMode(char);
+    char getMode();
+    void getXYZ(int16_t raw[3]);
+    char getStatus();
+    double getHeadingXY();
+    double getHeadingXYDeg() {
+        return (getHeadingXY() * RAD_TO_DEG);//radianからdegreeへ変換
+    }
+
+private:
+    I2C &i2c_;
+    /**
+     * The raw buffer for allocating I2C object in its own without heap memory.
+     */
+    char i2cRaw[sizeof(I2C)];
+};
+#endif // HMC5883L
\ No newline at end of file

--- a/main.cpp	Sat Oct 20 03:10:04 2018 +0000
+++ b/main.cpp	Mon Oct 22 01:28:54 2018 +0000
@@ -63,7 +63,42 @@
     }    
     fprintf(fp,"GPS finish\r\n");
     fclose(fp);                      //GPSの測定終了  
-  
+    
+float mc1,mc2; //地磁気センサのキャリブレーション
+    mc1=3.0;
+    mc2=3.0;
+    
+    motor1.speed(mc1); //車体を時計回りに3秒回転
+    motor2.speed(-mc2);
+    wait(2);
+    
+    motor1.stop(0);
+    motor2.stop(0);
+    wait(1);
+    
+    motor1.speed(-mc1); //車体を反時計回りに３秒回転
+    motor2.speed(mc2);
+    wait(2);
+    
+    motor1.stop(0);
+    motor2.stop(0);
+    wait(1);
+    printf("compass carriblation\r\n"); //キャリブレーション終了
+    
+    if(M_PI<raw<357.5){
+        motor1.speed(mc1);
+        motor2.speed(-mc2);
+        wait(1);
+        }else if(357.5<=raw<PI2 && raw<=2.5){
+            motor1.speed(-mc1);
+            motor2.speed(mc2);
+            wait(1);
+            }else
+            motor1.stop(0);
+            motor2.stop(0);
+            wait(2);
+            printf("search north\r\n"); //機体が北を向く
+    
         mu.startUpdates();//start mesuring the distance(超音波センサー)
         int distance; 
         

--- a/motordriver.cpp	Sat Oct 20 03:10:04 2018 +0000
+++ b/motordriver.cpp	Mon Oct 22 01:28:54 2018 +0000
@@ -1,33 +1,4 @@
-/*motor driver libary modified from the following libary,
-*
-* mbed simple H-bridge motor controller
-* Copyright (c) 2007-2010, sford
-*
-* by Christopher Hasler.
-*
-* from sford's libary,
-*
-* Permission is hereby granted, free of charge, to any person obtaining a copy
-* of this software and associated documentation files (the "Software"), to deal
-* in the Software without restriction, including without limitation the rights
-* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-* copies of the Software, and to permit persons to whom the Software is
-* furnished to do so, subject to the following conditions:
-*
-* The above copyright notice and this permission notice shall be included in
-* all copies or substantial portions of the Software.
-*
-* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-* THE SOFTWARE.
-*/
-
 #include "motordriver.h"
-
 #include "mbed.h"
 
 Motor::Motor(PinName pwm, PinName fwd, PinName rev, int brakeable):

--- a/motordriver.h	Sat Oct 20 03:10:04 2018 +0000
+++ b/motordriver.h	Mon Oct 22 01:28:54 2018 +0000
@@ -1,32 +1,4 @@
-/*motor driver libary modified from the following libary,
-*  
-* mbed simple H-bridge motor controller
-* Copyright (c) 2007-2010, sford
-* 
-* by Christopher Hasler.
-* 
-* from sford's libary,
-*
-* Permission is hereby granted, free of charge, to any person obtaining a copy
-* of this software and associated documentation files (the "Software"), to deal
-* in the Software without restriction, including without limitation the rights
-* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-* copies of the Software, and to permit persons to whom the Software is
-* furnished to do so, subject to the following conditions:
-*
-* The above copyright notice and this permission notice shall be included in
-* all copies or substantial portions of the Software.
-*
-* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-* THE SOFTWARE.
-*/
- 
-#ifndef MBED_MOTOR_H
+ #ifndef MBED_MOTOR_H
 #define MBED_MOTOR_H
  
 #include "mbed.h"
@@ -84,8 +56,4 @@
  
 };
 
-
-
-
-
-#endif
+#endif
\ No newline at end of file