Revision 1:a525356bd830, committed 2014-11-06

Comitter:

dwini

Date:

Thu Nov 06 10:58:59 2014 +0000

Parent:

0:17670db9a782

Commit message:

Add basic status readout of the slave. Also add automatic battery voltage decreasing in time.

Changed in this revision

diff -r 17670db9a782 -r a525356bd830 TRex.cpp
--- a/TRex.cpp	Thu Nov 06 08:15:51 2014 +0000
+++ b/TRex.cpp	Thu Nov 06 10:58:59 2014 +0000
@@ -3,4 +3,51 @@
 
 TRex::TRex()
 {
+    this->startbyte = 0x0F;
+    this->errorflags = CLEAR;
+    this->batteryvoltage = 753;
+    this->leftmotorcurrent = 0;
+    this->rightmotorcurrent = 0;
+    this->leftencodercount = 0;
+    this->rightencodercount = 0;
+    this->accelero_x = 748;
+    this->accelero_y = 748;
+    this->accelero_z = 748;
+    this->impact_x = 0;
+    this->impact_y = 0;
+    this->impact_z = 0;
+}
+
+void TRex::toBigEndianByteArray(char * bytes, unsigned short value) {
+    *bytes = (char)(value >> 8);
+    *(bytes+1) = (char)(value);
+}
+
+void TRex::getStatusDataPacket(char * buffer) {
+    buffer[STATUS_START] = this->startbyte;
+    buffer[STATUS_ERROR] = this->errorflags;
+
+    this->toBigEndianByteArray(&buffer[STATUS_BATTERY_MSB], this->batteryvoltage);
+    this->toBigEndianByteArray(&buffer[STATUS_LEFT_MOTOR_CURRENT_MSB], this->leftmotorcurrent);
+    this->toBigEndianByteArray(&buffer[STATUS_LEFT_ENCODER_COUNT_MSB], this->leftencodercount);
+    this->toBigEndianByteArray(&buffer[STATUS_RIGHT_MOTOR_CURRENT_MSB], this->rightmotorcurrent);
+    this->toBigEndianByteArray(&buffer[STATUS_RIGHT_ENCODER_COUNT_MSB], this->rightencodercount);
+
+    this->toBigEndianByteArray(&buffer[STATUS_ACCELEROMETER_X_MSB], this->accelero_x);
+    this->toBigEndianByteArray(&buffer[STATUS_ACCELEROMETER_Y_MSB], this->accelero_y);
+    this->toBigEndianByteArray(&buffer[STATUS_ACCELEROMETER_Z_MSB], this->accelero_z);
+    
+    this->toBigEndianByteArray(&buffer[STATUS_IMPACT_X_MSB], this->impact_x);
+    this->toBigEndianByteArray(&buffer[STATUS_IMPACT_Y_MSB], this->impact_y);
+    this->toBigEndianByteArray(&buffer[STATUS_IMPACT_Z_MSB], this->impact_z);
+}
+
+void TRex::determineBatteryVoltage(void) {
+    static int i = 0;
+    
+    // Constant battery decrease of poweron
+    i = (i + 1) % 5000000;
+    if (!i) {
+        this->batteryvoltage--;
+    }
 }
\ No newline at end of file

diff -r 17670db9a782 -r a525356bd830 TRex.h
--- a/TRex.h	Thu Nov 06 08:15:51 2014 +0000
+++ b/TRex.h	Thu Nov 06 10:58:59 2014 +0000
@@ -1,11 +1,71 @@
 #ifndef TREX_HEADER
 #define TREX_HEADER
 
+enum StatusDataPacketFields {
+    STATUS_START = 0,
+    STATUS_ERROR = 1,
+    STATUS_BATTERY_MSB = 2,
+    STATUS_BATTERY_LSB = 3,
+    STATUS_LEFT_MOTOR_CURRENT_MSB = 4,
+    STATUS_LEFT_MOTOR_CURRENT_LSB = 5,
+    STATUS_LEFT_ENCODER_COUNT_MSB = 6,
+    STATUS_LEFT_ENCODER_COUNT_LSB = 7,
+    STATUS_RIGHT_MOTOR_CURRENT_MSB = 8,
+    STATUS_RIGHT_MOTOR_CURRENT_LSB = 9,
+    STATUS_RIGHT_ENCODER_COUNT_MSB = 10,
+    STATUS_RIGHT_ENCODER_COUNT_LSB = 11,
+    STATUS_ACCELEROMETER_X_MSB = 12,
+    STATUS_ACCELEROMETER_X_LSB = 13,
+    STATUS_ACCELEROMETER_Y_MSB = 14,
+    STATUS_ACCELEROMETER_Y_LSB = 15,
+    STATUS_ACCELEROMETER_Z_MSB = 16,
+    STATUS_ACCELEROMETER_Z_LSB = 17,
+    STATUS_IMPACT_X_MSB = 18,
+    STATUS_IMPACT_X_LSB = 19,
+    STATUS_IMPACT_Y_MSB = 20,
+    STATUS_IMPACT_Y_LSB = 21,
+    STATUS_IMPACT_Z_MSB = 22,
+    STATUS_IMPACT_Z_LSB = 23
+};
+
+enum ErrorFlag {
+    CLEAR = 0,
+    START_BYTE = 1,
+    PWM_FREQ = 2,
+    SPEED = 4,
+    SERVO = 8,
+    IMPACT = 16,
+    BATTERY = 32,
+    I2C_ADDRESS = 64,
+    I2C_SPEED = 128
+};
+
 class TRex {
-    
+    public:
+        unsigned char startbyte;
+        unsigned char errorflags;
+        unsigned short batteryvoltage;
+        unsigned short leftmotorcurrent;
+        unsigned short rightmotorcurrent;
+        unsigned short leftencodercount;
+        unsigned short rightencodercount;
+        unsigned short accelero_x;
+        unsigned short accelero_y;
+        unsigned short accelero_z;
+        unsigned short impact_x;
+        unsigned short impact_y;
+        unsigned short impact_z;
+
+        /* Number of bytes in the TREX byte array representation */
+        const static int SIZE_TREX_DATA_PACKET = 24;
     public:
         /* Constructor */
         TRex();
+        void getStatusDataPacket(char * buffer);
+        void determineBatteryVoltage(void);
+        
+    private:
+        void toBigEndianByteArray(char * bytes, unsigned short value);
 };
 
 

diff -r 17670db9a782 -r a525356bd830 main.cpp
--- a/main.cpp	Thu Nov 06 08:15:51 2014 +0000
+++ b/main.cpp	Thu Nov 06 10:58:59 2014 +0000
@@ -1,11 +1,11 @@
 #include "mbed.h"
-#include "memory.h"
+#include "TRex.h"
 
 DigitalOut aLED(LED1);      // Alive LED
 Serial pc(USBTX, USBRX);    // TX, RX
 I2CSlave slave(p28, p27);   // SDA, SCL
 
-const int SLAVE_ADDRESS = 0x07;
+const int SLAVE_ADDRESS = 0x0E;
 const int I2C_FREQUENCY = 100000;
 const int I2C_BUFFER_SIZE = 30;
 
@@ -15,6 +15,9 @@
     // Alive LED
     int cAlive = 0;
     
+    // Battery decrease counter
+    int cBattery = 0;
+    
     // Configure I2C
     slave.frequency(I2C_FREQUENCY);
     pc.printf("Slave is working @ %dHz\r\n", I2C_FREQUENCY);
@@ -24,11 +27,20 @@
     // I2C buffer
     char buffer[I2C_BUFFER_SIZE];
     
+    // Create TRex
+    TRex trex;
+    
     // Go infinite
     while (1) {
         int rec = slave.receive();
         switch (rec) {
             case I2CSlave::ReadAddressed:
+                trex.getStatusDataPacket(buffer);
+                if (!slave.write(buffer, TRex::SIZE_TREX_DATA_PACKET)) {
+                    pc.printf("Send status to master success\r\n");
+                } else {
+                    pc.printf("Send status to master failed\r\n");
+                }
                 break;
 
             case I2CSlave::WriteAddressed:
@@ -45,5 +57,8 @@
         if (!cAlive) {
             aLED = !aLED;
         }
+        
+        // Battery decrease of TRex
+        trex.determineBatteryVoltage();
     }
 }