Nico De Witte
/
i2c_mbed_trex_emulator
TRex motor controller emulator for Vives students
Diff: TRex.cpp
- Revision:
- 1:a525356bd830
- Parent:
- 0:17670db9a782
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