Diff: TRex.cpp

Revision:

1:a525356bd830

Parent:

0:17670db9a782

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