Diff: Peripherien/Phaserunner.cpp

Revision:

7:15e6fc689368

Child:

9:56aed8c6779f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Peripherien/Phaserunner.cpp	Thu May 16 20:42:39 2019 +0000
@@ -0,0 +1,229 @@
+#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;
+}
\ No newline at end of file