SimonK_I2C_ESC.cpp

00001 #include "SimonK_I2C_ESC.h"
00002 
00003 namespace {
00004     char _buffer[9];
00005 }
00006 
00007 SimonK_I2C_ESC::SimonK_I2C_ESC(I2C &i2c, char address, char poleCount) : _i2c(i2c){
00008     _address = address << 1;
00009     _poleCount = poleCount;
00010     // A timer is needed to calculate the RPM.
00011     mbed_rpm_timer.start();
00012     _rpmTimer = mbed_rpm_timer.read_ms();
00013 }
00014 
00015 // Read the incoming data buffer from an ESC
00016 void SimonK_I2C_ESC::readBuffer(char buffer[]) {
00017     char readStartAddress = 0x02;
00018     _i2c.write(_address,&readStartAddress,1,false);
00019     _i2c.read(_address,buffer,9);
00020 }
00021 
00022 // Send motor speed command to ESC
00023 void SimonK_I2C_ESC::set(short throttle) {  
00024     char    throttleData[3];
00025     throttleData[0]   = 0x00; // Throttle Start Address
00026     throttleData[1]   = throttle>>8;
00027     throttleData[2]   = throttle;
00028     _i2c.write(_address, throttleData, 3, false);
00029 }
00030 
00031 // Send motor speed command to ESC
00032 void SimonK_I2C_ESC::setPWM(short pwm) {  
00033     set((pwm - 1100) * (32767 - -32767) / (1900 - 1100) + -32767);
00034 }
00035 
00036 void SimonK_I2C_ESC::update() {  
00037     _buffer[8] = 0x00; // Reset last byte so we can check for alive
00038     readBuffer(_buffer);
00039     _rpm = (_buffer[0] << 8) | _buffer[1];
00040     _voltage_raw = (_buffer[2] << 8) | _buffer[3];
00041     _temp_raw = (_buffer[4] << 8) | _buffer[5];
00042     _current_raw = (_buffer[6] << 8) | _buffer[7];
00043     _identifier = _buffer[8];
00044     _rpm = float(_rpm) / (abs( mbed_rpm_timer.read_ms() -_rpmTimer)/1000.0f)*60/ float(_poleCount);
00045     _rpmTimer = mbed_rpm_timer.read_ms();
00046 }
00047 
00048 bool SimonK_I2C_ESC::isAlive() {
00049     return (_identifier == 0xab);
00050 }
00051 
00052 float SimonK_I2C_ESC::voltage() {
00053     return float(_voltage_raw)/65536.0f*5.0f*6.45f;
00054 }
00055 
00056 float SimonK_I2C_ESC::current() {
00057     return (float(_current_raw)-32767)/65535.0f*5.0f*14.706f;
00058 }
00059 
00060 float SimonK_I2C_ESC::temperature() {
00061   // This code was taken from an Adafruit
00062     float resistance = SERIESRESISTOR/(65535/float(_temp_raw)-1);
00063 
00064     float steinhart;
00065     steinhart = resistance / THERMISTORNOMINAL;  // (R/Ro)
00066     steinhart = log(steinhart);                  // ln(R/Ro)
00067     steinhart /= BCOEFFICIENT;                   // 1/B * ln(R/Ro)
00068     steinhart += float(1.0) / (TEMPERATURENOMINAL + 273.15); // + (1/To)
00069     steinhart = float (1.0) / steinhart;                 // Invert
00070     steinhart -= float(273.15);                         // convert to C
00071 
00072     return steinhart;
00073 }
00074 
00075 short SimonK_I2C_ESC::rpm() {
00076   return _rpm;
00077 }