Revision 6:67263dc2c2cf, committed 2018-04-26

Comitter:

matajarb

Date:

Thu Apr 26 12:22:58 2018 +0000

Parent:

5:59079b76ac7f

Commit message:

s?mme du schluch

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU.cpp	Thu Apr 26 12:22:58 2018 +0000
@@ -0,0 +1,218 @@
+/*
+ * IMU.cpp
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#include <cmath>
+#include "IMU.h"
+
+using namespace std;
+
+const float IMU::PI = 3.14159265f;                  // the constant PI
+
+/**
+ * Creates an IMU object.
+ * @param spi a reference to an spi controller to use.
+ * @param csG the chip select output for the gyro sensor.
+ * @param csXM the chip select output for the accelerometer and the magnetometer.
+ */
+IMU::IMU(SPI& spi, DigitalOut& csG, DigitalOut& csXM) : spi(spi), csG(csG), csXM(csXM) {
+    
+    // initialize SPI interface
+    
+    spi.format(8, 3);
+    spi.frequency(1000000);
+    
+    // reset chip select lines to logical high
+    
+    csG = 1;
+    csXM = 1;
+    
+    // initialize gyro
+    
+    writeRegister(csG, CTRL_REG1_G, 0x0F);  // enable gyro in all 3 axis
+    
+    // initialize accelerometer
+    
+    writeRegister(csXM, CTRL_REG0_XM, 0x00);
+    writeRegister(csXM, CTRL_REG1_XM, 0x5F);
+    writeRegister(csXM, CTRL_REG2_XM, 0x00);
+    writeRegister(csXM, CTRL_REG3_XM, 0x04);
+    
+    // initialize magnetometer
+    
+    writeRegister(csXM, CTRL_REG5_XM, 0x94);
+    writeRegister(csXM, CTRL_REG6_XM, 0x00);
+    writeRegister(csXM, CTRL_REG7_XM, 0x00);
+    writeRegister(csXM, CTRL_REG4_XM, 0x04);
+    writeRegister(csXM, INT_CTRL_REG_M, 0x09);
+}
+
+/**
+ * Deletes the IMU object.
+ */
+IMU::~IMU() {}
+
+/**
+ * This private method allows to write a register value.
+ * @param cs the chip select output to use, either csG or csXM.
+ * @param address the 7 bit address of the register.
+ * @param value the value to write into the register.
+ */
+void IMU::writeRegister(DigitalOut& cs, char address, char value) {
+    
+    cs = 0;
+    
+    spi.write(0x7F & address);
+    spi.write(value & 0xFF);
+    
+    cs = 1;
+}
+
+/**
+ * This private method allows to read a register value.
+ * @param cs the chip select output to use, either csG or csXM.
+ * @param address the 7 bit address of the register.
+ * @return the value read from the register.
+ */
+char IMU::readRegister(DigitalOut& cs, char address) {
+    
+    cs = 0;
+    
+    spi.write(0x80 | address);
+    int value = spi.write(0xFF);
+    
+    cs = 1;
+    
+    return (char)(value & 0xFF);
+}
+
+/**
+ * Reads the gyroscope about the x-axis.
+ * @return the rotational speed about the x-axis given in [rad/s].
+ */
+float IMU::readGyroX() {
+    
+    char low = readRegister(csG, OUT_X_L_G);
+    char high = readRegister(csG, OUT_X_H_G);
+    
+    short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
+    
+    return (float)value/32768.0f*245.0f*PI/180.0f;
+}
+
+/**
+ * Reads the gyroscope about the y-axis.
+ * @return the rotational speed about the y-axis given in [rad/s].
+ */
+float IMU::readGyroY() {
+
+    char low = readRegister(csG, OUT_Y_L_G);
+    char high = readRegister(csG, OUT_Y_H_G);
+    
+    short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
+    
+    return (float)value/32768.0f*245.0f*PI/180.0f;
+}
+
+/**
+ * Reads the gyroscope about the z-axis.
+ * @return the rotational speed about the z-axis given in [rad/s].
+ */
+float IMU::readGyroZ() {
+    
+    char low = readRegister(csG, OUT_Z_L_G);
+    char high = readRegister(csG, OUT_Z_H_G);
+    
+    short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
+    
+    return (float)value/32768.0f*245.0f*PI/180.0f;
+}
+
+/**
+ * Reads the acceleration in x-direction.
+ * @return the acceleration in x-direction, given in [m/s2].
+ */
+float IMU::readAccelerationX() {
+    
+    char low = readRegister(csXM, OUT_X_L_A);
+    char high = readRegister(csXM, OUT_X_H_A);
+    
+    short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
+    
+    return (float)value/32768.0f*2.0f*9.81f;
+}
+
+/**
+ * Reads the acceleration in y-direction.
+ * @return the acceleration in y-direction, given in [m/s2].
+ */
+float IMU::readAccelerationY() {
+    
+    char low = readRegister(csXM, OUT_Y_L_A);
+    char high = readRegister(csXM, OUT_Y_H_A);
+    
+    short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
+    
+    return (float)value/32768.0f*2.0f*9.81f;
+}
+
+/**
+ * Reads the acceleration in z-direction.
+ * @return the acceleration in z-direction, given in [m/s2].
+ */
+float IMU::readAccelerationZ() {
+    
+    char low = readRegister(csXM, OUT_Z_L_A);
+    char high = readRegister(csXM, OUT_Z_H_A);
+    
+    short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
+    
+    return (float)value/32768.0f*2.0f*9.81f;
+}
+
+/**
+ * Reads the magnetic field in x-direction.
+ * @return the magnetic field in x-direction, given in [Gauss].
+ */
+float IMU::readMagnetometerX() {
+    
+    // bitte implementieren!
+    
+    return 0.0f;
+}
+
+/**
+ * Reads the magnetic field in x-direction.
+ * @return the magnetic field in x-direction, given in [Gauss].
+ */
+float IMU::readMagnetometerY() {
+    
+    // bitte implementieren!
+    
+    return 0.0f;
+}
+
+/**
+ * Reads the magnetic field in x-direction.
+ * @return the magnetic field in x-direction, given in [Gauss].
+ */
+float IMU::readMagnetometerZ() {
+    
+    // bitte implementieren!
+    
+    return 0.0f;
+}
+
+/**
+ * Reads the compass heading about the z-axis.
+ * @return the compass heading in the range -PI to +PI, given in [rad].
+ */
+float IMU::readHeading() {
+    
+    // bitte implementieren!
+    
+    return 0.0f;
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU.h	Thu Apr 26 12:22:58 2018 +0000
@@ -0,0 +1,74 @@
+/*
+ * IMU.h
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef IMU_H_
+#define IMU_H_
+
+#include <cstdlib>
+#include <mbed.h>
+
+/**
+ * This is a device driver class for the ST LSM9DS0 inertial measurement unit.
+ */
+class IMU {
+
+    public:
+        
+                    IMU(SPI& spi, DigitalOut& csG, DigitalOut& csXM);
+        virtual     ~IMU();
+        float       readGyroX();
+        float       readGyroY();
+        float       readGyroZ();
+        float       readAccelerationX();
+        float       readAccelerationY();
+        float       readAccelerationZ();
+        float       readMagnetometerX();
+        float       readMagnetometerY();
+        float       readMagnetometerZ();
+        float       readHeading();
+        
+    private:
+        
+        static const char   WHO_AM_I_G = 0x0F;
+        static const char   CTRL_REG1_G = 0x20;
+        static const char   OUT_X_L_G = 0x28;
+        static const char   OUT_X_H_G = 0x29;
+        static const char   OUT_Y_L_G = 0x2A;
+        static const char   OUT_Y_H_G = 0x2B;
+        static const char   OUT_Z_L_G = 0x2C;
+        static const char   OUT_Z_H_G = 0x2D;
+        
+        static const char   WHO_AM_I_XM = 0x0F;
+        
+        static const char   INT_CTRL_REG_M = 0x12;
+        static const char   CTRL_REG0_XM = 0x1F;
+        static const char   CTRL_REG1_XM = 0x20;
+        static const char   CTRL_REG2_XM = 0x21;
+        static const char   CTRL_REG3_XM = 0x22;
+        static const char   CTRL_REG4_XM = 0x23;
+        static const char   CTRL_REG5_XM = 0x24;
+        static const char   CTRL_REG6_XM = 0x25;
+        static const char   CTRL_REG7_XM = 0x26;
+        
+        static const char   OUT_X_L_A = 0x28;
+        static const char   OUT_X_H_A = 0x29;
+        static const char   OUT_Y_L_A = 0x2A;
+        static const char   OUT_Y_H_A = 0x2B;
+        static const char   OUT_Z_L_A = 0x2C;
+        static const char   OUT_Z_H_A = 0x2D;
+        
+        static const float  PI;
+    
+        SPI&            spi;
+        DigitalOut&     csG;
+        DigitalOut&     csXM;
+    
+        void            writeRegister(DigitalOut& cs, char address, char value);
+        char            readRegister(DigitalOut& cs, char address);
+};
+
+#endif /* IMU_H_ */
+

--- a/Main.cpp	Thu Apr 12 14:56:27 2018 +0000
+++ b/Main.cpp	Thu Apr 26 12:22:58 2018 +0000
@@ -8,8 +8,10 @@
 #include <mbed.h>
 #include "EncoderCounter.h"
 #include "IRSensor.h"
+#include "IMU.h"
 #include "Controller.h"
 #include "StateMachine.h"
+#include "SerialServer.h"
 
 int main() {
     

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SerialServer.cpp	Thu Apr 26 12:22:58 2018 +0000
@@ -0,0 +1,128 @@
+/*
+ * SerialServer.cpp
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#include <vector>
+#include "SerialServer.h"
+
+using namespace std;
+
+inline string float2string(float f) {
+
+    char buffer[32];
+    sprintf(buffer, "%.3f", f);
+    
+    return string(buffer);
+}
+
+inline string int2string(int i) {
+    
+    char buffer[32];
+    sprintf(buffer, "%d", i);
+    
+    return string(buffer);
+}
+
+const float SerialServer::PERIOD = 0.001f; // period of transmit task, given in [s]
+
+/**
+ * Creates a serial server object.
+ */
+SerialServer::SerialServer(Serial& serial, IMU& imu, Controller& controller) : serial(serial), imu(imu), controller(controller) {
+    
+    input.clear();
+    output.clear();
+    
+    serial.attach(callback(this, &SerialServer::receive), Serial::RxIrq);
+    //serial.attach(callback(this, &SerialServer::transmit), Serial::TxIrq);
+    ticker.attach(callback(this, &SerialServer::transmit), PERIOD);
+}
+
+/**
+ * Deletes the serial server object.
+ */
+SerialServer::~SerialServer() {}
+
+/**
+ * Callback method of serial interface.
+ */
+void SerialServer::receive() {
+    
+    // read received characters while input buffer is full
+    
+    while (serial.readable()) {
+        int c = serial.getc();
+        if (input.size() < BUFFER_SIZE) input += (char)c;
+    }
+    
+    // check if input is complete (terminated with CR & LF)
+    
+    if (input.find("\r\n") != string::npos) {
+
+        // parse request
+        
+        string request = input.substr(0, input.find("\r\n"));
+        string name;
+        vector<string> values;
+        
+        if (request.find(' ') != string::npos) {
+
+            name = request.substr(0, request.find(' '));
+            request = request.substr(request.find(' ')+1);
+
+            while (request.find(' ') != string::npos) {
+                values.push_back(request.substr(0, request.find(' ')));
+                request = request.substr(request.find(' ')+1);
+            }
+            values.push_back(request);
+
+        } else {
+            
+            name = request;
+        }
+        
+        input.clear();
+        
+        // process request
+        
+        if (name.compare("getRobotPose") == 0) {
+            float x = controller.getX();
+            float y = controller.getY();
+            float alpha = controller.getAlpha();
+            output = "pose "+float2string(x)+" "+float2string(y)+" "+float2string(alpha)+"\r\n";
+        } else if (name.compare("getOrientation") == 0) {
+            float heading = imu.readHeading();
+            float alpha = controller.getAlpha();
+            output = "orientation "+float2string(heading)+" "+float2string(alpha)+"\r\n";
+        } else {
+            output = "request unknown\r\n";
+        }
+        
+        // transmit first byte of output buffer
+        
+        if (serial.writeable() && (output.size() > 0)) {
+            serial.putc(output[0]);
+            output.erase(0, 1);
+        }
+        
+    } else if (input.size() >= BUFFER_SIZE) {
+        
+        input.clear();
+    }
+}
+
+/**
+ * Callback method of serial interface.
+ */
+void SerialServer::transmit() {
+    
+    // transmit output
+    
+    while (serial.writeable() && (output.size() > 0)) {
+        serial.putc(output[0]);
+        output.erase(0, 1);
+    }
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SerialServer.h	Thu Apr 26 12:22:58 2018 +0000
@@ -0,0 +1,45 @@
+/*
+ * SerialServer.h
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef SERIAL_SERVER_H_
+#define SERIAL_SERVER_H_
+
+#include <cstdlib>
+#include <string>
+#include <mbed.h>
+#include "IMU.h"
+#include "Controller.h"
+
+using namespace std;
+
+/**
+ * This class implements a communication server using a serial interface.
+ */
+class SerialServer {
+    
+    public:
+        
+                        SerialServer(Serial& serial, IMU& imu, Controller& controller);
+        virtual         ~SerialServer();
+        
+    private:
+        
+        static const float  PERIOD;
+        static const int    BUFFER_SIZE = 64;
+        
+        Serial&         serial;
+        IMU&            imu;
+        Controller&     controller;
+        string          input;
+        string          output;
+        Ticker          ticker;
+        
+        void            receive();
+        void            transmit();
+};
+
+#endif /* SERIAL_SERVER_H_ */
+

--- a/StateMachine.cpp	Thu Apr 12 14:56:27 2018 +0000
+++ b/StateMachine.cpp	Thu Apr 26 12:22:58 2018 +0000
@@ -73,12 +73,7 @@
                 enableMotorDriver = 1;
                 
                 taskList.push_back(new TaskWait(controller, 0.5f));
-                taskList.push_back(new TaskMoveTo(controller, 0.0f, 1.0f, 0.0f, 1.5f, 0.01f));
-                //taskList.push_back(new TaskWait(controller, 1.0f));
-                taskList.push_back(new TaskMoveTo(controller, 0.0f, 0.0f, 0.0f, 1.5f, 0.05f));
-                taskList.push_back(new TaskMoveTo(controller, 0.0f, 1.0f, 0.0f, 1.5f, 0.05f));
-                taskList.push_back(new TaskMoveTo(controller, 0.0f, 0.0f, 0.0f, 1.5f, 0.01f));
-                taskList.push_back(new TaskMoveTo(controller, 0.0f, 0.0f, 0.0f, 1.5f, 0.09f));
+                taskList.push_back(new TaskMove(controller, 0.0f, 1.0f));
                 
                 state = PROCESSING_TASKS;
             }
@@ -194,3 +189,4 @@
             state = ROBOT_OFF;
     }
 }
+

--- a/StateMachine.h	Thu Apr 12 14:56:27 2018 +0000
+++ b/StateMachine.h	Thu Apr 26 12:22:58 2018 +0000
@@ -14,6 +14,7 @@
 #include "IRSensor.h"
 #include "Task.h"
 #include "TaskWait.h"
+#include "TaskMove.h"
 #include "TaskMoveTo.h"
 
 /**
@@ -68,3 +69,4 @@
 };
 
 #endif /* STATE_MACHINE_H_ */
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TaskMove.cpp	Thu Apr 26 12:22:58 2018 +0000
@@ -0,0 +1,74 @@
+/*
+ * TaskMove.cpp
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#include <cmath>
+#include "TaskMove.h"
+
+using namespace std;
+
+const float TaskMove::DEFAULT_DURATION = 3600.0f;
+
+/**
+ * Creates a task object that moves the robot with given velocities.
+ * @param conroller a reference to the controller object of the robot.
+ * @param translationalVelocity the translational velocity, given in [m/s].
+ * @param rotationalVelocity the rotational velocity, given in [rad/s].
+ */
+TaskMove::TaskMove(Controller& controller, float translationalVelocity, float rotationalVelocity) : controller(controller) {
+    
+    this->translationalVelocity = translationalVelocity;
+    this->rotationalVelocity = rotationalVelocity;
+    this->duration = DEFAULT_DURATION;
+    
+    time = 0.0f;
+}
+
+/**
+ * Creates a task object that moves the robot with given velocities.
+ * @param conroller a reference to the controller object of the robot.
+ * @param translationalVelocity the translational velocity, given in [m/s].
+ * @param rotationalVelocity the rotational velocity, given in [rad/s].
+ * @param duration the duration to move the robot, given in [s].
+ */
+TaskMove::TaskMove(Controller& controller, float translationalVelocity, float rotationalVelocity, float duration) : controller(controller) {
+    
+    this->translationalVelocity = translationalVelocity;
+    this->rotationalVelocity = rotationalVelocity;
+    this->duration = duration;
+    
+    time = 0.0f;
+}
+
+/**
+ * Deletes the task object.
+ */
+TaskMove::~TaskMove() {}
+
+/**
+ * This method is called periodically by a task sequencer.
+ * @param period the period of the task sequencer, given in [s].
+ * @return the status of this task, i.e. RUNNING or DONE.
+ */
+int TaskMove::run(float period) {
+    
+    time += period;
+    
+    if (time < duration) {
+        
+        controller.setTranslationalVelocity(translationalVelocity);
+        controller.setRotationalVelocity(rotationalVelocity);
+        
+        return RUNNING;
+        
+    } else {
+        
+        controller.setTranslationalVelocity(0.0f);
+        controller.setRotationalVelocity(0.0f);
+        
+        return DONE;
+    }
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TaskMove.h	Thu Apr 26 12:22:58 2018 +0000
@@ -0,0 +1,38 @@
+/*
+ * TaskMove.h
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef TASK_MOVE_H_
+#define TASK_MOVE_H_
+
+#include <cstdlib>
+#include "Controller.h"
+#include "Task.h"
+
+/**
+ * This is a specific implementation of a task class that moves the robot with given velocities.
+ */
+class TaskMove : public Task {
+    
+    public:
+        
+        static const float  DEFAULT_DURATION;   /**< Default duration, given in [s]. */
+        
+                    TaskMove(Controller& controller, float translationalVelocity, float rotationalVelocity);
+                    TaskMove(Controller& controller, float translationalVelocity, float rotationalVelocity, float duration);
+        virtual     ~TaskMove();
+        virtual int run(float period);
+        
+    private:
+        
+        Controller& controller;             // reference to controller object to use
+        float       translationalVelocity;  // translational velocity, given in [m/s]
+        float       rotationalVelocity;     // rotational velocity, given in [rad/s]
+        float       duration;               // duration to move the robot, given in [s]
+        float       time;                   // current time, given in [s]
+};
+
+#endif /* TASK_MOVE_H_ */
+

--- a/TaskMoveTo.cpp	Thu Apr 12 14:56:27 2018 +0000
+++ b/TaskMoveTo.cpp	Thu Apr 26 12:22:58 2018 +0000
@@ -111,4 +111,3 @@
     return (rho < zone) ? DONE : RUNNING;
 }
 
-