File content as of revision 7:15e6fc689368:

#include "Phaserunner.h"

Phaserunner::Phaserunner(RawSerial& connection): connection(connection), led(LED2){
    //this->connection = connection;
    //this->connection.attach(callback(this, &Phaserunner::Rx_interrupt), Serial::RxIrq);

    //this->thread.start(callback(this, &Phaserunner::writeToPhaserunner));
    //this->ticker.attach(callback(this, &Phaserunner::writeToPhaserunner), 0.5f);
}

int Phaserunner::WriteRegister(uint8_t *buf, unsigned short addr, unsigned short value){
    // lokale variablen deklarieren
    uint16_t crc;

    buf[0] = 1;                     // Slave ID
    buf[1] = 0x10;                  // Function Code

    buf[2] = (addr >> 8) & 0xFF;    // Start Address High Byte
    buf[3] = addr & 0xFF;           // Start Address Low Byte

    buf[4] = 0;                     // Number of Registers High Byte
    buf[5] = 1;                     // Number of Registers Low Byte

    buf[6] = 2;                     // Number of Data Bytes

    buf[7] = (value >> 8) & 0xFF;   // Register Value High Byte
    buf[8] = value & 0xFF;          // Register Value Low Byte

    crc = getCRC(buf, 9);           // prüfziffer berechnen

    buf[9] = crc & 0xFF;            // CRC Low Byte
    buf[10] = (crc >> 8) & 0xFF;    // CRC High Byte
    return 11;
}
int Phaserunner::readRegister(uint8_t *buf, uint16_t registerAddress){
    // lokale variablen deklarieren
    uint16_t crc;

    buf[0] = 1;                             // Slave ID
    buf[1] = 0x03;                          // Function Code

    buf[2] = (registerAddress >> 8) & 0xFF; // Start Address High Byte
    buf[3] = registerAddress & 0xFF;        // Start Address Low Byte

    buf[4] = 0;                             // Number of Registers High Byte
    buf[5] = 4;                             // Number of Registers Low Byte

    crc = getCRC(buf, 6);                   // prüfziffer berechnen

    buf[6] = crc & 0xFF;                    // CRC Low Byte
    buf[7] = (crc >> 8) & 0xFF;             // CRC High Byte

    return 8;
}
int Phaserunner::sendBuffer(unsigned short adress, unsigned short value){
    int length;
    length = Phaserunner::WriteRegister(this->writeBuffer, adress, value);

    return this->sendBuffer(length);
}
int Phaserunner::sendBuffer(int length){
    int i;
    for( i=0; i<length; i++ ){
        this->connection.putc(this->writeBuffer[i]);
    }
    return length;
}
int Phaserunner::readBuffer(uint16_t adress){
    int length;

    length = Phaserunner::readRegister(this->writeBuffer, adress);
    length = this->sendBuffer(length);

    return 0;
}
uint16_t Phaserunner::getCRC(uint8_t *msgByte, uint8_t length){
    uint16_t crcRegister = 0xFFFF;
    uint8_t i;
    uint8_t j;

    for (i = 0; i < length; i++) {
        crcRegister ^= msgByte[i];
        for (j = 0; j < 8; j++) {
            if (crcRegister & 1)
                crcRegister = (crcRegister >> 1) ^ 0xA001;
            else
                crcRegister >>= 1;
        } // end for (j)
    } // end for (i)

    return crcRegister;
}
void Phaserunner::writeToPhaserunner(){
    //while( true ){
    //RPM
    this->readRPM();
    wait_ms(WRITE_PERIOD);

    //Torque
    this->writeTorque(this->newTorque);
    wait_ms(WRITE_PERIOD);

    //Recuperation
    this->writeRecuperation(this->newRecuperation);
    wait_ms(WRITE_PERIOD);
    //}
}
void Phaserunner::setTorque(uint8_t torque){
    if( torque > 100 ){
        torque = 100;
    }
    if( torque > newTorque && torque - newTorque > MAX_TORQUE_GAIN ){
        this->newTorque += MAX_TORQUE_GAIN;
    } else {
        this->newTorque = torque;
    }
    //this->writeTorque(torque);
}
void Phaserunner::setRecuperation(uint8_t recuperation){
    this->newRecuperation = recuperation > 100 ? 100 : recuperation;
}
void Phaserunner::writeTorque(uint8_t torque){
    unsigned short value = (torque * 4096) / 100;
    this->sendBuffer(REMOTE_THROTTLE_VOLTAGE, value);
}
void Phaserunner::writeRecuperation(uint8_t recuperation){
    unsigned short value = (recuperation * 4096) / 100;
    this->sendBuffer(REMOTE_ANALOG_BREAK_VOLTAGE, value);
}
void Phaserunner::readRPM(){
    //TODO: Check how registers are read...
    this->readBuffer(MOTOR_CURRENT);
}

void Phaserunner::Rx_interrupt(){
    enum states {waiting, readAnswer, writeAnswer, error};
    uint8_t recByte;
    static uint8_t messageLength = 0;
    static states state = waiting;
    static uint8_t read_buffer_index = 0;

    switch( state ){
        case waiting:
            recByte = this->connection.getc();
            read_buffer_index = 0;

            switch( recByte ){
                case 0x04:
                    //Read Answer
                    this->read_buffer[read_buffer_index] = recByte;
                    state = readAnswer;
                    break;

                case 0x03:
                    //Write Answer
                    this->read_buffer[read_buffer_index] = recByte;
                    state = writeAnswer;
                    break;

                case 0x83: case 0x84:
                    //Error Read
                    this->read_buffer[read_buffer_index] = recByte;
                    state = error;
                    break;

                default: break;
            }
            break;

        case writeAnswer:
            recByte = this->connection.getc();
            this->read_buffer[read_buffer_index] = recByte;
            if( read_buffer_index == 1 ){
                messageLength = recByte;
            }
            else if( read_buffer_index == messageLength + 3 ){
                this->current =     ((this->read_buffer[2] << 8) | this->read_buffer[3]) / 32.0f;
                this->frequency =   ((this->read_buffer[4] << 8) | this->read_buffer[5]);
                this->voltage =     ((this->read_buffer[8] << 8) | this->read_buffer[9]) / 32.0f;
                state = waiting;
                messageLength = 0;
            }
            break;

        case readAnswer:
            recByte = this->connection.getc();
            this->read_buffer[read_buffer_index] = recByte;
            if( read_buffer_index == 1 ){
                messageLength = recByte * 2;
            }
            else if( read_buffer_index == messageLength + 1 ){
                state = waiting;
                messageLength = 0;
            }
            break;
    }
    read_buffer_index++;

//    this->buf[this->bufPointer] = this->connection.getc();
//    this->bufPointer++;
//
//    if( this->bufPointer >= 13 ){
//        this->frequency =   ((this->buf[3] << 8) | this->buf[4]);
//        this->voltage =     ((this->buf[7] << 8) | this->buf[8]) / 60.0f;
//        this->current =     ((this->buf[9] << 8) | this->buf[10]) / 32.0f;
//
//        this->bufPointer = 0;
//    }
}

float Phaserunner::getFrequency(){
    return this->frequency;
}
float Phaserunner::getCurrent(){
    return this->current;
}
float Phaserunner::getVoltage(){
    return this->voltage;
}

int Phaserunner::getRegister(int address){

 readRegister(this->read_buffer,address);
    return this->read_buffer[2]+this->read_buffer[3]<<8;
}

uint16_t Phaserunner::getRecup(){
    return this->newRecuperation;
}