Nico De Witte / Mbed 2 deprecated i2c_mbed_trex_emulator

TRex motor controller emulator for Vives students

Dependencies:   mbed

TRex.cpp@1:a525356bd830, 2014-11-06 (annotated)

Committer:
dwini
Date:
Thu Nov 06 10:58:59 2014 +0000
Revision:
1:a525356bd830
Parent:
0:17670db9a782 
Add basic status readout of the slave. Also add automatic battery voltage decreasing in time.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dwini 0:17670db9a782 1 #include "mbed.h"
dwini 0:17670db9a782 2 #include "TRex.h"
dwini 0:17670db9a782 3
dwini 0:17670db9a782 4 TRex::TRex()
dwini 0:17670db9a782 5 {
dwini 1:a525356bd830 6 this->startbyte = 0x0F;
dwini 1:a525356bd830 7 this->errorflags = CLEAR;
dwini 1:a525356bd830 8 this->batteryvoltage = 753;
dwini 1:a525356bd830 9 this->leftmotorcurrent = 0;
dwini 1:a525356bd830 10 this->rightmotorcurrent = 0;
dwini 1:a525356bd830 11 this->leftencodercount = 0;
dwini 1:a525356bd830 12 this->rightencodercount = 0;
dwini 1:a525356bd830 13 this->accelero_x = 748;
dwini 1:a525356bd830 14 this->accelero_y = 748;
dwini 1:a525356bd830 15 this->accelero_z = 748;
dwini 1:a525356bd830 16 this->impact_x = 0;
dwini 1:a525356bd830 17 this->impact_y = 0;
dwini 1:a525356bd830 18 this->impact_z = 0;
dwini 1:a525356bd830 19 }
dwini 1:a525356bd830 20
dwini 1:a525356bd830 21 void TRex::toBigEndianByteArray(char * bytes, unsigned short value) {
dwini 1:a525356bd830 22 *bytes = (char)(value >> 8);
dwini 1:a525356bd830 23 *(bytes+1) = (char)(value);
dwini 1:a525356bd830 24 }
dwini 1:a525356bd830 25
dwini 1:a525356bd830 26 void TRex::getStatusDataPacket(char * buffer) {
dwini 1:a525356bd830 27 buffer[STATUS_START] = this->startbyte;
dwini 1:a525356bd830 28 buffer[STATUS_ERROR] = this->errorflags;
dwini 1:a525356bd830 29
dwini 1:a525356bd830 30 this->toBigEndianByteArray(&buffer[STATUS_BATTERY_MSB], this->batteryvoltage);
dwini 1:a525356bd830 31 this->toBigEndianByteArray(&buffer[STATUS_LEFT_MOTOR_CURRENT_MSB], this->leftmotorcurrent);
dwini 1:a525356bd830 32 this->toBigEndianByteArray(&buffer[STATUS_LEFT_ENCODER_COUNT_MSB], this->leftencodercount);
dwini 1:a525356bd830 33 this->toBigEndianByteArray(&buffer[STATUS_RIGHT_MOTOR_CURRENT_MSB], this->rightmotorcurrent);
dwini 1:a525356bd830 34 this->toBigEndianByteArray(&buffer[STATUS_RIGHT_ENCODER_COUNT_MSB], this->rightencodercount);
dwini 1:a525356bd830 35
dwini 1:a525356bd830 36 this->toBigEndianByteArray(&buffer[STATUS_ACCELEROMETER_X_MSB], this->accelero_x);
dwini 1:a525356bd830 37 this->toBigEndianByteArray(&buffer[STATUS_ACCELEROMETER_Y_MSB], this->accelero_y);
dwini 1:a525356bd830 38 this->toBigEndianByteArray(&buffer[STATUS_ACCELEROMETER_Z_MSB], this->accelero_z);
dwini 1:a525356bd830 39
dwini 1:a525356bd830 40 this->toBigEndianByteArray(&buffer[STATUS_IMPACT_X_MSB], this->impact_x);
dwini 1:a525356bd830 41 this->toBigEndianByteArray(&buffer[STATUS_IMPACT_Y_MSB], this->impact_y);
dwini 1:a525356bd830 42 this->toBigEndianByteArray(&buffer[STATUS_IMPACT_Z_MSB], this->impact_z);
dwini 1:a525356bd830 43 }
dwini 1:a525356bd830 44
dwini 1:a525356bd830 45 void TRex::determineBatteryVoltage(void) {
dwini 1:a525356bd830 46 static int i = 0;
dwini 1:a525356bd830 47
dwini 1:a525356bd830 48 // Constant battery decrease of poweron
dwini 1:a525356bd830 49 i = (i + 1) % 5000000;
dwini 1:a525356bd830 50 if (!i) {
dwini 1:a525356bd830 51 this->batteryvoltage--;
dwini 1:a525356bd830 52 }
dwini 0:17670db9a782 53 }