File content as of revision 1:a525356bd830:

#include "mbed.h"
#include "TRex.h"

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--;
    }
}