Revision 0:c15430f1895f, committed 2017-02-11

Comitter:

mk1

Date:

Sat Feb 11 15:55:34 2017 +0000

Commit message:

succes

Changed in this revision

diff -r 000000000000 -r c15430f1895f BNO055.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BNO055.cpp	Sat Feb 11 15:55:34 2017 +0000
@@ -0,0 +1,134 @@
+#include "BNO055.h"
+#include <vector>
+
+namespace BNO055
+{
+/*
+*  Sensor constructor. This will take care of all necessary initialization and sensor calibration
+*/    
+Sensor::Sensor(MicroBit & bit) : 
+    _bit(bit),
+    _I2CAddress(0x52),  // 0x28 : 0x29
+    _currentPage(0xFF)       
+ {
+    LogDeviceId();
+    Init();
+ }
+ 
+ /*
+ *  This is just a sanity check. Verfies chip/accelerometer/magnetometer and gyro id.
+ */
+void Sensor::LogDeviceId()
+{
+      SelectPage(0); 
+ 
+    char error[200];
+    char cmd[7] = {CHIP_ID_REGISTER, 0, 0, 0, 0, 0, 0};
+    _bit.i2c.write(_I2CAddress, cmd, 1, true);
+    _bit.i2c.read(_I2CAddress, cmd, 7, false);
+    
+    if (cmd[0] == BNO055_CHIP_ID) {
+        _bit.serial.send("BNO055 CHIP ID - OK\n");
+    } else {
+        sprintf(error, "BNO055 CHIP ID - ERROR. Got: %d\n", cmd[0]);
+        _bit.serial.send(error);
+    }
+    if (cmd[1] == BNO055_ACC_ID) {
+        _bit.serial.send("BNO055 ACCELEROMETER ID - OK\n");
+    } else {
+        sprintf(error, "BNO055 ACCELEROMETER ID - ERROR. Got: %d\n", cmd[1]);
+        _bit.serial.send(error);
+    }
+    if (cmd[2] == BNO055_MAG_ID) {
+        _bit.serial.send("BNO055 MAGNETOMETER ID - OK\n");
+    } else {
+        sprintf(error, "BNO055 ACCELERMAGNETOMETEROMETER ID - ERROR. Got: %d\n", cmd[2]);
+        _bit.serial.send(error);
+    }
+    if (cmd[3] == BNO055_GYRO_ID) {
+      _bit.serial.send("BNO055 GYRO ID - OK\n");
+    } else {
+        sprintf(error, "BNO055 GYRO ID - ERROR. Got: %d\n", cmd[3]);
+        _bit.serial.send(error);
+    }
+}
+
+uint8_t Sensor::CalibrationStatus(void)
+{
+    SelectPage(0);
+    char cmd[1] = {CALIBRATION_STATUS};
+    _bit.i2c.write(_I2CAddress, cmd, 1, true);
+    _bit.i2c.read(_I2CAddress, cmd, 1, false);
+    return cmd[0];
+}
+
+void Sensor::Calibrate()
+{
+    _bit.display.scroll("CALIBRATE");
+    while  (true) {
+        uint8_t status = (CalibrationStatus() & 0b11000000) >> 6;
+        _bit.display.scroll(status);
+        if (3 == status)
+            break;
+        wait(0.1);
+    }
+    
+    _bit.display.scroll("OK");
+    
+    for (int i=5; i>= 0; i--) {
+        _bit.display.printChar(i+48);
+        wait(0.5);
+    }
+  
+    
+}
+
+
+void Sensor::SelectNDOFFusion()
+{
+   SelectPage(0); 
+    char cmd[2] = { OPR_MODE_REGISTER, NDOF_FUSION_MODE };
+    _bit.i2c.write(_I2CAddress, cmd, 2); 
+}
+   
+void Sensor::SelectUnits()
+{
+     SelectPage(0);
+     char cmd[2] = {UNIT_SELECTION_REGISTER, UNIT_ORI_WIN + UNIT_ACC_MSS + UNIT_GYR_DPS + UNIT_EULER_DEG + UNIT_TEMP_C};
+    _bit.i2c.write(_I2CAddress, cmd, 2, false);
+}
+
+void Sensor::Init()
+{
+    SelectUnits();
+    SelectNDOFFusion();
+}
+
+void Sensor::SelectPage(uint8_t page)
+{
+    if (page != _currentPage)
+    {
+        char cmd[2] = {PAGE_ID_REGISTER, page};
+        _bit.i2c.write(_I2CAddress, cmd, 2, false);
+        _currentPage = page; // Happy path ;)
+    }
+}
+
+void Sensor::ReadEulerAngles(double & heading, double & roll, double & pitch)
+{
+    char cmd[6] = {EULER_H_REGISTER_LSB, 0, 0, 0, 0, 0};
+    int16_t _heading;
+    int16_t _roll;
+    int16_t _pitch;
+    SelectPage(0);
+    _bit.i2c.write(_I2CAddress, cmd, 1, true);
+    _bit.i2c.read(_I2CAddress, cmd, 6, false);
+    _heading = cmd[1] << 8 | cmd[0];
+    _pitch = cmd[3] << 8 | cmd[2];
+    _roll = cmd[5] << 8 | cmd[4];
+    heading = (double)_heading / 16;
+    roll = (double)_roll / 16;
+    pitch = (double)_pitch / 16;
+}
+
+}

diff -r 000000000000 -r c15430f1895f BNO055.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BNO055.h	Sat Feb 11 15:55:34 2017 +0000
@@ -0,0 +1,64 @@
+#ifndef _BNO055_h_
+#define _BNO055_h_
+
+#include "MicroBit.h"
+
+// Registers
+#define CHIP_ID_REGISTER          0x00
+#define PAGE_ID_REGISTER          0x07
+#define OPR_MODE_REGISTER         0x3d
+#define UNIT_SELECTION_REGISTER   0x3b
+#define EULER_H_REGISTER_LSB      0x1a
+#define EULER_H_REGISTER_MSB      0x1b
+#define CALIBRATION_STATUS        0x35
+
+
+// IDs
+#define BNO055_CHIP_ID        0xa0    
+#define BNO055_ACC_ID         0xfb    
+#define BNO055_MAG_ID         0x32    
+#define BNO055_GYRO_ID         0x0f    
+
+// Units
+#define UNIT_ACC_MSS            0x00 
+#define UNIT_GYR_DPS            0x00 
+#define UNIT_EULER_DEG          0x00 
+#define UNIT_TEMP_C             0x00 
+#define UNIT_ORI_WIN            0x00 
+
+
+#define NDOF_FUSION_MODE 0x0C
+#define IMU_FUSION_MODE 0x08
+
+
+namespace BNO055
+{
+
+
+class Sensor
+{
+    public:
+        Sensor(MicroBit & bit);
+        void Calibrate();
+
+        void ReadEulerAngles(double & heading, double & roll, double & pitch);
+ 
+    private:
+        MicroBit & _bit;
+        char _buffer[12];
+        uint8_t _I2CAddress;
+        
+        void LogDeviceId();
+        void SelectPage(uint8_t page);
+        void SelectNDOFFusion();
+        void SelectUnits();
+        void Init();
+        void CheckID();
+        uint8_t CalibrationStatus(void);
+        
+        uint8_t _currentPage;
+};
+
+}
+    
+#endif
\ No newline at end of file

diff -r 000000000000 -r c15430f1895f PID.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.cpp	Sat Feb 11 15:55:34 2017 +0000
@@ -0,0 +1,38 @@
+// Nicked this code from https://nicisdigital.wordpress.com/2011/06/27/proportional-integral-derivative-pid-controller/
+
+#include "PID.h"
+
+void pid_zeroize(PID* pid) {
+    // set prev and integrated error to zero
+    pid->prev_error = 0;
+    pid->int_error = 0;
+}
+ 
+void pid_update(PID* pid, double curr_error, double dt) 
+{
+    double diff;
+    double p_term;
+    double i_term;
+    double d_term;
+ 
+    // integration with windup guarding
+    pid->int_error += (curr_error * dt);
+    if (pid->int_error < -(pid->windup_guard))
+        pid->int_error = -(pid->windup_guard);
+    else if (pid->int_error > pid->windup_guard)
+        pid->int_error = pid->windup_guard;
+ 
+    // differentiation
+    diff = ((curr_error - pid->prev_error) / dt);
+ 
+    // scaling
+    p_term = (pid->proportional_gain * curr_error);
+    i_term = (pid->integral_gain     * pid->int_error);
+    d_term = (pid->derivative_gain   * diff);
+ 
+    // summation of terms
+    pid->control = p_term + i_term + d_term;
+ 
+    // save current error as previous error for next iteration
+    pid->prev_error = curr_error;
+}
\ No newline at end of file

diff -r 000000000000 -r c15430f1895f PID.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.h	Sat Feb 11 15:55:34 2017 +0000
@@ -0,0 +1,19 @@
+#ifndef _PID_H
+#define _PID_H
+
+// Shamelessly ripped from the interwebs
+
+typedef struct {
+    double windup_guard;
+    double proportional_gain;
+    double integral_gain;
+    double derivative_gain;
+    double prev_error;
+    double int_error;
+    double control;
+} PID;
+ 
+void pid_zeroize(PID* pid);
+void pid_update(PID* pid, double curr_error, double dt);
+    
+#endif
\ No newline at end of file

diff -r 000000000000 -r c15430f1895f kalman.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kalman.cpp	Sat Feb 11 15:55:34 2017 +0000
@@ -0,0 +1,39 @@
+#include "kalman.h"
+
+// Kalman filter module
+ // Shamelessly ripped from http://forum.arduino.cc/index.php/topic,8652.0.html
+
+ float Q_angle  =  0.001;
+ float Q_gyro   =  0.003;
+ float R_angle  =  0.03; 
+
+ float x_angle = 0;
+ float x_bias = 0;
+ float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;      
+ float y, S;
+ float K_0, K_1;
+
+
+ float kalmanCalculate(float newAngle, float newRate, float  dt) 
+ {
+  // dt = float(looptime)/1000;                                  
+   x_angle += dt * (newRate - x_bias);
+   P_00 +=  - dt * (P_10 + P_01) + Q_angle * dt;
+   P_01 +=  - dt * P_11;
+   P_10 +=  - dt * P_11;
+   P_11 +=  + Q_gyro * dt;
+   
+   y = newAngle - x_angle;
+   S = P_00 + R_angle;
+   K_0 = P_00 / S;
+   K_1 = P_10 / S;
+   
+   x_angle +=  K_0 * y;
+   x_bias  +=  K_1 * y;
+   P_00 -= K_0 * P_00;
+   P_01 -= K_0 * P_01;
+   P_10 -= K_1 * P_00;
+   P_11 -= K_1 * P_01;
+   
+   return x_angle;
+ }

diff -r 000000000000 -r c15430f1895f kalman.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kalman.h	Sat Feb 11 15:55:34 2017 +0000
@@ -0,0 +1,6 @@
+#ifndef _KALMAN_H_
+#define _KALMAN_H_
+
+float kalmanCalculate(float newAngle, float newRate, float  dt);
+ 
+#endif
\ No newline at end of file

diff -r 000000000000 -r c15430f1895f main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Feb 11 15:55:34 2017 +0000
@@ -0,0 +1,72 @@
+#include "MicroBit.h"
+#include "sample.h"
+#include "motor.h"
+#include "PID.h"
+#include "kalman.h"
+#include "mbed.h"
+#include "BNO055.h"
+
+MicroBit uBit;
+
+Ticker stepInterrupt;
+
+int main()
+{
+    
+    BNO055::Sensor sensor(uBit);
+    
+    sensor.Calibrate();
+ 
+   // Initialize PID controller
+    PID pid;
+    pid_zeroize(&pid);
+    pid.windup_guard = 0; 
+    pid.proportional_gain = 95;
+    pid.integral_gain = 0.75;
+    pid.derivative_gain = 0.5;
+  
+   // Initialize motor and set up step interrupt
+    Motor motor(uBit);
+    stepInterrupt.attach(&motor, &Motor::Step, STEP_PERIOD);  
+
+    char logMessage[200];
+    float error = 0;
+    float dt;
+    float loopTime = 0.003; 
+    double heading = 0;
+    double roll = 0;
+    double pitch = 0;
+    while(true)
+    {
+       Timer t;
+        t.start();
+      
+        sensor.ReadEulerAngles(heading, roll, pitch);
+        
+        error = 89-roll;
+        
+        pid_update(&pid, error, dt); 
+  
+        if (error < 0)
+            motor.SetDirection(FORWARD);
+        else
+            motor.SetDirection(REVERSE);
+     
+  
+        motor.SetSpeed(pid.control);      
+       
+        t.stop();
+        dt = t.read();
+        if (dt < loopTime)
+            wait(loopTime-dt);
+   
+        //sprintf(logMessage, "h: %f, r: %f, p: %f\n", heading, roll, pitch);
+        //uBit.serial.send(logMessage);
+ 
+    }
+        
+    release_fiber();
+}
+
+
+

diff -r 000000000000 -r c15430f1895f microbit.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/microbit.lib	Sat Feb 11 15:55:34 2017 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/teams/Lancaster-University/code/microbit/#4b89e7e3494f

diff -r 000000000000 -r c15430f1895f motor.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motor.cpp	Sat Feb 11 15:55:34 2017 +0000
@@ -0,0 +1,56 @@
+#include "mbed.h"
+#include "motor.h"
+
+
+Motor::Motor(MicroBit & bit)
+    : _bit(bit),
+      _speed(0),
+      _targetSpeed(0)
+{
+    // 50% duty cycle will make a nice step pulse
+    SetPWMDutyCycle(900);
+}
+
+void Motor::SetSpeed(float targetSpeed)
+{
+    _targetSpeed = targetSpeed;
+}
+
+float Motor::Speed()
+{
+    return _speed;
+}
+
+void Motor::Step()
+{
+    if ((_speed - _targetSpeed) > ACCELERATION)
+        _speed -= ACCELERATION;
+    else if ((_speed - _targetSpeed) < -ACCELERATION)
+        _speed += ACCELERATION;
+    else
+        _speed = _targetSpeed;
+ 
+    if ((_speed < 0.00005) && (_speed > -0.00005))
+        _speed = 0.00005; // Prevent int overflow in SetPWMPeriod. 
+        
+    SetPWMPeriod(abs(100000/_speed));
+}
+
+void Motor::SetPWMDutyCycle(int value) const
+{
+    _bit.io.P3.setAnalogValue(value);
+    _bit.io.P4.setAnalogValue(value);
+}
+ 
+void Motor::SetPWMPeriod(int usValue) const
+{
+    _bit.io.P3.setAnalogPeriodUs(usValue); 
+    _bit.io.P4.setAnalogPeriodUs(usValue);
+}
+
+void Motor::SetDirection(STEP_DIRECTION direction) const
+{
+    _bit.io.P8.setDigitalValue(direction);
+    _bit.io.P16.setDigitalValue(direction);
+}
+  

diff -r 000000000000 -r c15430f1895f motor.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motor.h	Sat Feb 11 15:55:34 2017 +0000
@@ -0,0 +1,37 @@
+#ifndef _MOTOR_H_
+#define _MOTOR_H_
+
+#include "MicroBit.h"
+
+// If you change step frequency, you will implicitly also change acceleration
+// If you change acceleration, you will - wait for it - change acceleration
+const float STEP_FREQUENCY = 4000; // kHz
+const float STEP_PERIOD = 1.0/STEP_FREQUENCY; 
+const float ACCELERATION = 8;
+
+typedef enum 
+{
+    FORWARD = 1,
+    REVERSE = 0
+} STEP_DIRECTION;
+
+class Motor
+{
+public:
+    Motor(MicroBit & bit);
+    void Step();
+    void SetSpeed(float targetSpeed);
+    float Speed();
+    void SetDirection(STEP_DIRECTION direction) const;
+ 
+private:
+    MicroBit & _bit;
+    void SetPWMDutyCycle(int value) const;
+    void SetPWMPeriod(int usValue) const;
+    
+    float _speed;
+    float _targetSpeed;
+};
+
+
+#endif
\ No newline at end of file

diff -r 000000000000 -r c15430f1895f sample.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sample.cpp	Sat Feb 11 15:55:34 2017 +0000
@@ -0,0 +1,18 @@
+#include "sample.h"
+#include "MicroBit.h"
+
+Sample::Sample(float gx, float ay, float az) 
+            : _gx(gx)
+            , _ay(ay)
+            , _az(az) 
+            {}
+   
+double Sample::GyroRate()
+{
+    return -_gx;
+}
+double Sample::AccelerometerAngle()
+{
+    return (atan2(_ay, _az)+pi)*360/pi; // Noise-O-rama
+}
+

diff -r 000000000000 -r c15430f1895f sample.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sample.h	Sat Feb 11 15:55:34 2017 +0000
@@ -0,0 +1,18 @@
+#ifndef _SAMPLE_H
+#define _SAMPLE_H
+
+const float pi = 3.1415926;
+
+struct Sample
+{
+    public:
+        Sample(float gx = 0, float ay = 0, float az = 0);
+        double GyroRate();
+        double AccelerometerAngle();
+        
+    float _gx;
+    float _ay;
+    float _az;
+};
+
+#endif
\ No newline at end of file